58 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
58 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
59 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
59 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
60 void ec_fsm_master_state_reg_request(ec_fsm_master_t *); |
60 void ec_fsm_master_state_reg_request(ec_fsm_master_t *); |
61 void ec_fsm_master_state_foe_request(ec_fsm_master_t *); |
61 void ec_fsm_master_state_foe_request(ec_fsm_master_t *); |
62 |
62 |
|
63 |
63 /*****************************************************************************/ |
64 /*****************************************************************************/ |
64 |
65 |
65 /** Constructor. |
66 /** Constructor. |
66 */ |
67 */ |
67 void ec_fsm_master_init( |
68 void ec_fsm_master_init( |
113 |
114 |
114 /** Executes the current state of the state machine. |
115 /** Executes the current state of the state machine. |
115 * |
116 * |
116 * If the state machine's datagram is not sent or received yet, the execution |
117 * If the state machine's datagram is not sent or received yet, the execution |
117 * of the state machine is delayed to the next cycle. |
118 * of the state machine is delayed to the next cycle. |
118 */ |
119 * |
119 void ec_fsm_master_exec( |
120 * \return true, if the state machine was executed |
|
121 */ |
|
122 int ec_fsm_master_exec( |
120 ec_fsm_master_t *fsm /**< Master state machine. */ |
123 ec_fsm_master_t *fsm /**< Master state machine. */ |
121 ) |
124 ) |
122 { |
125 { |
123 if (fsm->datagram->state == EC_DATAGRAM_SENT |
126 if (fsm->datagram->state == EC_DATAGRAM_SENT |
124 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
127 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
125 // datagram was not sent or received yet. |
128 // datagram was not sent or received yet. |
126 return; |
129 return 0; |
127 } |
130 } |
128 |
131 |
129 fsm->state(fsm); |
132 fsm->state(fsm); |
|
133 return 1; |
130 } |
134 } |
131 |
135 |
132 /*****************************************************************************/ |
136 /*****************************************************************************/ |
133 |
137 |
134 /** |
138 /** |
409 ) |
413 ) |
410 { |
414 { |
411 ec_master_t *master = fsm->master; |
415 ec_master_t *master = fsm->master; |
412 ec_slave_t *slave; |
416 ec_slave_t *slave; |
413 ec_sdo_request_t *req; |
417 ec_sdo_request_t *req; |
414 ec_master_sdo_request_t *request; |
|
415 |
418 |
416 // search for internal requests to be processed |
419 // search for internal requests to be processed |
417 for (slave = master->slaves; |
420 for (slave = master->slaves; |
418 slave < master->slaves + master->slave_count; |
421 slave < master->slaves + master->slave_count; |
419 slave++) { |
422 slave++) { |
423 if (req->state == EC_INT_REQUEST_QUEUED) { |
426 if (req->state == EC_INT_REQUEST_QUEUED) { |
424 |
427 |
425 if (ec_sdo_request_timed_out(req)) { |
428 if (ec_sdo_request_timed_out(req)) { |
426 req->state = EC_INT_REQUEST_FAILURE; |
429 req->state = EC_INT_REQUEST_FAILURE; |
427 if (master->debug_level) |
430 if (master->debug_level) |
428 EC_DBG("SDO request for slave %u timed out...\n", |
431 EC_DBG("Internal SDO request for slave %u timed out...\n", |
429 slave->ring_position); |
432 slave->ring_position); |
430 continue; |
433 continue; |
431 } |
434 } |
432 |
435 |
433 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
436 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
435 continue; |
438 continue; |
436 } |
439 } |
437 |
440 |
438 req->state = EC_INT_REQUEST_BUSY; |
441 req->state = EC_INT_REQUEST_BUSY; |
439 if (master->debug_level) |
442 if (master->debug_level) |
440 EC_DBG("Processing SDO request for slave %u...\n", |
443 EC_DBG("Processing internal SDO request for slave %u...\n", |
441 slave->ring_position); |
444 slave->ring_position); |
442 |
445 |
443 fsm->idle = 0; |
446 fsm->idle = 0; |
444 fsm->sdo_request = req; |
447 fsm->sdo_request = req; |
445 fsm->slave = slave; |
448 fsm->slave = slave; |
448 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
451 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
449 return 1; |
452 return 1; |
450 } |
453 } |
451 } |
454 } |
452 } |
455 } |
453 |
|
454 // search the first external request to be processed |
|
455 while (1) { |
|
456 if (list_empty(&master->slave_sdo_requests)) |
|
457 break; |
|
458 |
|
459 // get first request |
|
460 request = list_entry(master->slave_sdo_requests.next, |
|
461 ec_master_sdo_request_t, list); |
|
462 list_del_init(&request->list); // dequeue |
|
463 request->req.state = EC_INT_REQUEST_BUSY; |
|
464 |
|
465 slave = request->slave; |
|
466 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
|
467 EC_ERR("Discarding SDO request, slave %u is in INIT.\n", |
|
468 slave->ring_position); |
|
469 request->req.state = EC_INT_REQUEST_FAILURE; |
|
470 wake_up(&master->sdo_queue); |
|
471 continue; |
|
472 } |
|
473 |
|
474 // Found pending SDO request. Execute it! |
|
475 if (master->debug_level) |
|
476 EC_DBG("Processing SDO request for slave %u...\n", |
|
477 slave->ring_position); |
|
478 |
|
479 // Start uploading SDO |
|
480 fsm->idle = 0; |
|
481 fsm->sdo_request = &request->req; |
|
482 fsm->slave = slave; |
|
483 fsm->state = ec_fsm_master_state_sdo_request; |
|
484 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
|
485 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
486 return 1; |
|
487 } |
|
488 |
|
489 return 0; |
456 return 0; |
490 } |
457 } |
491 |
458 |
492 /*****************************************************************************/ |
459 /*****************************************************************************/ |
493 |
460 |
1045 ec_sdo_request_t *request = fsm->sdo_request; |
1012 ec_sdo_request_t *request = fsm->sdo_request; |
1046 |
1013 |
1047 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1014 if (ec_fsm_coe_exec(&fsm->fsm_coe)) return; |
1048 |
1015 |
1049 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1016 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
1050 EC_DBG("Failed to process SDO request for slave %u.\n", |
1017 EC_DBG("Failed to process internal SDO request for slave %u.\n", |
1051 fsm->slave->ring_position); |
1018 fsm->slave->ring_position); |
1052 request->state = EC_INT_REQUEST_FAILURE; |
1019 request->state = EC_INT_REQUEST_FAILURE; |
1053 wake_up(&master->sdo_queue); |
1020 wake_up(&master->sdo_queue); |
1054 ec_fsm_master_restart(fsm); |
1021 ec_fsm_master_restart(fsm); |
1055 return; |
1022 return; |
1058 // SDO request finished |
1025 // SDO request finished |
1059 request->state = EC_INT_REQUEST_SUCCESS; |
1026 request->state = EC_INT_REQUEST_SUCCESS; |
1060 wake_up(&master->sdo_queue); |
1027 wake_up(&master->sdo_queue); |
1061 |
1028 |
1062 if (master->debug_level) |
1029 if (master->debug_level) |
1063 EC_DBG("Finished SDO request for slave %u.\n", |
1030 EC_DBG("Finished internal SDO request for slave %u.\n", |
1064 fsm->slave->ring_position); |
1031 fsm->slave->ring_position); |
1065 |
1032 |
1066 // check for another SDO request |
1033 // check for another SDO request |
1067 if (ec_fsm_master_action_process_sdo(fsm)) |
1034 if (ec_fsm_master_action_process_sdo(fsm)) |
1068 return; // processing another request |
1035 return; // processing another request |