178 int ec_fsm_slave_action_process_sdo( |
178 int ec_fsm_slave_action_process_sdo( |
179 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
179 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
180 ) |
180 ) |
181 { |
181 { |
182 ec_slave_t *slave = fsm->slave; |
182 ec_slave_t *slave = fsm->slave; |
183 ec_sdo_request_t *request, *next; |
183 ec_sdo_request_t *request; |
184 |
184 |
185 // search the first external request to be processed |
185 if (list_empty(&slave->sdo_requests)) { |
186 list_for_each_entry_safe(request, next, &slave->sdo_requests, list) { |
186 return 0; |
187 |
187 } |
188 list_del_init(&request->list); // dequeue |
188 |
189 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
189 // take the first request to be processed |
190 EC_SLAVE_WARN(slave, "Aborting SDO request," |
190 request = list_entry(slave->sdo_requests.next, ec_sdo_request_t, list); |
191 " slave has error flag set.\n"); |
191 list_del_init(&request->list); // dequeue |
192 request->state = EC_INT_REQUEST_FAILURE; |
192 |
193 wake_up(&slave->sdo_queue); |
193 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
194 fsm->sdo_request = NULL; |
194 EC_SLAVE_WARN(slave, "Aborting SDO request," |
195 fsm->state = ec_fsm_slave_state_idle; |
195 " slave has error flag set.\n"); |
196 return 0; |
196 request->state = EC_INT_REQUEST_FAILURE; |
197 } |
197 wake_up(&slave->master->request_queue); |
198 |
198 fsm->state = ec_fsm_slave_state_idle; |
199 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
|
200 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
|
201 request->state = EC_INT_REQUEST_FAILURE; |
|
202 wake_up(&slave->sdo_queue); |
|
203 fsm->sdo_request = NULL; |
|
204 fsm->state = ec_fsm_slave_state_idle; |
|
205 return 0; |
|
206 } |
|
207 |
|
208 request->state = EC_INT_REQUEST_BUSY; |
|
209 |
|
210 // Found pending SDO request. Execute it! |
|
211 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
|
212 |
|
213 // Start SDO transfer |
|
214 fsm->sdo_request = request; |
|
215 fsm->state = ec_fsm_slave_state_sdo_request; |
|
216 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request); |
|
217 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
218 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
|
219 return 1; |
199 return 1; |
220 } |
200 } |
221 return 0; |
201 |
|
202 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
|
203 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
|
204 request->state = EC_INT_REQUEST_FAILURE; |
|
205 wake_up(&slave->master->request_queue); |
|
206 fsm->state = ec_fsm_slave_state_idle; |
|
207 return 1; |
|
208 } |
|
209 |
|
210 request->state = EC_INT_REQUEST_BUSY; |
|
211 |
|
212 // Found pending SDO request. Execute it! |
|
213 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
|
214 |
|
215 // Start SDO transfer |
|
216 fsm->sdo_request = request; |
|
217 fsm->state = ec_fsm_slave_state_sdo_request; |
|
218 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, request); |
|
219 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
220 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
|
221 return 1; |
222 } |
222 } |
223 |
223 |
224 /*****************************************************************************/ |
224 /*****************************************************************************/ |
225 |
225 |
226 /** Slave state: SDO_REQUEST. |
226 /** Slave state: SDO_REQUEST. |
238 } |
238 } |
239 |
239 |
240 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
240 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
241 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
241 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
242 request->state = EC_INT_REQUEST_FAILURE; |
242 request->state = EC_INT_REQUEST_FAILURE; |
243 wake_up(&slave->sdo_queue); |
243 wake_up(&slave->master->request_queue); |
244 fsm->sdo_request = NULL; |
244 fsm->sdo_request = NULL; |
245 fsm->state = ec_fsm_slave_state_ready; |
245 fsm->state = ec_fsm_slave_state_ready; |
246 return; |
246 return; |
247 } |
247 } |
248 |
248 |
249 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
249 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
250 |
250 |
251 // SDO request finished |
251 // SDO request finished |
252 request->state = EC_INT_REQUEST_SUCCESS; |
252 request->state = EC_INT_REQUEST_SUCCESS; |
253 wake_up(&slave->sdo_queue); |
253 wake_up(&slave->master->request_queue); |
254 |
|
255 fsm->sdo_request = NULL; |
254 fsm->sdo_request = NULL; |
256 fsm->state = ec_fsm_slave_state_ready; |
255 fsm->state = ec_fsm_slave_state_ready; |
257 } |
256 } |
258 |
257 |
259 /*****************************************************************************/ |
258 /*****************************************************************************/ |
294 |
293 |
295 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
294 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
296 EC_SLAVE_WARN(slave, "Aborting register request," |
295 EC_SLAVE_WARN(slave, "Aborting register request," |
297 " slave has error flag set.\n"); |
296 " slave has error flag set.\n"); |
298 fsm->reg_request->state = EC_INT_REQUEST_FAILURE; |
297 fsm->reg_request->state = EC_INT_REQUEST_FAILURE; |
299 wake_up(&slave->reg_queue); |
298 wake_up(&slave->master->request_queue); |
|
299 fsm->reg_request = NULL; |
300 fsm->state = ec_fsm_slave_state_idle; |
300 fsm->state = ec_fsm_slave_state_idle; |
301 return 1; |
301 return 1; |
302 } |
302 } |
303 |
303 |
304 // Found pending register request. Execute it! |
304 // Found pending register request. Execute it! |
343 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
343 if (fsm->datagram->state != EC_DATAGRAM_RECEIVED) { |
344 EC_SLAVE_ERR(slave, "Failed to receive register" |
344 EC_SLAVE_ERR(slave, "Failed to receive register" |
345 " request datagram: "); |
345 " request datagram: "); |
346 ec_datagram_print_state(fsm->datagram); |
346 ec_datagram_print_state(fsm->datagram); |
347 reg->state = EC_INT_REQUEST_FAILURE; |
347 reg->state = EC_INT_REQUEST_FAILURE; |
348 wake_up(&slave->reg_queue); |
348 wake_up(&slave->master->request_queue); |
|
349 fsm->reg_request = NULL; |
349 fsm->state = ec_fsm_slave_state_ready; |
350 fsm->state = ec_fsm_slave_state_ready; |
350 return; |
351 return; |
351 } |
352 } |
352 |
353 |
353 if (fsm->datagram->working_counter == 1) { |
354 if (fsm->datagram->working_counter == 1) { |
363 EC_SLAVE_ERR(slave, "Register request failed" |
364 EC_SLAVE_ERR(slave, "Register request failed" |
364 " (working counter is %u).\n", |
365 " (working counter is %u).\n", |
365 fsm->datagram->working_counter); |
366 fsm->datagram->working_counter); |
366 } |
367 } |
367 |
368 |
368 wake_up(&slave->reg_queue); |
369 wake_up(&slave->master->request_queue); |
|
370 fsm->reg_request = NULL; |
369 fsm->state = ec_fsm_slave_state_ready; |
371 fsm->state = ec_fsm_slave_state_ready; |
370 } |
372 } |
371 |
373 |
372 /*****************************************************************************/ |
374 /*****************************************************************************/ |
373 |
375 |
392 |
394 |
393 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
395 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
394 EC_SLAVE_WARN(slave, "Aborting FoE request," |
396 EC_SLAVE_WARN(slave, "Aborting FoE request," |
395 " slave has error flag set.\n"); |
397 " slave has error flag set.\n"); |
396 request->state = EC_INT_REQUEST_FAILURE; |
398 request->state = EC_INT_REQUEST_FAILURE; |
397 wake_up(&slave->sdo_queue); |
399 wake_up(&slave->master->request_queue); |
398 return 0; |
400 fsm->state = ec_fsm_slave_state_idle; |
|
401 return 1; |
399 } |
402 } |
400 |
403 |
401 request->state = EC_INT_REQUEST_BUSY; |
404 request->state = EC_INT_REQUEST_BUSY; |
402 |
405 |
403 EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); |
406 EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); |
427 } |
430 } |
428 |
431 |
429 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
432 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
430 EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); |
433 EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); |
431 request->state = EC_INT_REQUEST_FAILURE; |
434 request->state = EC_INT_REQUEST_FAILURE; |
432 wake_up(&slave->foe_queue); |
435 wake_up(&slave->master->request_queue); |
433 fsm->foe_request = NULL; |
436 fsm->foe_request = NULL; |
434 fsm->state = ec_fsm_slave_state_ready; |
437 fsm->state = ec_fsm_slave_state_ready; |
435 return; |
438 return; |
436 } |
439 } |
437 |
440 |
438 // finished transferring FoE |
441 // finished transferring FoE |
439 EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE" |
442 EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE" |
440 " data.\n", request->data_size); |
443 " data.\n", request->data_size); |
441 |
444 |
442 request->state = EC_INT_REQUEST_SUCCESS; |
445 request->state = EC_INT_REQUEST_SUCCESS; |
443 wake_up(&slave->foe_queue); |
446 wake_up(&slave->master->request_queue); |
444 |
|
445 fsm->foe_request = NULL; |
447 fsm->foe_request = NULL; |
446 fsm->state = ec_fsm_slave_state_ready; |
448 fsm->state = ec_fsm_slave_state_ready; |
447 } |
449 } |
448 |
450 |
449 /*****************************************************************************/ |
451 /*****************************************************************************/ |
469 |
471 |
470 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
472 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
471 EC_SLAVE_WARN(slave, "Aborting SoE request," |
473 EC_SLAVE_WARN(slave, "Aborting SoE request," |
472 " slave has error flag set.\n"); |
474 " slave has error flag set.\n"); |
473 req->state = EC_INT_REQUEST_FAILURE; |
475 req->state = EC_INT_REQUEST_FAILURE; |
474 wake_up(&slave->soe_queue); |
476 wake_up(&slave->master->request_queue); |
475 return 0; |
477 fsm->state = ec_fsm_slave_state_idle; |
|
478 return 1; |
476 } |
479 } |
477 |
480 |
478 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
481 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
479 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
482 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
480 req->state = EC_INT_REQUEST_FAILURE; |
483 req->state = EC_INT_REQUEST_FAILURE; |
481 wake_up(&slave->soe_queue); |
484 wake_up(&slave->master->request_queue); |
|
485 fsm->state = ec_fsm_slave_state_idle; |
482 return 0; |
486 return 0; |
483 } |
487 } |
484 |
488 |
485 req->state = EC_INT_REQUEST_BUSY; |
489 req->state = EC_INT_REQUEST_BUSY; |
486 |
490 |
513 } |
517 } |
514 |
518 |
515 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
519 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
516 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
520 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
517 request->state = EC_INT_REQUEST_FAILURE; |
521 request->state = EC_INT_REQUEST_FAILURE; |
518 wake_up(&slave->soe_queue); |
522 wake_up(&slave->master->request_queue); |
519 fsm->soe_request = NULL; |
523 fsm->soe_request = NULL; |
520 fsm->state = ec_fsm_slave_state_ready; |
524 fsm->state = ec_fsm_slave_state_ready; |
521 return; |
525 return; |
522 } |
526 } |
523 |
527 |
524 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
528 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
525 |
529 |
526 // SoE request finished |
530 // SoE request finished |
527 request->state = EC_INT_REQUEST_SUCCESS; |
531 request->state = EC_INT_REQUEST_SUCCESS; |
528 wake_up(&slave->soe_queue); |
532 wake_up(&slave->master->request_queue); |
529 |
|
530 fsm->soe_request = NULL; |
533 fsm->soe_request = NULL; |
531 fsm->state = ec_fsm_slave_state_ready; |
534 fsm->state = ec_fsm_slave_state_ready; |
532 } |
535 } |
533 |
536 |
534 /*****************************************************************************/ |
537 /*****************************************************************************/ |