50 void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *); |
51 void ec_fsm_master_state_clear_addresses(ec_fsm_master_t *); |
51 void ec_fsm_master_state_scan_slave(ec_fsm_master_t *); |
52 void ec_fsm_master_state_scan_slave(ec_fsm_master_t *); |
52 void ec_fsm_master_state_write_sii(ec_fsm_master_t *); |
53 void ec_fsm_master_state_write_sii(ec_fsm_master_t *); |
53 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
54 void ec_fsm_master_state_sdo_dictionary(ec_fsm_master_t *); |
54 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
55 void ec_fsm_master_state_sdo_request(ec_fsm_master_t *); |
|
56 void ec_fsm_master_state_foe_request(ec_fsm_master_t *); |
55 |
57 |
56 /*****************************************************************************/ |
58 /*****************************************************************************/ |
57 |
59 |
58 /** Constructor. |
60 /** Constructor. |
59 */ |
61 */ |
71 fsm->topology_change_pending = 0; |
73 fsm->topology_change_pending = 0; |
72 fsm->slave_states = EC_SLAVE_STATE_UNKNOWN; |
74 fsm->slave_states = EC_SLAVE_STATE_UNKNOWN; |
73 |
75 |
74 // init sub-state-machines |
76 // init sub-state-machines |
75 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
77 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
|
78 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
76 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
79 ec_fsm_pdo_init(&fsm->fsm_pdo, &fsm->fsm_coe); |
77 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
80 ec_fsm_change_init(&fsm->fsm_change, fsm->datagram); |
78 ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, |
81 ec_fsm_slave_config_init(&fsm->fsm_slave_config, fsm->datagram, |
79 &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_pdo); |
82 &fsm->fsm_change, &fsm->fsm_coe, &fsm->fsm_pdo); |
80 ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, |
83 ec_fsm_slave_scan_init(&fsm->fsm_slave_scan, fsm->datagram, |
90 ec_fsm_master_t *fsm /**< Master state machine. */ |
93 ec_fsm_master_t *fsm /**< Master state machine. */ |
91 ) |
94 ) |
92 { |
95 { |
93 // clear sub-state machines |
96 // clear sub-state machines |
94 ec_fsm_coe_clear(&fsm->fsm_coe); |
97 ec_fsm_coe_clear(&fsm->fsm_coe); |
|
98 ec_fsm_foe_clear(&fsm->fsm_foe); |
95 ec_fsm_pdo_clear(&fsm->fsm_pdo); |
99 ec_fsm_pdo_clear(&fsm->fsm_pdo); |
96 ec_fsm_change_clear(&fsm->fsm_change); |
100 ec_fsm_change_clear(&fsm->fsm_change); |
97 ec_fsm_slave_config_clear(&fsm->fsm_slave_config); |
101 ec_fsm_slave_config_clear(&fsm->fsm_slave_config); |
98 ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan); |
102 ec_fsm_slave_scan_clear(&fsm->fsm_slave_scan); |
99 ec_fsm_sii_clear(&fsm->fsm_sii); |
103 ec_fsm_sii_clear(&fsm->fsm_sii); |
207 if (!master->allow_scan) { |
211 if (!master->allow_scan) { |
208 up(&master->scan_sem); |
212 up(&master->scan_sem); |
209 } else { |
213 } else { |
210 master->scan_busy = 1; |
214 master->scan_busy = 1; |
211 up(&master->scan_sem); |
215 up(&master->scan_sem); |
212 |
216 |
213 // topology change when scan is allowed: |
217 // topology change when scan is allowed: |
214 // clear all slaves and scan the bus |
218 // clear all slaves and scan the bus |
215 fsm->topology_change_pending = 0; |
219 fsm->topology_change_pending = 0; |
216 fsm->idle = 0; |
220 fsm->idle = 0; |
217 fsm->scan_jiffies = jiffies; |
221 fsm->scan_jiffies = jiffies; |
276 } |
280 } |
277 |
281 |
278 /*****************************************************************************/ |
282 /*****************************************************************************/ |
279 |
283 |
280 /** Check for pending SII write requests and process one. |
284 /** Check for pending SII write requests and process one. |
281 * |
285 * |
282 * \return non-zero, if an SII write request is processed. |
286 * \return non-zero, if an SII write request is processed. |
283 */ |
287 */ |
284 int ec_fsm_master_action_process_sii( |
288 int ec_fsm_master_action_process_sii( |
285 ec_fsm_master_t *fsm /**< Master state machine. */ |
289 ec_fsm_master_t *fsm /**< Master state machine. */ |
286 ) |
290 ) |
316 } |
320 } |
317 |
321 |
318 /*****************************************************************************/ |
322 /*****************************************************************************/ |
319 |
323 |
320 /** Check for pending SDO requests and process one. |
324 /** Check for pending SDO requests and process one. |
321 * |
325 * |
322 * \return non-zero, if an SDO request is processed. |
326 * \return non-zero, if an SDO request is processed. |
323 */ |
327 */ |
324 int ec_fsm_master_action_process_sdo( |
328 int ec_fsm_master_action_process_sdo( |
325 ec_fsm_master_t *fsm /**< Master state machine. */ |
329 ec_fsm_master_t *fsm /**< Master state machine. */ |
326 ) |
330 ) |
398 fsm->sdo_request = &request->req; |
402 fsm->sdo_request = &request->req; |
399 fsm->slave = slave; |
403 fsm->slave = slave; |
400 fsm->state = ec_fsm_master_state_sdo_request; |
404 fsm->state = ec_fsm_master_state_sdo_request; |
401 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
405 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
402 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
406 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
|
407 return 1; |
|
408 } |
|
409 |
|
410 return 0; |
|
411 } |
|
412 |
|
413 /*****************************************************************************/ |
|
414 |
|
415 /** Check for pending FoE requests and process one. |
|
416 * |
|
417 * \return non-zero, if an FoE request is processed. |
|
418 */ |
|
419 int ec_fsm_master_action_process_foe( |
|
420 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
421 ) |
|
422 { |
|
423 ec_master_t *master = fsm->master; |
|
424 ec_slave_t *slave; |
|
425 ec_master_foe_request_t *request; |
|
426 |
|
427 // search the first request to be processed |
|
428 while (1) { |
|
429 if (list_empty(&master->foe_requests)) |
|
430 break; |
|
431 |
|
432 // get first request |
|
433 request = list_entry(master->foe_requests.next, |
|
434 ec_master_foe_request_t, list); |
|
435 list_del_init(&request->list); // dequeue |
|
436 request->req.state = EC_REQUEST_BUSY; |
|
437 slave = request->slave; |
|
438 |
|
439 EC_DBG("---- Master read command from queue ----\n"); |
|
440 // found pending FOE write operation. execute it! |
|
441 if (master->debug_level) |
|
442 EC_DBG("Writing FOE data to slave %u...\n", |
|
443 request->slave->ring_position); |
|
444 |
|
445 fsm->foe_request = &request->req; |
|
446 fsm->slave = slave; |
|
447 fsm->state = ec_fsm_master_state_foe_request; |
|
448 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
|
449 //(&fsm->fsm_foe, request->slave, request->offset, request->words); |
|
450 ec_fsm_foe_exec(&fsm->fsm_foe); |
403 return 1; |
451 return 1; |
404 } |
452 } |
405 |
453 |
406 return 0; |
454 return 0; |
407 } |
455 } |
657 void ec_fsm_master_state_scan_slave( |
705 void ec_fsm_master_state_scan_slave( |
658 ec_fsm_master_t *fsm /**< Master state machine. */ |
706 ec_fsm_master_t *fsm /**< Master state machine. */ |
659 ) |
707 ) |
660 { |
708 { |
661 ec_master_t *master = fsm->master; |
709 ec_master_t *master = fsm->master; |
|
710 #ifdef EC_EOE |
662 ec_slave_t *slave = fsm->slave; |
711 ec_slave_t *slave = fsm->slave; |
663 |
712 #endif |
664 if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan)) |
713 if (ec_fsm_slave_scan_exec(&fsm->fsm_slave_scan)) |
665 return; |
714 return; |
666 |
715 |
667 #ifdef EC_EOE |
716 #ifdef EC_EOE |
668 if (slave->sii.mailbox_protocols & EC_MBOX_EOE) { |
717 if (slave->sii.mailbox_protocols & EC_MBOX_EOE) { |
776 if (request->offset <= 4 && request->offset + request->nwords > 4) { |
825 if (request->offset <= 4 && request->offset + request->nwords > 4) { |
777 // alias was written |
826 // alias was written |
778 slave->sii.alias = EC_READ_U16(request->words + 4); |
827 slave->sii.alias = EC_READ_U16(request->words + 4); |
779 } |
828 } |
780 // TODO: Evaluate other SII contents! |
829 // TODO: Evaluate other SII contents! |
781 |
830 |
782 request->state = EC_REQUEST_SUCCESS; |
831 request->state = EC_REQUEST_SUCCESS; |
783 wake_up(&master->sii_queue); |
832 wake_up(&master->sii_queue); |
784 |
833 |
785 // check for another SII write request |
834 // check for another SII write request |
786 if (ec_fsm_master_action_process_sii(fsm)) |
835 if (ec_fsm_master_action_process_sii(fsm)) |
787 return; // processing another request |
836 return; // processing another request |
|
837 |
|
838 ec_fsm_master_restart(fsm); |
|
839 } |
|
840 |
|
841 /*****************************************************************************/ |
|
842 |
|
843 /** Master state: WRITE FOE. |
|
844 */ |
|
845 void ec_fsm_master_state_foe_request( |
|
846 ec_fsm_master_t *fsm /**< Master state machine. */ |
|
847 ) |
|
848 { |
|
849 ec_master_t *master = fsm->master; |
|
850 ec_foe_request_t *request = fsm->foe_request; |
|
851 ec_slave_t *slave = fsm->slave; |
|
852 |
|
853 if (ec_fsm_foe_exec(&fsm->fsm_foe)) return; |
|
854 |
|
855 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
|
856 EC_ERR("Failed to handle FOE request to slave %u.\n", |
|
857 slave->ring_position); |
|
858 request->state = EC_REQUEST_FAILURE; |
|
859 wake_up(&master->foe_queue); |
|
860 ec_fsm_master_restart(fsm); |
|
861 return; |
|
862 } |
|
863 |
|
864 // finished writing FOE |
|
865 if (master->debug_level) |
|
866 EC_DBG("Finished writing %u words of FOE data to slave %u.\n", |
|
867 request->data_size, slave->ring_position); |
|
868 |
|
869 request->state = EC_REQUEST_SUCCESS; |
|
870 wake_up(&master->foe_queue); |
788 |
871 |
789 ec_fsm_master_restart(fsm); |
872 ec_fsm_master_restart(fsm); |
790 } |
873 } |
791 |
874 |
792 /*****************************************************************************/ |
875 /*****************************************************************************/ |
846 wake_up(&master->sdo_queue); |
929 wake_up(&master->sdo_queue); |
847 ec_fsm_master_restart(fsm); |
930 ec_fsm_master_restart(fsm); |
848 return; |
931 return; |
849 } |
932 } |
850 |
933 |
851 // SDO request finished |
934 // SDO request finished |
852 request->state = EC_REQUEST_SUCCESS; |
935 request->state = EC_REQUEST_SUCCESS; |
853 wake_up(&master->sdo_queue); |
936 wake_up(&master->sdo_queue); |
854 |
937 |
855 if (master->debug_level) |
938 if (master->debug_level) |
856 EC_DBG("Finished SDO request for slave %u.\n", |
939 EC_DBG("Finished SDO request for slave %u.\n", |