62 { |
62 { |
63 fsm->slave = slave; |
63 fsm->slave = slave; |
64 fsm->datagram = datagram; |
64 fsm->datagram = datagram; |
65 fsm->datagram->data_size = 0; |
65 fsm->datagram->data_size = 0; |
66 |
66 |
67 if (slave->master->debug_level) |
67 EC_SLAVE_DBG(slave, 1, "Init FSM.\n"); |
68 EC_DBG("Init FSM for slave %u...\n", slave->ring_position); |
|
69 |
68 |
70 fsm->state = ec_fsm_slave_state_idle; |
69 fsm->state = ec_fsm_slave_state_idle; |
71 |
70 |
72 // init sub-state-machines |
71 // init sub-state-machines |
73 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
72 ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram); |
117 void ec_fsm_slave_ready( |
116 void ec_fsm_slave_ready( |
118 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
117 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
119 ) |
118 ) |
120 { |
119 { |
121 if (fsm->state == ec_fsm_slave_state_idle) { |
120 if (fsm->state == ec_fsm_slave_state_idle) { |
122 if (fsm->slave->master->debug_level) { |
121 EC_SLAVE_DBG(fsm->slave, 1, "Ready for requests.\n"); |
123 EC_DBG("Slave %u ready for requests.\n", |
|
124 fsm->slave->ring_position); |
|
125 } |
|
126 fsm->state = ec_fsm_slave_state_ready; |
122 fsm->state = ec_fsm_slave_state_ready; |
127 } |
123 } |
128 } |
124 } |
129 |
125 |
130 /****************************************************************************** |
126 /****************************************************************************** |
171 int ec_fsm_slave_action_process_sdo( |
167 int ec_fsm_slave_action_process_sdo( |
172 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
168 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
173 ) |
169 ) |
174 { |
170 { |
175 ec_slave_t *slave = fsm->slave; |
171 ec_slave_t *slave = fsm->slave; |
176 ec_master_t *master = slave->master; |
|
177 ec_master_sdo_request_t *request, *next; |
172 ec_master_sdo_request_t *request, *next; |
178 |
173 |
179 // search the first external request to be processed |
174 // search the first external request to be processed |
180 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
175 list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) { |
181 |
176 |
182 list_del_init(&request->list); // dequeue |
177 list_del_init(&request->list); // dequeue |
183 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
178 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
184 EC_WARN("Aborting SDO request, slave %u has ERROR.\n", |
179 EC_SLAVE_WARN(slave, "Aborting SDO request," |
185 slave->ring_position); |
180 " slave has error flag set.\n"); |
186 request->req.state = EC_INT_REQUEST_FAILURE; |
181 request->req.state = EC_INT_REQUEST_FAILURE; |
187 wake_up(&slave->sdo_queue); |
182 wake_up(&slave->sdo_queue); |
188 fsm->sdo_request = NULL; |
183 fsm->sdo_request = NULL; |
189 fsm->state = ec_fsm_slave_state_idle; |
184 fsm->state = ec_fsm_slave_state_idle; |
190 return 0; |
185 return 0; |
191 } |
186 } |
192 |
187 |
193 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
188 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
194 EC_WARN("Aborting SDO request, slave %u is in INIT.\n", |
189 EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n"); |
195 slave->ring_position); |
|
196 request->req.state = EC_INT_REQUEST_FAILURE; |
190 request->req.state = EC_INT_REQUEST_FAILURE; |
197 wake_up(&slave->sdo_queue); |
191 wake_up(&slave->sdo_queue); |
198 fsm->sdo_request = NULL; |
192 fsm->sdo_request = NULL; |
199 fsm->state = ec_fsm_slave_state_idle; |
193 fsm->state = ec_fsm_slave_state_idle; |
200 return 0; |
194 return 0; |
201 } |
195 } |
202 |
196 |
203 request->req.state = EC_INT_REQUEST_BUSY; |
197 request->req.state = EC_INT_REQUEST_BUSY; |
204 |
198 |
205 // Found pending SDO request. Execute it! |
199 // Found pending SDO request. Execute it! |
206 if (master->debug_level) |
200 EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n"); |
207 EC_DBG("Processing SDO request for slave %u...\n", |
|
208 slave->ring_position); |
|
209 |
201 |
210 // Start SDO transfer |
202 // Start SDO transfer |
211 fsm->sdo_request = &request->req; |
203 fsm->sdo_request = &request->req; |
212 fsm->state = ec_fsm_slave_state_sdo_request; |
204 fsm->state = ec_fsm_slave_state_sdo_request; |
213 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
205 ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req); |
225 void ec_fsm_slave_state_sdo_request( |
217 void ec_fsm_slave_state_sdo_request( |
226 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
218 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
227 ) |
219 ) |
228 { |
220 { |
229 ec_slave_t *slave = fsm->slave; |
221 ec_slave_t *slave = fsm->slave; |
230 ec_master_t *master = slave->master; |
|
231 ec_sdo_request_t *request = fsm->sdo_request; |
222 ec_sdo_request_t *request = fsm->sdo_request; |
232 |
223 |
233 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
224 if (ec_fsm_coe_exec(&fsm->fsm_coe)) |
234 { |
225 { |
235 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
226 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
236 return; |
227 return; |
237 } |
228 } |
238 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
229 if (!ec_fsm_coe_success(&fsm->fsm_coe)) { |
239 EC_ERR("Failed to process SDO request for slave %u.\n", |
230 EC_SLAVE_ERR(slave, "Failed to process SDO request.\n"); |
240 fsm->slave->ring_position); |
|
241 request->state = EC_INT_REQUEST_FAILURE; |
231 request->state = EC_INT_REQUEST_FAILURE; |
242 wake_up(&slave->sdo_queue); |
232 wake_up(&slave->sdo_queue); |
243 fsm->sdo_request = NULL; |
233 fsm->sdo_request = NULL; |
244 fsm->state = ec_fsm_slave_state_idle; |
234 fsm->state = ec_fsm_slave_state_idle; |
245 return; |
235 return; |
246 } |
236 } |
247 |
237 |
248 if (master->debug_level) |
238 EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n"); |
249 EC_DBG("Finished SDO request for slave %u.\n", |
|
250 fsm->slave->ring_position); |
|
251 |
239 |
252 // SDO request finished |
240 // SDO request finished |
253 request->state = EC_INT_REQUEST_SUCCESS; |
241 request->state = EC_INT_REQUEST_SUCCESS; |
254 wake_up(&slave->sdo_queue); |
242 wake_up(&slave->sdo_queue); |
255 |
243 |
266 int ec_fsm_slave_action_process_foe( |
254 int ec_fsm_slave_action_process_foe( |
267 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
255 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
268 ) |
256 ) |
269 { |
257 { |
270 ec_slave_t *slave = fsm->slave; |
258 ec_slave_t *slave = fsm->slave; |
271 ec_master_t *master = slave->master; |
|
272 ec_master_foe_request_t *request, *next; |
259 ec_master_foe_request_t *request, *next; |
273 |
260 |
274 // search the first request to be processed |
261 // search the first request to be processed |
275 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
262 list_for_each_entry_safe(request, next, &slave->foe_requests, list) { |
276 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
263 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
277 EC_WARN("Aborting FOE request, slave %u has ERROR.\n", |
264 EC_SLAVE_WARN(slave, "Aborting FOE request," |
278 slave->ring_position); |
265 " slave has error flag set.\n"); |
279 request->req.state = EC_INT_REQUEST_FAILURE; |
266 request->req.state = EC_INT_REQUEST_FAILURE; |
280 wake_up(&slave->sdo_queue); |
267 wake_up(&slave->sdo_queue); |
281 fsm->sdo_request = NULL; |
268 fsm->sdo_request = NULL; |
282 fsm->state = ec_fsm_slave_state_idle; |
269 fsm->state = ec_fsm_slave_state_idle; |
283 return 0; |
270 return 0; |
284 } |
271 } |
285 list_del_init(&request->list); // dequeue |
272 list_del_init(&request->list); // dequeue |
286 request->req.state = EC_INT_REQUEST_BUSY; |
273 request->req.state = EC_INT_REQUEST_BUSY; |
287 |
274 |
288 if (master->debug_level) |
275 EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n"); |
289 EC_DBG("Processing FOE request for slave %u.\n", |
|
290 slave->ring_position); |
|
291 |
276 |
292 fsm->foe_request = &request->req; |
277 fsm->foe_request = &request->req; |
293 fsm->state = ec_fsm_slave_state_foe_request; |
278 fsm->state = ec_fsm_slave_state_foe_request; |
294 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
279 ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req); |
295 ec_fsm_foe_exec(&fsm->fsm_foe); |
280 ec_fsm_foe_exec(&fsm->fsm_foe); |
306 void ec_fsm_slave_state_foe_request( |
291 void ec_fsm_slave_state_foe_request( |
307 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
292 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
308 ) |
293 ) |
309 { |
294 { |
310 ec_slave_t *slave = fsm->slave; |
295 ec_slave_t *slave = fsm->slave; |
311 ec_master_t *master = slave->master; |
|
312 ec_foe_request_t *request = fsm->foe_request; |
296 ec_foe_request_t *request = fsm->foe_request; |
313 |
297 |
314 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
298 if (ec_fsm_foe_exec(&fsm->fsm_foe)) |
315 { |
299 { |
316 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
300 ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram); |
317 return; |
301 return; |
318 } |
302 } |
319 |
303 |
320 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
304 if (!ec_fsm_foe_success(&fsm->fsm_foe)) { |
321 EC_ERR("Failed to handle FoE request to slave %u.\n", |
305 EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n"); |
322 slave->ring_position); |
|
323 request->state = EC_INT_REQUEST_FAILURE; |
306 request->state = EC_INT_REQUEST_FAILURE; |
324 wake_up(&slave->foe_queue); |
307 wake_up(&slave->foe_queue); |
325 fsm->foe_request = NULL; |
308 fsm->foe_request = NULL; |
326 fsm->state = ec_fsm_slave_state_idle; |
309 fsm->state = ec_fsm_slave_state_idle; |
327 return; |
310 return; |
328 } |
311 } |
329 |
312 |
330 // finished transferring FoE |
313 // finished transferring FoE |
331 if (master->debug_level) |
314 EC_SLAVE_DBG(slave, 1, "Successfully transferred %u bytes of FoE" |
332 EC_DBG("Successfully transferred %u bytes of FoE data from/to" |
315 " data.\n", request->data_size); |
333 " slave %u.\n", request->data_size, slave->ring_position); |
|
334 |
316 |
335 request->state = EC_INT_REQUEST_SUCCESS; |
317 request->state = EC_INT_REQUEST_SUCCESS; |
336 wake_up(&slave->foe_queue); |
318 wake_up(&slave->foe_queue); |
337 |
319 |
338 fsm->foe_request = NULL; |
320 fsm->foe_request = NULL; |
348 int ec_fsm_slave_action_process_soe( |
330 int ec_fsm_slave_action_process_soe( |
349 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
331 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
350 ) |
332 ) |
351 { |
333 { |
352 ec_slave_t *slave = fsm->slave; |
334 ec_slave_t *slave = fsm->slave; |
353 ec_master_t *master = slave->master; |
|
354 ec_master_soe_request_t *req, *next; |
335 ec_master_soe_request_t *req, *next; |
355 |
336 |
356 // search the first request to be processed |
337 // search the first request to be processed |
357 list_for_each_entry_safe(req, next, &slave->soe_requests, list) { |
338 list_for_each_entry_safe(req, next, &slave->soe_requests, list) { |
358 |
339 |
359 list_del_init(&req->list); // dequeue |
340 list_del_init(&req->list); // dequeue |
360 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
341 if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) { |
361 EC_WARN("Aborting SoE request, slave %u has ERROR.\n", |
342 EC_SLAVE_WARN(slave, "Aborting SoE request," |
362 slave->ring_position); |
343 " slave has error flag set.\n"); |
363 req->req.state = EC_INT_REQUEST_FAILURE; |
344 req->req.state = EC_INT_REQUEST_FAILURE; |
364 wake_up(&slave->soe_queue); |
345 wake_up(&slave->soe_queue); |
365 fsm->state = ec_fsm_slave_state_idle; |
346 fsm->state = ec_fsm_slave_state_idle; |
366 return 0; |
347 return 0; |
367 } |
348 } |
368 |
349 |
369 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
350 if (slave->current_state == EC_SLAVE_STATE_INIT) { |
370 EC_WARN("Aborting SoE request, slave %u is in INIT.\n", |
351 EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n"); |
371 slave->ring_position); |
|
372 req->req.state = EC_INT_REQUEST_FAILURE; |
352 req->req.state = EC_INT_REQUEST_FAILURE; |
373 wake_up(&slave->soe_queue); |
353 wake_up(&slave->soe_queue); |
374 fsm->state = ec_fsm_slave_state_idle; |
354 fsm->state = ec_fsm_slave_state_idle; |
375 return 0; |
355 return 0; |
376 } |
356 } |
377 |
357 |
378 req->req.state = EC_INT_REQUEST_BUSY; |
358 req->req.state = EC_INT_REQUEST_BUSY; |
379 |
359 |
380 // Found pending request. Execute it! |
360 // Found pending request. Execute it! |
381 if (master->debug_level) |
361 EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n"); |
382 EC_DBG("Processing SoE request for slave %u...\n", |
|
383 slave->ring_position); |
|
384 |
362 |
385 // Start SoE transfer |
363 // Start SoE transfer |
386 fsm->soe_request = &req->req; |
364 fsm->soe_request = &req->req; |
387 fsm->state = ec_fsm_slave_state_soe_request; |
365 fsm->state = ec_fsm_slave_state_soe_request; |
388 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req); |
366 ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req); |
400 void ec_fsm_slave_state_soe_request( |
378 void ec_fsm_slave_state_soe_request( |
401 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
379 ec_fsm_slave_t *fsm /**< Slave state machine. */ |
402 ) |
380 ) |
403 { |
381 { |
404 ec_slave_t *slave = fsm->slave; |
382 ec_slave_t *slave = fsm->slave; |
405 ec_master_t *master = slave->master; |
|
406 ec_soe_request_t *request = fsm->soe_request; |
383 ec_soe_request_t *request = fsm->soe_request; |
407 |
384 |
408 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
385 if (ec_fsm_soe_exec(&fsm->fsm_soe)) { |
409 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
386 ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram); |
410 return; |
387 return; |
411 } |
388 } |
412 |
389 |
413 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
390 if (!ec_fsm_soe_success(&fsm->fsm_soe)) { |
414 EC_ERR("Failed to process SoE request for slave %u.\n", |
391 EC_SLAVE_ERR(slave, "Failed to process SoE request.\n"); |
415 fsm->slave->ring_position); |
|
416 request->state = EC_INT_REQUEST_FAILURE; |
392 request->state = EC_INT_REQUEST_FAILURE; |
417 wake_up(&slave->soe_queue); |
393 wake_up(&slave->soe_queue); |
418 fsm->soe_request = NULL; |
394 fsm->soe_request = NULL; |
419 fsm->state = ec_fsm_slave_state_idle; |
395 fsm->state = ec_fsm_slave_state_idle; |
420 return; |
396 return; |
421 } |
397 } |
422 |
398 |
423 if (master->debug_level) |
399 EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n"); |
424 EC_DBG("Finished SoE request for slave %u.\n", |
|
425 fsm->slave->ring_position); |
|
426 |
400 |
427 // SoE request finished |
401 // SoE request finished |
428 request->state = EC_INT_REQUEST_SUCCESS; |
402 request->state = EC_INT_REQUEST_SUCCESS; |
429 wake_up(&slave->soe_queue); |
403 wake_up(&slave->soe_queue); |
430 |
404 |