60 ) |
60 ) |
61 { |
61 { |
62 fsm->slave = slave; |
62 fsm->slave = slave; |
63 fsm->datagram = datagram; |
63 fsm->datagram = datagram; |
64 fsm->datagram->data_size = 0; |
64 fsm->datagram->data_size = 0; |
65 fsm->state = ec_fsm_slave_state_idle; |
65 if (slave->master->debug_level) |
|
66 EC_DBG("init fsm for slave %u...\n",slave->ring_position); |
|
67 fsm->state = ec_fsm_slave_state_idle; |
66 |
68 |
67 // init sub-state-machines |
69 // init sub-state-machines |
68 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
70 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
69 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
71 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
70 } |
72 } |
175 ec_master_sdo_request_t *request, *next; |
177 ec_master_sdo_request_t *request, *next; |
176 |
178 |
177 // search the first external request to be processed |
179 // search the first external request to be processed |
178 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
180 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
179 |
181 |
|
182 list_del_init(&request->list); // dequeue |
180 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
183 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
181 EC_WARN("Postponing SDO request, slave %u has ERROR.\n", |
184 EC_WARN("Aborting SDO request, slave %u has ERROR.\n", |
182 slave->ring_position); |
185 slave->ring_position); |
|
186 request->req.state = EC_INT_REQUEST_FAILURE; |
|
187 wake_up(&slave->sdo_queue); |
|
188 fsm->sdo_request = NULL; |
183 fsm->state = ec_fsm_slave_state_idle; |
189 fsm->state = ec_fsm_slave_state_idle; |
184 return 0; |
190 return 0; |
185 } |
191 } |
186 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
192 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
187 EC_WARN("Postponing SDO request, slave %u is in INIT.\n", |
193 EC_WARN("Aborting SDO request, slave %u is in INIT.\n", |
188 slave->ring_position); |
194 slave->ring_position); |
|
195 request->req.state = EC_INT_REQUEST_FAILURE; |
|
196 wake_up(&slave->sdo_queue); |
|
197 fsm->sdo_request = NULL; |
189 fsm->state = ec_fsm_slave_state_idle; |
198 fsm->state = ec_fsm_slave_state_idle; |
190 return 0; |
199 return 0; |
191 } |
200 } |
192 list_del_init(&request->list); // dequeue |
201 request->req.state = EC_INT_REQUEST_BUSY; |
193 request->req.state = EC_INT_REQUEST_BUSY; |
|
194 |
202 |
195 // Found pending SDO request. Execute it! |
203 // Found pending SDO request. Execute it! |
196 if (master->debug_level) |
204 if (master->debug_level) |
197 EC_DBG("Processing SDO request for slave %u...\n", |
205 EC_DBG("Processing SDO request for slave %u...\n", |
198 slave->ring_position); |
206 slave->ring_position); |
224 ec_master_foe_request_t *request, *next; |
232 ec_master_foe_request_t *request, *next; |
225 |
233 |
226 // search the first request to be processed |
234 // search the first request to be processed |
227 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
235 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
228 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
236 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
229 EC_WARN("Postponing FOE request, slave %u has ERROR.\n", |
237 EC_WARN("Aborting FOE request, slave %u has ERROR.\n", |
230 slave->ring_position); |
238 slave->ring_position); |
|
239 request->req.state = EC_INT_REQUEST_FAILURE; |
|
240 wake_up(&slave->sdo_queue); |
|
241 fsm->sdo_request = NULL; |
231 fsm->state = ec_fsm_slave_state_idle; |
242 fsm->state = ec_fsm_slave_state_idle; |
232 return 0; |
243 return 0; |
233 } |
244 } |
234 list_del_init(&request->list); // dequeue |
245 list_del_init(&request->list); // dequeue |
235 request->req.state = EC_INT_REQUEST_BUSY; |
246 request->req.state = EC_INT_REQUEST_BUSY; |
269 } |
280 } |
270 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
281 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
271 EC_DBG("Failed to process SDO request for slave %u.\n", |
282 EC_DBG("Failed to process SDO request for slave %u.\n", |
272 fsm->slave->ring_position); |
283 fsm->slave->ring_position); |
273 request->state = EC_INT_REQUEST_FAILURE; |
284 request->state = EC_INT_REQUEST_FAILURE; |
274 wake_up(&slave->sdo_queue); |
285 wake_up(&slave->foe_queue); |
275 fsm->sdo_request = NULL; |
286 fsm->foe_request = NULL; |
276 fsm->state = ec_fsm_slave_state_idle; |
287 fsm->state = ec_fsm_slave_state_idle; |
277 return; |
288 return; |
278 } |
289 } |
279 |
290 |
280 if (master->debug_level) |
291 if (master->debug_level) |
281 EC_DBG("Finished SDO request for slave %u.\n", |
292 EC_DBG("Finished SDO request for slave %u.\n", |