113 ec_slave_t *slave = fsm->slave; |
113 ec_slave_t *slave = fsm->slave; |
114 ec_master_t *master = slave->master; |
114 ec_master_t *master = slave->master; |
115 ec_master_sdo_request_t *request, *next; |
115 ec_master_sdo_request_t *request, *next; |
116 |
116 |
117 // search the first matching external request to be processed |
117 // search the first matching external request to be processed |
118 list_for_each_entry_safe(request, next, &master->slave_sdo_requests, list) { |
118 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
119 if (request->slave != slave) |
|
120 continue; |
|
121 list_del_init(&request->list); // dequeue |
119 list_del_init(&request->list); // dequeue |
122 request->req.state = EC_INT_REQUEST_BUSY; |
120 request->req.state = EC_INT_REQUEST_BUSY; |
123 |
121 |
124 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
122 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
125 EC_ERR("Discarding SDO request, slave %u is in INIT.\n", |
123 EC_ERR("Discarding SDO request, slave %u is in INIT.\n", |
126 slave->ring_position); |
124 slave->ring_position); |
127 request->req.state = EC_INT_REQUEST_FAILURE; |
125 request->req.state = EC_INT_REQUEST_FAILURE; |
128 wake_up(&master->sdo_queue); |
126 wake_up(&slave->sdo_queue); |
129 continue; |
127 continue; |
130 } |
128 } |
131 |
129 |
132 // Found pending SDO request. Execute it! |
130 // Found pending SDO request. Execute it! |
133 if (master->debug_level) |
131 if (master->debug_level) |
163 } |
161 } |
164 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
162 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
165 EC_DBG("Failed to process SDO request for slave %u.\n", |
163 EC_DBG("Failed to process SDO request for slave %u.\n", |
166 fsm->slave->ring_position); |
164 fsm->slave->ring_position); |
167 request->state = EC_INT_REQUEST_FAILURE; |
165 request->state = EC_INT_REQUEST_FAILURE; |
168 wake_up(&master->sdo_queue); |
166 wake_up(&slave->sdo_queue); |
169 fsm->sdo_request = NULL; |
167 fsm->sdo_request = NULL; |
170 fsm->state = ec_fsm_slave_state_idle; |
168 fsm->state = ec_fsm_slave_state_idle; |
171 return; |
169 return; |
172 } |
170 } |
173 |
171 |
174 // SDO request finished |
172 // SDO request finished |
175 request->state = EC_INT_REQUEST_SUCCESS; |
173 request->state = EC_INT_REQUEST_SUCCESS; |
176 wake_up(&master->sdo_queue); |
174 wake_up(&slave->sdo_queue); |
177 |
175 |
178 if (master->debug_level) |
176 if (master->debug_level) |
179 EC_DBG("Finished SDO request for slave %u.\n", |
177 EC_DBG("Finished SDO request for slave %u.\n", |
180 fsm->slave->ring_position); |
178 fsm->slave->ring_position); |
181 |
179 |