55 /** Constructor. |
55 /** Constructor. |
56 */ |
56 */ |
57 void ec_fsm_slave_init( |
57 void ec_fsm_slave_init( |
58 ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
58 ec_fsm_slave_t *fsm, /**< Slave state machine. */ |
59 ec_slave_t *slave, /**< EtherCAT slave. */ |
59 ec_slave_t *slave, /**< EtherCAT slave. */ |
60 ec_datagram_t *datagram /**< Datagram object to use. */ |
60 ec_mailbox_t *mbox/**< Datagram object to use. */ |
61 ) |
61 ) |
62 { |
62 { |
63 fsm->slave = slave; |
63 fsm->slave = slave; |
64 fsm->datagram = datagram; |
64 fsm->mbox = mbox; |
65 fsm->datagram->data_size = 0; |
65 slave->datagram.data_size = 0; |
66 |
66 |
67 EC_SLAVE_DBG(slave, 1, "Init FSM.\n"); |
67 EC_SLAVE_DBG(slave, 1, "Init FSM.\n"); |
68 |
68 |
69 fsm->state = ec_fsm_slave_state_idle; |
69 fsm->state = ec_fsm_slave_state_idle; |
70 |
70 |
71 // init sub-state-machines |
71 // init sub-state-machines |
72 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
72 ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox); |
73 ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram); |
73 ec_fsm_foe_init(&fsm->fsm_foe, fsm->mbox); |
74 ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram); |
74 ec_fsm_soe_init(&fsm->fsm_soe, fsm->mbox); |
75 } |
75 } |
76 |
76 |
77 /*****************************************************************************/ |
77 /*****************************************************************************/ |
78 |
78 |
79 /** Destructor. |
79 /** Destructor. |
99 */ |
99 */ |
100 int ec_fsm_slave_exec( |
100 int ec_fsm_slave_exec( |
101 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
101 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
102 ) |
102 ) |
103 { |
103 { |
104 if (fsm->datagram->state == EC_DATAGRAM_SENT |
104 if (ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_QUEUED) |
105 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
105 || ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_SENT)) { |
106 // datagram was not sent or received yet. |
106 // datagram was not sent or received yet. |
107 return 0; |
107 return 0; |
108 } |
108 } |
109 |
109 |
110 fsm->state(fsm); |
110 fsm->state(fsm); |
206 // Start SDO transfer |
206 // Start SDO transfer |
207 fsm->sdo_request = request; |
207 fsm->sdo_request = request; |
208 fsm->state = ec_fsm_slave_state_sdo_request; |
208 fsm->state = ec_fsm_slave_state_sdo_request; |
209 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
209 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
210 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
210 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
211 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
211 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
212 return 1; |
212 return 1; |
213 } |
213 } |
214 return 0; |
214 return 0; |
215 } |
215 } |
216 |
216 |
225 ec_slave_t *slave = fsm->slave; |
225 ec_slave_t *slave = fsm->slave; |
226 ec_master_sdo_request_t *request = fsm->sdo_request; |
226 ec_master_sdo_request_t *request = fsm->sdo_request; |
227 |
227 |
228 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
228 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
229 { |
229 { |
230 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
230 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
231 return; |
231 return; |
232 } |
232 } |
233 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
233 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
234 EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request); |
234 EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request); |
235 request->req.state = EC_INT_REQUEST_FAILURE; |
235 request->req.state = EC_INT_REQUEST_FAILURE; |
283 |
283 |
284 fsm->foe_request = request; |
284 fsm->foe_request = request; |
285 fsm->state = ec_fsm_slave_state_foe_request; |
285 fsm->state = ec_fsm_slave_state_foe_request; |
286 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
286 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
287 ec_fsm_foe_exec(&fsm->fsm_foe); |
287 ec_fsm_foe_exec(&fsm->fsm_foe); |
288 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
288 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
289 return 1; |
289 return 1; |
290 } |
290 } |
291 return 0; |
291 return 0; |
292 } |
292 } |
293 |
293 |
302 ec_slave_t *slave = fsm->slave; |
302 ec_slave_t *slave = fsm->slave; |
303 ec_master_foe_request_t *request = fsm->foe_request; |
303 ec_master_foe_request_t *request = fsm->foe_request; |
304 |
304 |
305 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
305 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
306 { |
306 { |
307 ec_master_queue_request_fsm_datagram(fsm->slave->master,fsm->datagram); |
307 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
308 return; |
308 return; |
309 } |
309 } |
310 |
310 |
311 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
311 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
312 EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n",request); |
312 EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n",request); |
374 // Start SoE transfer |
374 // Start SoE transfer |
375 fsm->soe_request = request; |
375 fsm->soe_request = request; |
376 fsm->state = ec_fsm_slave_state_soe_request; |
376 fsm->state = ec_fsm_slave_state_soe_request; |
377 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req); |
377 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req); |
378 ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately |
378 ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately |
379 ec_master_queue_request_fsm_datagram(fsm->slave->master, fsm->datagram); |
379 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
380 return 1; |
380 return 1; |
381 } |
381 } |
382 return 0; |
382 return 0; |
383 } |
383 } |
384 |
384 |
392 { |
392 { |
393 ec_slave_t *slave = fsm->slave; |
393 ec_slave_t *slave = fsm->slave; |
394 ec_master_soe_request_t *request = fsm->soe_request; |
394 ec_master_soe_request_t *request = fsm->soe_request; |
395 |
395 |
396 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
396 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
397 ec_master_queue_request_fsm_datagram(fsm->slave->master, fsm->datagram); |
397 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
398 return; |
398 return; |
399 } |
399 } |
400 |
400 |
401 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
401 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
402 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
402 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |