40 |
39 |
41 #include "fsm_pdo.h" |
40 #include "fsm_pdo.h" |
42 |
41 |
43 /*****************************************************************************/ |
42 /*****************************************************************************/ |
44 |
43 |
45 void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *); |
44 void ec_fsm_pdo_read_state_start(ec_fsm_pdo_t *, ec_datagram_t *); |
46 void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *); |
45 void ec_fsm_pdo_read_state_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *); |
47 void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *); |
46 void ec_fsm_pdo_read_state_pdo(ec_fsm_pdo_t *, ec_datagram_t *); |
48 void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *); |
47 void ec_fsm_pdo_read_state_pdo_entries(ec_fsm_pdo_t *, ec_datagram_t *); |
49 |
48 |
50 void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *); |
49 void ec_fsm_pdo_read_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *); |
51 void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *); |
50 void ec_fsm_pdo_read_action_next_pdo(ec_fsm_pdo_t *, ec_datagram_t *); |
52 |
51 |
53 void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *); |
52 void ec_fsm_pdo_conf_state_start(ec_fsm_pdo_t *, ec_datagram_t *); |
54 void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *); |
53 void ec_fsm_pdo_conf_state_read_mapping(ec_fsm_pdo_t *, ec_datagram_t *); |
55 void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *); |
54 void ec_fsm_pdo_conf_state_mapping(ec_fsm_pdo_t *, ec_datagram_t *); |
56 void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *); |
55 void ec_fsm_pdo_conf_state_zero_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *); |
57 void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *); |
56 void ec_fsm_pdo_conf_state_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *); |
58 void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *); |
57 void ec_fsm_pdo_conf_state_set_pdo_count(ec_fsm_pdo_t *, ec_datagram_t *); |
59 |
58 |
60 void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *); |
59 void ec_fsm_pdo_conf_action_next_sync(ec_fsm_pdo_t *, ec_datagram_t *); |
61 void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *); |
60 void ec_fsm_pdo_conf_action_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *); |
62 void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *); |
61 void ec_fsm_pdo_conf_action_check_mapping(ec_fsm_pdo_t *, ec_datagram_t *); |
63 void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *); |
62 void ec_fsm_pdo_conf_action_next_pdo_mapping(ec_fsm_pdo_t *, ec_datagram_t *); |
64 void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *); |
63 void ec_fsm_pdo_conf_action_check_assignment(ec_fsm_pdo_t *, ec_datagram_t *); |
65 void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *); |
64 void ec_fsm_pdo_conf_action_assign_pdo(ec_fsm_pdo_t *, ec_datagram_t *); |
66 |
65 |
67 void ec_fsm_pdo_state_end(ec_fsm_pdo_t *); |
66 void ec_fsm_pdo_state_end(ec_fsm_pdo_t *, ec_datagram_t *); |
68 void ec_fsm_pdo_state_error(ec_fsm_pdo_t *); |
67 void ec_fsm_pdo_state_error(ec_fsm_pdo_t *, ec_datagram_t *); |
69 |
68 |
70 /*****************************************************************************/ |
69 /*****************************************************************************/ |
71 |
70 |
72 /** Constructor. |
71 /** Constructor. |
73 */ |
72 */ |
187 *****************************************************************************/ |
188 *****************************************************************************/ |
188 |
189 |
189 /** Start reading PDO assignment. |
190 /** Start reading PDO assignment. |
190 */ |
191 */ |
191 void ec_fsm_pdo_read_state_start( |
192 void ec_fsm_pdo_read_state_start( |
192 ec_fsm_pdo_t *fsm /**< finite state machine */ |
193 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
194 ec_datagram_t *datagram /**< Datagram to use. */ |
193 ) |
195 ) |
194 { |
196 { |
195 // read PDO assignment for first sync manager not reserved for mailbox |
197 // read PDO assignment for first sync manager not reserved for mailbox |
196 fsm->sync_index = 1; // next is 2 |
198 fsm->sync_index = 1; // next is 2 |
197 ec_fsm_pdo_read_action_next_sync(fsm); |
199 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
198 } |
200 } |
199 |
201 |
200 /*****************************************************************************/ |
202 /*****************************************************************************/ |
201 |
203 |
202 /** Read PDO assignment of next sync manager. |
204 /** Read PDO assignment of next sync manager. |
203 */ |
205 */ |
204 void ec_fsm_pdo_read_action_next_sync( |
206 void ec_fsm_pdo_read_action_next_sync( |
205 ec_fsm_pdo_t *fsm /**< Finite state machine */ |
207 ec_fsm_pdo_t *fsm, /**< finite state machine. */ |
|
208 ec_datagram_t *datagram /**< Datagram to use. */ |
206 ) |
209 ) |
207 { |
210 { |
208 ec_slave_t *slave = fsm->slave; |
211 ec_slave_t *slave = fsm->slave; |
209 |
212 |
210 fsm->sync_index++; |
213 fsm->sync_index++; |
235 /*****************************************************************************/ |
238 /*****************************************************************************/ |
236 |
239 |
237 /** Count assigned PDOs. |
240 /** Count assigned PDOs. |
238 */ |
241 */ |
239 void ec_fsm_pdo_read_state_pdo_count( |
242 void ec_fsm_pdo_read_state_pdo_count( |
240 ec_fsm_pdo_t *fsm /**< finite state machine */ |
243 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
241 ) |
244 ec_datagram_t *datagram /**< Datagram to use. */ |
242 { |
245 ) |
243 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
246 { |
|
247 if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { |
|
248 return; |
|
249 } |
244 |
250 |
245 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
251 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
246 EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs" |
252 EC_SLAVE_ERR(fsm->slave, "Failed to read number of assigned PDOs" |
247 " for SM%u.\n", fsm->sync_index); |
253 " for SM%u.\n", fsm->sync_index); |
248 ec_fsm_pdo_read_action_next_sync(fsm); |
254 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
249 return; |
255 return; |
250 } |
256 } |
251 |
257 |
252 if (fsm->request.data_size != sizeof(uint8_t)) { |
258 if (fsm->request.data_size != sizeof(uint8_t)) { |
253 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" |
259 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" |
254 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, |
260 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, |
255 fsm->request.index, fsm->request.subindex); |
261 fsm->request.index, fsm->request.subindex); |
256 ec_fsm_pdo_read_action_next_sync(fsm); |
262 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
257 return; |
263 return; |
258 } |
264 } |
259 fsm->pdo_count = EC_READ_U8(fsm->request.data); |
265 fsm->pdo_count = EC_READ_U8(fsm->request.data); |
260 |
266 |
261 EC_SLAVE_DBG(fsm->slave, 1, "%u PDOs assigned.\n", fsm->pdo_count); |
267 EC_SLAVE_DBG(fsm->slave, 1, "%u PDOs assigned.\n", fsm->pdo_count); |
262 |
268 |
263 // read first PDO |
269 // read first PDO |
264 fsm->pdo_pos = 1; |
270 fsm->pdo_pos = 1; |
265 ec_fsm_pdo_read_action_next_pdo(fsm); |
271 ec_fsm_pdo_read_action_next_pdo(fsm, datagram); |
266 } |
272 } |
267 |
273 |
268 /*****************************************************************************/ |
274 /*****************************************************************************/ |
269 |
275 |
270 /** Read next PDO. |
276 /** Read next PDO. |
271 */ |
277 */ |
272 void ec_fsm_pdo_read_action_next_pdo( |
278 void ec_fsm_pdo_read_action_next_pdo( |
273 ec_fsm_pdo_t *fsm /**< finite state machine */ |
279 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
280 ec_datagram_t *datagram /**< Datagram to use. */ |
274 ) |
281 ) |
275 { |
282 { |
276 if (fsm->pdo_pos <= fsm->pdo_count) { |
283 if (fsm->pdo_pos <= fsm->pdo_count) { |
277 ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index, |
284 ecrt_sdo_request_index(&fsm->request, 0x1C10 + fsm->sync_index, |
278 fsm->pdo_pos); |
285 fsm->pdo_pos); |
279 ecrt_sdo_request_read(&fsm->request); |
286 ecrt_sdo_request_read(&fsm->request); |
280 fsm->state = ec_fsm_pdo_read_state_pdo; |
287 fsm->state = ec_fsm_pdo_read_state_pdo; |
281 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
288 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
282 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
289 ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately |
283 return; |
290 return; |
284 } |
291 } |
285 |
292 |
286 // finished reading PDO configuration |
293 // finished reading PDO configuration |
287 |
294 |
288 ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos); |
295 ec_pdo_list_copy(&fsm->sync->pdos, &fsm->pdos); |
289 ec_pdo_list_clear_pdos(&fsm->pdos); |
296 ec_pdo_list_clear_pdos(&fsm->pdos); |
290 |
297 |
291 // next sync manager |
298 // next sync manager |
292 ec_fsm_pdo_read_action_next_sync(fsm); |
299 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
293 } |
300 } |
294 |
301 |
295 /*****************************************************************************/ |
302 /*****************************************************************************/ |
296 |
303 |
297 /** Fetch PDO information. |
304 /** Fetch PDO information. |
298 */ |
305 */ |
299 void ec_fsm_pdo_read_state_pdo( |
306 void ec_fsm_pdo_read_state_pdo( |
300 ec_fsm_pdo_t *fsm /**< finite state machine */ |
307 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
301 ) |
308 ec_datagram_t *datagram /**< Datagram to use. */ |
302 { |
309 ) |
303 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
310 { |
|
311 if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { |
|
312 return; |
|
313 } |
304 |
314 |
305 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
315 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
306 EC_SLAVE_ERR(fsm->slave, "Failed to read index of" |
316 EC_SLAVE_ERR(fsm->slave, "Failed to read index of" |
307 " assigned PDO %u from SM%u.\n", |
317 " assigned PDO %u from SM%u.\n", |
308 fsm->pdo_pos, fsm->sync_index); |
318 fsm->pdo_pos, fsm->sync_index); |
309 ec_fsm_pdo_read_action_next_sync(fsm); |
319 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
310 return; |
320 return; |
311 } |
321 } |
312 |
322 |
313 if (fsm->request.data_size != sizeof(uint16_t)) { |
323 if (fsm->request.data_size != sizeof(uint16_t)) { |
314 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" |
324 EC_SLAVE_ERR(fsm->slave, "Invalid data size %zu returned" |
315 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, |
325 " when uploading SDO 0x%04X:%02X.\n", fsm->request.data_size, |
316 fsm->request.index, fsm->request.subindex); |
326 fsm->request.index, fsm->request.subindex); |
317 ec_fsm_pdo_read_action_next_sync(fsm); |
327 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
318 return; |
328 return; |
319 } |
329 } |
320 |
330 |
321 if (!(fsm->pdo = (ec_pdo_t *) |
331 if (!(fsm->pdo = (ec_pdo_t *) |
322 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
332 kmalloc(sizeof(ec_pdo_t), GFP_KERNEL))) { |
323 EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n"); |
333 EC_SLAVE_ERR(fsm->slave, "Failed to allocate PDO.\n"); |
324 ec_fsm_pdo_read_action_next_sync(fsm); |
334 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
325 return; |
335 return; |
326 } |
336 } |
327 |
337 |
328 ec_pdo_init(fsm->pdo); |
338 ec_pdo_init(fsm->pdo); |
329 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
339 fsm->pdo->index = EC_READ_U16(fsm->request.data); |
333 |
343 |
334 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
344 list_add_tail(&fsm->pdo->list, &fsm->pdos.list); |
335 |
345 |
336 fsm->state = ec_fsm_pdo_read_state_pdo_entries; |
346 fsm->state = ec_fsm_pdo_read_state_pdo_entries; |
337 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo); |
347 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, fsm->pdo); |
338 fsm->state(fsm); // execute immediately |
348 fsm->state(fsm, datagram); // execute immediately |
339 } |
349 } |
340 |
350 |
341 /*****************************************************************************/ |
351 /*****************************************************************************/ |
342 |
352 |
343 /** Fetch PDO information. |
353 /** Fetch PDO information. |
344 */ |
354 */ |
345 void ec_fsm_pdo_read_state_pdo_entries( |
355 void ec_fsm_pdo_read_state_pdo_entries( |
346 ec_fsm_pdo_t *fsm /**< finite state machine */ |
356 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
347 ) |
357 ec_datagram_t *datagram /**< Datagram to use. */ |
348 { |
358 ) |
349 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
359 { |
350 return; |
360 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) { |
|
361 return; |
|
362 } |
351 |
363 |
352 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) { |
364 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) { |
353 EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries" |
365 EC_SLAVE_ERR(fsm->slave, "Failed to read mapped PDO entries" |
354 " for PDO 0x%04X.\n", fsm->pdo->index); |
366 " for PDO 0x%04X.\n", fsm->pdo->index); |
355 ec_fsm_pdo_read_action_next_sync(fsm); |
367 ec_fsm_pdo_read_action_next_sync(fsm, datagram); |
356 return; |
368 return; |
357 } |
369 } |
358 |
370 |
359 // next PDO |
371 // next PDO |
360 fsm->pdo_pos++; |
372 fsm->pdo_pos++; |
361 ec_fsm_pdo_read_action_next_pdo(fsm); |
373 ec_fsm_pdo_read_action_next_pdo(fsm, datagram); |
362 } |
374 } |
363 |
375 |
364 /****************************************************************************** |
376 /****************************************************************************** |
365 * Writing state functions. |
377 * Writing state functions. |
366 *****************************************************************************/ |
378 *****************************************************************************/ |
367 |
379 |
368 /** Start PDO configuration. |
380 /** Start PDO configuration. |
369 */ |
381 */ |
370 void ec_fsm_pdo_conf_state_start( |
382 void ec_fsm_pdo_conf_state_start( |
371 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
383 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
384 ec_datagram_t *datagram /**< Datagram to use. */ |
372 ) |
385 ) |
373 { |
386 { |
374 if (!fsm->slave->config) { |
387 if (!fsm->slave->config) { |
375 fsm->state = ec_fsm_pdo_state_end; |
388 fsm->state = ec_fsm_pdo_state_end; |
376 return; |
389 return; |
377 } |
390 } |
378 |
391 |
379 fsm->sync_index = 1; // next is 2 |
392 fsm->sync_index = 1; // next is 2 |
380 ec_fsm_pdo_conf_action_next_sync(fsm); |
393 ec_fsm_pdo_conf_action_next_sync(fsm, datagram); |
381 } |
394 } |
382 |
395 |
383 /*****************************************************************************/ |
396 /*****************************************************************************/ |
384 |
397 |
385 /** Assign next PDO. |
398 /** Assign next PDO. |
467 |
482 |
468 // pdo mapping is unknown; start loading it |
483 // pdo mapping is unknown; start loading it |
469 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, |
484 ec_fsm_pdo_entry_start_reading(&fsm->fsm_pdo_entry, fsm->slave, |
470 &fsm->slave_pdo); |
485 &fsm->slave_pdo); |
471 fsm->state = ec_fsm_pdo_conf_state_read_mapping; |
486 fsm->state = ec_fsm_pdo_conf_state_read_mapping; |
472 fsm->state(fsm); // execute immediately |
487 fsm->state(fsm, datagram); // execute immediately |
473 return; |
488 return; |
474 } |
489 } |
475 |
490 |
476 // pdo mapping is known, check if it most be re-configured |
491 // pdo mapping is known, check if it most be re-configured |
477 ec_fsm_pdo_conf_action_check_mapping(fsm); |
492 ec_fsm_pdo_conf_action_check_mapping(fsm, datagram); |
478 } |
493 } |
479 |
494 |
480 /*****************************************************************************/ |
495 /*****************************************************************************/ |
481 |
496 |
482 /** Execute the PDO entry state machine to read the current PDO's mapping. |
497 /** Execute the PDO entry state machine to read the current PDO's mapping. |
483 */ |
498 */ |
484 void ec_fsm_pdo_conf_state_read_mapping( |
499 void ec_fsm_pdo_conf_state_read_mapping( |
485 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
500 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
486 ) |
501 ec_datagram_t *datagram /**< Datagram to use. */ |
487 { |
502 ) |
488 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
503 { |
489 return; |
504 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) { |
|
505 return; |
|
506 } |
490 |
507 |
491 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
508 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
492 EC_SLAVE_WARN(fsm->slave, |
509 EC_SLAVE_WARN(fsm->slave, |
493 "Failed to read PDO entries for PDO 0x%04X.\n", |
510 "Failed to read PDO entries for PDO 0x%04X.\n", |
494 fsm->pdo->index); |
511 fsm->pdo->index); |
495 |
512 |
496 // check if the mapping must be re-configured |
513 // check if the mapping must be re-configured |
497 ec_fsm_pdo_conf_action_check_mapping(fsm); |
514 ec_fsm_pdo_conf_action_check_mapping(fsm, datagram); |
498 } |
515 } |
499 |
516 |
500 /*****************************************************************************/ |
517 /*****************************************************************************/ |
501 |
518 |
502 /** Check if the mapping has to be re-configured. |
519 /** Check if the mapping has to be re-configured. |
503 * |
520 * |
504 * \todo Display mapping differences. |
521 * \todo Display mapping differences. |
505 */ |
522 */ |
506 void ec_fsm_pdo_conf_action_check_mapping( |
523 void ec_fsm_pdo_conf_action_check_mapping( |
507 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
524 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
525 ec_datagram_t *datagram /**< Datagram to use. */ |
508 ) |
526 ) |
509 { |
527 { |
510 // check, if slave supports PDO configuration |
528 // check, if slave supports PDO configuration |
511 if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
529 if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
512 && fsm->slave->sii.has_general |
530 && fsm->slave->sii.has_general |
528 printk(". Entries to map: "); |
546 printk(". Entries to map: "); |
529 ec_pdo_print_entries(fsm->pdo); |
547 ec_pdo_print_entries(fsm->pdo); |
530 printk("\n"); |
548 printk("\n"); |
531 } |
549 } |
532 |
550 |
533 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
551 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram); |
534 } |
552 } |
535 |
553 |
536 /*****************************************************************************/ |
554 /*****************************************************************************/ |
537 |
555 |
538 /** Let the PDO entry state machine configure the current PDO's mapping. |
556 /** Let the PDO entry state machine configure the current PDO's mapping. |
539 */ |
557 */ |
540 void ec_fsm_pdo_conf_state_mapping( |
558 void ec_fsm_pdo_conf_state_mapping( |
541 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
559 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
542 ) |
560 ec_datagram_t *datagram /**< Datagram to use. */ |
543 { |
561 ) |
544 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry)) |
562 { |
545 return; |
563 if (ec_fsm_pdo_entry_exec(&fsm->fsm_pdo_entry, datagram)) { |
|
564 return; |
|
565 } |
546 |
566 |
547 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
567 if (!ec_fsm_pdo_entry_success(&fsm->fsm_pdo_entry)) |
548 EC_SLAVE_WARN(fsm->slave, |
568 EC_SLAVE_WARN(fsm->slave, |
549 "Failed to configure mapping of PDO 0x%04X.\n", |
569 "Failed to configure mapping of PDO 0x%04X.\n", |
550 fsm->pdo->index); |
570 fsm->pdo->index); |
551 |
571 |
552 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm); |
572 ec_fsm_pdo_conf_action_next_pdo_mapping(fsm, datagram); |
553 } |
573 } |
554 |
574 |
555 /*****************************************************************************/ |
575 /*****************************************************************************/ |
556 |
576 |
557 /** Check mapping of next PDO, otherwise configure assignment. |
577 /** Check mapping of next PDO, otherwise configure assignment. |
558 */ |
578 */ |
559 void ec_fsm_pdo_conf_action_next_pdo_mapping( |
579 void ec_fsm_pdo_conf_action_next_pdo_mapping( |
560 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
580 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
581 ec_datagram_t *datagram /**< Datagram to use. */ |
561 ) |
582 ) |
562 { |
583 { |
563 // get next configured PDO |
584 // get next configured PDO |
564 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
585 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdo->list))) { |
565 // no more configured pdos |
586 // no more configured pdos |
566 ec_fsm_pdo_conf_action_check_assignment(fsm); |
587 ec_fsm_pdo_conf_action_check_assignment(fsm, datagram); |
567 return; |
588 return; |
568 } |
589 } |
569 |
590 |
570 ec_fsm_pdo_conf_action_pdo_mapping(fsm); |
591 ec_fsm_pdo_conf_action_pdo_mapping(fsm, datagram); |
571 } |
592 } |
572 |
593 |
573 /*****************************************************************************/ |
594 /*****************************************************************************/ |
574 |
595 |
575 /** Check if the PDO assignment of the current SM has to be re-configured. |
596 /** Check if the PDO assignment of the current SM has to be re-configured. |
576 */ |
597 */ |
577 void ec_fsm_pdo_conf_action_check_assignment( |
598 void ec_fsm_pdo_conf_action_check_assignment( |
578 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
599 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
600 ec_datagram_t *datagram /**< Datagram to use. */ |
579 ) |
601 ) |
580 { |
602 { |
581 if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
603 if ((fsm->slave->sii.mailbox_protocols & EC_MBOX_COE) |
582 && fsm->slave->sii.has_general |
604 && fsm->slave->sii.has_general |
583 && fsm->slave->sii.coe_details.enable_pdo_assign) { |
605 && fsm->slave->sii.coe_details.enable_pdo_assign) { |
603 EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned" |
625 EC_SLAVE_DBG(fsm->slave, 1, "Setting number of assigned" |
604 " PDOs to zero.\n"); |
626 " PDOs to zero.\n"); |
605 |
627 |
606 fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count; |
628 fsm->state = ec_fsm_pdo_conf_state_zero_pdo_count; |
607 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
629 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
608 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
630 ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately |
609 return; |
631 return; |
610 } |
632 } |
611 else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) { |
633 else if (!ec_pdo_list_equal(&fsm->sync->pdos, &fsm->pdos)) { |
612 EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n"); |
634 EC_SLAVE_WARN(fsm->slave, "Slave does not support assigning PDOs!\n"); |
613 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); |
635 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); |
614 } |
636 } |
615 |
637 |
616 ec_fsm_pdo_conf_action_next_sync(fsm); |
638 ec_fsm_pdo_conf_action_next_sync(fsm, datagram); |
617 } |
639 } |
618 |
640 |
619 /*****************************************************************************/ |
641 /*****************************************************************************/ |
620 |
642 |
621 /** Set the number of assigned PDOs to zero. |
643 /** Set the number of assigned PDOs to zero. |
622 */ |
644 */ |
623 void ec_fsm_pdo_conf_state_zero_pdo_count( |
645 void ec_fsm_pdo_conf_state_zero_pdo_count( |
624 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
646 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
625 ) |
647 ec_datagram_t *datagram /**< Datagram to use. */ |
626 { |
648 ) |
627 if (ec_fsm_coe_exec(fsm->fsm_coe)) |
649 { |
628 return; |
650 if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { |
|
651 return; |
|
652 } |
629 |
653 |
630 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
654 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
631 EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n", |
655 EC_SLAVE_WARN(fsm->slave, "Failed to clear PDO assignment of SM%u.\n", |
632 fsm->sync_index); |
656 fsm->sync_index); |
633 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); |
657 EC_SLAVE_WARN(fsm->slave, ""); |
634 ec_fsm_pdo_conf_action_next_sync(fsm); |
658 ec_fsm_pdo_print(fsm); |
|
659 ec_fsm_pdo_conf_action_next_sync(fsm, datagram); |
635 return; |
660 return; |
636 } |
661 } |
637 |
662 |
638 // the sync manager's assigned PDOs have been cleared |
663 // the sync manager's assigned PDOs have been cleared |
639 ec_pdo_list_clear_pdos(&fsm->sync->pdos); |
664 ec_pdo_list_clear_pdos(&fsm->sync->pdos); |
640 |
665 |
641 // assign all PDOs belonging to the current sync manager |
666 // assign all PDOs belonging to the current sync manager |
642 |
667 |
643 // find first PDO |
668 // find first PDO |
644 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) { |
669 if (!(fsm->pdo = ec_fsm_pdo_conf_action_next_pdo(fsm, &fsm->pdos.list))) { |
645 |
|
646 // check for mapping to be altered |
670 // check for mapping to be altered |
647 ec_fsm_pdo_conf_action_next_sync(fsm); |
671 ec_fsm_pdo_conf_action_next_sync(fsm, datagram); |
648 return; |
672 return; |
649 } |
673 } |
650 |
674 |
651 // assign first PDO |
675 // assign first PDO |
652 fsm->pdo_pos = 1; |
676 fsm->pdo_pos = 1; |
653 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
677 ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram); |
654 } |
678 } |
655 |
679 |
656 /*****************************************************************************/ |
680 /*****************************************************************************/ |
657 |
681 |
658 /** Assign a PDO. |
682 /** Assign a PDO. |
659 */ |
683 */ |
660 void ec_fsm_pdo_conf_action_assign_pdo( |
684 void ec_fsm_pdo_conf_action_assign_pdo( |
661 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
685 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
686 ec_datagram_t *datagram /**< Datagram to use. */ |
662 ) |
687 ) |
663 { |
688 { |
664 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
689 EC_WRITE_U16(fsm->request.data, fsm->pdo->index); |
665 fsm->request.data_size = 2; |
690 fsm->request.data_size = 2; |
666 ecrt_sdo_request_index(&fsm->request, |
691 ecrt_sdo_request_index(&fsm->request, |
670 EC_SLAVE_DBG(fsm->slave, 1, "Assigning PDO 0x%04X at position %u.\n", |
695 EC_SLAVE_DBG(fsm->slave, 1, "Assigning PDO 0x%04X at position %u.\n", |
671 fsm->pdo->index, fsm->pdo_pos); |
696 fsm->pdo->index, fsm->pdo_pos); |
672 |
697 |
673 fsm->state = ec_fsm_pdo_conf_state_assign_pdo; |
698 fsm->state = ec_fsm_pdo_conf_state_assign_pdo; |
674 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
699 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
675 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
700 ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately |
676 } |
701 } |
677 |
702 |
678 /*****************************************************************************/ |
703 /*****************************************************************************/ |
679 |
704 |
680 /** Add a PDO to the sync managers PDO assignment. |
705 /** Add a PDO to the sync managers PDO assignment. |
681 */ |
706 */ |
682 void ec_fsm_pdo_conf_state_assign_pdo( |
707 void ec_fsm_pdo_conf_state_assign_pdo( |
683 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
708 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
684 ) |
709 ec_datagram_t *datagram /**< Datagram to use. */ |
685 { |
710 ) |
686 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
711 { |
|
712 if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { |
|
713 return; |
|
714 } |
687 |
715 |
688 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
716 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
689 EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u" |
717 EC_SLAVE_WARN(fsm->slave, "Failed to assign PDO 0x%04X at position %u" |
690 " of SM%u.\n", |
718 " of SM%u.\n", |
691 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index); |
719 fsm->pdo->index, fsm->pdo_pos, fsm->sync_index); |
707 "Setting number of assigned PDOs to %u.\n", |
734 "Setting number of assigned PDOs to %u.\n", |
708 fsm->pdo_pos); |
735 fsm->pdo_pos); |
709 |
736 |
710 fsm->state = ec_fsm_pdo_conf_state_set_pdo_count; |
737 fsm->state = ec_fsm_pdo_conf_state_set_pdo_count; |
711 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
738 ec_fsm_coe_transfer(fsm->fsm_coe, fsm->slave, &fsm->request); |
712 ec_fsm_coe_exec(fsm->fsm_coe); // execute immediately |
739 ec_fsm_coe_exec(fsm->fsm_coe, datagram); // execute immediately |
713 return; |
740 return; |
714 } |
741 } |
715 |
742 |
716 // add next PDO to assignment |
743 // add next PDO to assignment |
717 fsm->pdo_pos++; |
744 fsm->pdo_pos++; |
718 ec_fsm_pdo_conf_action_assign_pdo(fsm); |
745 ec_fsm_pdo_conf_action_assign_pdo(fsm, datagram); |
719 } |
746 } |
720 |
747 |
721 /*****************************************************************************/ |
748 /*****************************************************************************/ |
722 |
749 |
723 /** Set the number of assigned PDOs. |
750 /** Set the number of assigned PDOs. |
724 */ |
751 */ |
725 void ec_fsm_pdo_conf_state_set_pdo_count( |
752 void ec_fsm_pdo_conf_state_set_pdo_count( |
726 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
753 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
727 ) |
754 ec_datagram_t *datagram /**< Datagram to use. */ |
728 { |
755 ) |
729 if (ec_fsm_coe_exec(fsm->fsm_coe)) return; |
756 { |
|
757 if (ec_fsm_coe_exec(fsm->fsm_coe, datagram)) { |
|
758 return; |
|
759 } |
730 |
760 |
731 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
761 if (!ec_fsm_coe_success(fsm->fsm_coe)) { |
732 EC_SLAVE_WARN(fsm->slave, "Failed to set number of" |
762 EC_SLAVE_WARN(fsm->slave, "Failed to set number of" |
733 " assigned PDOs of SM%u.\n", fsm->sync_index); |
763 " assigned PDOs of SM%u.\n", fsm->sync_index); |
734 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); |
764 EC_SLAVE_WARN(fsm->slave, ""); ec_fsm_pdo_print(fsm); |
741 |
771 |
742 EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured" |
772 EC_SLAVE_DBG(fsm->slave, 1, "Successfully configured" |
743 " PDO assignment of SM%u.\n", fsm->sync_index); |
773 " PDO assignment of SM%u.\n", fsm->sync_index); |
744 |
774 |
745 // check if PDO mapping has to be altered |
775 // check if PDO mapping has to be altered |
746 ec_fsm_pdo_conf_action_next_sync(fsm); |
776 ec_fsm_pdo_conf_action_next_sync(fsm, datagram); |
747 } |
777 } |
748 |
778 |
749 /****************************************************************************** |
779 /****************************************************************************** |
750 * Common state functions |
780 * Common state functions |
751 *****************************************************************************/ |
781 *****************************************************************************/ |
752 |
782 |
753 /** State: ERROR. |
783 /** State: ERROR. |
754 */ |
784 */ |
755 void ec_fsm_pdo_state_error( |
785 void ec_fsm_pdo_state_error( |
756 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
786 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
|
787 ec_datagram_t *datagram /**< Datagram to use. */ |
757 ) |
788 ) |
758 { |
789 { |
759 } |
790 } |
760 |
791 |
761 /*****************************************************************************/ |
792 /*****************************************************************************/ |
762 |
793 |
763 /** State: END. |
794 /** State: END. |
764 */ |
795 */ |
765 void ec_fsm_pdo_state_end( |
796 void ec_fsm_pdo_state_end( |
766 ec_fsm_pdo_t *fsm /**< PDO configuration state machine. */ |
797 ec_fsm_pdo_t *fsm, /**< Finite state machine. */ |
767 ) |
798 ec_datagram_t *datagram /**< Datagram to use. */ |
768 { |
799 ) |
769 } |
800 { |
770 |
801 } |
771 /*****************************************************************************/ |
802 |
|
803 /*****************************************************************************/ |