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. |
92 |
92 |
93 /** Executes the current state of the state machine. |
93 /** Executes the current state of the state machine. |
94 * |
94 * |
95 * If the state machine's datagram is not sent or received yet, the execution |
95 * If the state machine's datagram is not sent or received yet, the execution |
96 * of the state machine is delayed to the next cycle. |
96 * of the state machine is delayed to the next cycle. |
97 */ |
97 * |
98 void ec_fsm_slave_exec( |
98 * \return true, if the state machine was executed |
99 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
99 */ |
100 ) |
100 int ec_fsm_slave_exec( |
101 { |
101 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
102 if (fsm->datagram->state == EC_DATAGRAM_SENT |
102 ) |
103 || fsm->datagram->state == EC_DATAGRAM_QUEUED) { |
103 { |
|
104 if (ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_QUEUED) |
|
105 || ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_SENT)) { |
104 // datagram was not sent or received yet. |
106 // datagram was not sent or received yet. |
105 return; |
107 return 0; |
106 } |
108 } |
107 |
109 |
108 fsm->state(fsm); |
110 fsm->state(fsm); |
|
111 return 1; |
109 } |
112 } |
110 |
113 |
111 /*****************************************************************************/ |
114 /*****************************************************************************/ |
112 |
115 |
113 /** Sets the current state of the state machine to READY |
116 /** Sets the current state of the state machine to READY |
173 // search the first external request to be processed |
176 // search the first external request to be processed |
174 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
177 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
175 |
178 |
176 list_del_init(&request->list); // dequeue |
179 list_del_init(&request->list); // dequeue |
177 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
180 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
178 EC_SLAVE_WARN(slave, "Aborting SDO request," |
181 EC_SLAVE_WARN(slave, "Aborting SDO request %p," |
179 " slave has error flag set.\n"); |
182 " slave has error flag set.\n",request); |
180 request->req.state = EC_INT_REQUEST_FAILURE; |
183 request->req.state = EC_INT_REQUEST_FAILURE; |
|
184 kref_put(&request->refcount,ec_master_sdo_request_release); |
181 wake_up(&slave->sdo_queue); |
185 wake_up(&slave->sdo_queue); |
182 fsm->sdo_request = NULL; |
186 fsm->sdo_request = NULL; |
183 fsm->state = ec_fsm_slave_state_idle; |
187 fsm->state = ec_fsm_slave_state_idle; |
184 return 0; |
188 return 0; |
185 } |
189 } |
186 |
190 |
187 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
191 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
188 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
192 EC_SLAVE_WARN(slave, "Aborting SDO request %p, slave is in INIT.\n",request); |
189 request->req.state = EC_INT_REQUEST_FAILURE; |
193 request->req.state = EC_INT_REQUEST_FAILURE; |
|
194 kref_put(&request->refcount,ec_master_sdo_request_release); |
190 wake_up(&slave->sdo_queue); |
195 wake_up(&slave->sdo_queue); |
191 fsm->sdo_request = NULL; |
196 fsm->sdo_request = NULL; |
192 fsm->state = ec_fsm_slave_state_idle; |
197 fsm->state = ec_fsm_slave_state_idle; |
193 return 0; |
198 return 0; |
194 } |
199 } |
195 |
200 |
196 request->req.state = EC_INT_REQUEST_BUSY; |
201 request->req.state = EC_INT_REQUEST_BUSY; |
197 |
202 |
198 // Found pending SDO request. Execute it! |
203 // Found pending SDO request. Execute it! |
199 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
204 EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n",request); |
200 |
205 |
201 // Start SDO transfer |
206 // Start SDO transfer |
202 fsm->sdo_request = &request->req; |
207 fsm->sdo_request = request; |
203 fsm->state = ec_fsm_slave_state_sdo_request; |
208 fsm->state = ec_fsm_slave_state_sdo_request; |
204 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
209 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
205 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
210 ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately |
206 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
211 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
207 return 1; |
212 return 1; |
208 } |
213 } |
209 return 0; |
214 return 0; |
210 } |
215 } |
211 |
216 |
216 void ec_fsm_slave_state_sdo_request( |
221 void ec_fsm_slave_state_sdo_request( |
217 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
222 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
218 ) |
223 ) |
219 { |
224 { |
220 ec_slave_t *slave = fsm->slave; |
225 ec_slave_t *slave = fsm->slave; |
221 ec_sdo_request_t *request = fsm->sdo_request; |
226 ec_master_sdo_request_t *request = fsm->sdo_request; |
222 |
227 |
223 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
228 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
224 { |
229 { |
225 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
230 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
226 return; |
231 return; |
227 } |
232 } |
228 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
233 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
229 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
234 EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request); |
230 request->state = EC_INT_REQUEST_FAILURE; |
235 request->req.state = EC_INT_REQUEST_FAILURE; |
|
236 kref_put(&request->refcount,ec_master_sdo_request_release); |
231 wake_up(&slave->sdo_queue); |
237 wake_up(&slave->sdo_queue); |
232 fsm->sdo_request = NULL; |
238 fsm->sdo_request = NULL; |
233 fsm->state = ec_fsm_slave_state_idle; |
239 fsm->state = ec_fsm_slave_state_idle; |
234 return; |
240 return; |
235 } |
241 } |
236 |
242 |
237 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
243 EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n",request); |
238 |
244 |
239 // SDO request finished |
245 // SDO request finished |
240 request->state = EC_INT_REQUEST_SUCCESS; |
246 request->req.state = EC_INT_REQUEST_SUCCESS; |
|
247 kref_put(&request->refcount,ec_master_sdo_request_release); |
241 wake_up(&slave->sdo_queue); |
248 wake_up(&slave->sdo_queue); |
242 |
249 |
243 fsm->sdo_request = NULL; |
250 fsm->sdo_request = NULL; |
244 fsm->state = ec_fsm_slave_state_ready; |
251 fsm->state = ec_fsm_slave_state_ready; |
245 } |
252 } |
258 ec_master_foe_request_t *request, *next; |
265 ec_master_foe_request_t *request, *next; |
259 |
266 |
260 // search the first request to be processed |
267 // search the first request to be processed |
261 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
268 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
262 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
269 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
263 EC_SLAVE_WARN(slave, "Aborting FOE request," |
270 EC_SLAVE_WARN(slave, "Aborting FOE request %p," |
264 " slave has error flag set.\n"); |
271 " slave has error flag set.\n",request); |
265 request->req.state = EC_INT_REQUEST_FAILURE; |
272 request->req.state = EC_INT_REQUEST_FAILURE; |
266 wake_up(&slave->sdo_queue); |
273 kref_put(&request->refcount,ec_master_foe_request_release); |
|
274 wake_up(&slave->foe_queue); |
267 fsm->sdo_request = NULL; |
275 fsm->sdo_request = NULL; |
268 fsm->state = ec_fsm_slave_state_idle; |
276 fsm->state = ec_fsm_slave_state_idle; |
269 return 0; |
277 return 0; |
270 } |
278 } |
271 list_del_init(&request->list); // dequeue |
279 list_del_init(&request->list); // dequeue |
272 request->req.state = EC_INT_REQUEST_BUSY; |
280 request->req.state = EC_INT_REQUEST_BUSY; |
273 |
281 |
274 EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); |
282 EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n",request); |
275 |
283 |
276 fsm->foe_request = &request->req; |
284 fsm->foe_request = request; |
277 fsm->state = ec_fsm_slave_state_foe_request; |
285 fsm->state = ec_fsm_slave_state_foe_request; |
278 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
286 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
279 ec_fsm_foe_exec(&fsm->fsm_foe); |
287 ec_fsm_foe_exec(&fsm->fsm_foe); |
280 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
288 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
281 return 1; |
289 return 1; |
282 } |
290 } |
283 return 0; |
291 return 0; |
284 } |
292 } |
285 |
293 |
290 void ec_fsm_slave_state_foe_request( |
298 void ec_fsm_slave_state_foe_request( |
291 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
299 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
292 ) |
300 ) |
293 { |
301 { |
294 ec_slave_t *slave = fsm->slave; |
302 ec_slave_t *slave = fsm->slave; |
295 ec_foe_request_t *request = fsm->foe_request; |
303 ec_master_foe_request_t *request = fsm->foe_request; |
296 |
304 |
297 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
305 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
298 { |
306 { |
299 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
307 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
300 return; |
308 return; |
301 } |
309 } |
302 |
310 |
303 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
311 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
304 EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); |
312 EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n",request); |
305 request->state = EC_INT_REQUEST_FAILURE; |
313 request->req.state = EC_INT_REQUEST_FAILURE; |
|
314 kref_put(&request->refcount,ec_master_foe_request_release); |
306 wake_up(&slave->foe_queue); |
315 wake_up(&slave->foe_queue); |
307 fsm->foe_request = NULL; |
316 fsm->foe_request = NULL; |
308 fsm->state = ec_fsm_slave_state_idle; |
317 fsm->state = ec_fsm_slave_state_idle; |
309 return; |
318 return; |
310 } |
319 } |
311 |
320 |
312 // finished transferring FoE |
321 // finished transferring FoE |
313 EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE" |
322 EC_SLAVE_DBG(slave, 1, "FoE request %p successfully transferred %zu bytes.\n", |
314 " data.\n", request->data_size); |
323 request,request->req.data_size); |
315 |
324 |
316 request->state = EC_INT_REQUEST_SUCCESS; |
325 request->req.state = EC_INT_REQUEST_SUCCESS; |
|
326 kref_put(&request->refcount,ec_master_foe_request_release); |
317 wake_up(&slave->foe_queue); |
327 wake_up(&slave->foe_queue); |
318 |
328 |
319 fsm->foe_request = NULL; |
329 fsm->foe_request = NULL; |
320 fsm->state = ec_fsm_slave_state_ready; |
330 fsm->state = ec_fsm_slave_state_ready; |
321 } |
331 } |
329 int ec_fsm_slave_action_process_soe( |
339 int ec_fsm_slave_action_process_soe( |
330 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
340 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
331 ) |
341 ) |
332 { |
342 { |
333 ec_slave_t *slave = fsm->slave; |
343 ec_slave_t *slave = fsm->slave; |
334 ec_master_soe_request_t *req, *next; |
344 ec_master_soe_request_t *request, *next; |
335 |
345 |
336 // search the first request to be processed |
346 // search the first request to be processed |
337 list_for_each_entry_safe(req, next, &slave->soe_requests, list) { |
347 list_for_each_entry_safe(request, next, &slave->soe_requests, list) { |
338 |
348 |
339 list_del_init(&req->list); // dequeue |
349 list_del_init(&request->list); // dequeue |
340 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
350 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
341 EC_SLAVE_WARN(slave, "Aborting SoE request," |
351 EC_SLAVE_WARN(slave, "Aborting SoE request," |
342 " slave has error flag set.\n"); |
352 " slave has error flag set.\n"); |
343 req->req.state = EC_INT_REQUEST_FAILURE; |
353 request->req.state = EC_INT_REQUEST_FAILURE; |
|
354 kref_put(&request->refcount,ec_master_soe_request_release); |
344 wake_up(&slave->soe_queue); |
355 wake_up(&slave->soe_queue); |
345 fsm->state = ec_fsm_slave_state_idle; |
356 fsm->state = ec_fsm_slave_state_idle; |
346 return 0; |
357 return 0; |
347 } |
358 } |
348 |
359 |
349 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
360 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
350 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
361 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
351 req->req.state = EC_INT_REQUEST_FAILURE; |
362 request->req.state = EC_INT_REQUEST_FAILURE; |
|
363 kref_put(&request->refcount,ec_master_soe_request_release); |
352 wake_up(&slave->soe_queue); |
364 wake_up(&slave->soe_queue); |
353 fsm->state = ec_fsm_slave_state_idle; |
365 fsm->state = ec_fsm_slave_state_idle; |
354 return 0; |
366 return 0; |
355 } |
367 } |
356 |
368 |
357 req->req.state = EC_INT_REQUEST_BUSY; |
369 request->req.state = EC_INT_REQUEST_BUSY; |
358 |
370 |
359 // Found pending request. Execute it! |
371 // Found pending request. Execute it! |
360 EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); |
372 EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); |
361 |
373 |
362 // Start SoE transfer |
374 // Start SoE transfer |
363 fsm->soe_request = &req->req; |
375 fsm->soe_request = request; |
364 fsm->state = ec_fsm_slave_state_soe_request; |
376 fsm->state = ec_fsm_slave_state_soe_request; |
365 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req); |
377 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req); |
366 ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately |
378 ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately |
367 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
379 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
368 return 1; |
380 return 1; |
369 } |
381 } |
370 return 0; |
382 return 0; |
371 } |
383 } |
372 |
384 |
377 void ec_fsm_slave_state_soe_request( |
389 void ec_fsm_slave_state_soe_request( |
378 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
390 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
379 ) |
391 ) |
380 { |
392 { |
381 ec_slave_t *slave = fsm->slave; |
393 ec_slave_t *slave = fsm->slave; |
382 ec_soe_request_t *request = fsm->soe_request; |
394 ec_master_soe_request_t *request = fsm->soe_request; |
383 |
395 |
384 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
396 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
385 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
397 ec_slave_mbox_queue_datagrams(slave, fsm->mbox); |
386 return; |
398 return; |
387 } |
399 } |
388 |
400 |
389 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
401 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
390 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
402 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
391 request->state = EC_INT_REQUEST_FAILURE; |
403 request->req.state = EC_INT_REQUEST_FAILURE; |
|
404 kref_put(&request->refcount,ec_master_soe_request_release); |
392 wake_up(&slave->soe_queue); |
405 wake_up(&slave->soe_queue); |
393 fsm->soe_request = NULL; |
406 fsm->soe_request = NULL; |
394 fsm->state = ec_fsm_slave_state_idle; |
407 fsm->state = ec_fsm_slave_state_idle; |
395 return; |
408 return; |
396 } |
409 } |
397 |
410 |
398 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
411 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
399 |
412 |
400 // SoE request finished |
413 // SoE request finished |
401 request->state = EC_INT_REQUEST_SUCCESS; |
414 request->req.state = EC_INT_REQUEST_SUCCESS; |
|
415 kref_put(&request->refcount,ec_master_soe_request_release); |
402 wake_up(&slave->soe_queue); |
416 wake_up(&slave->soe_queue); |
403 |
417 |
404 fsm->soe_request = NULL; |
418 fsm->soe_request = NULL; |
405 fsm->state = ec_fsm_slave_state_ready; |
419 fsm->state = ec_fsm_slave_state_ready; |
406 } |
420 } |