259 EC_WRITE_U8 (data + 3, 0x00); |
259 EC_WRITE_U8 (data + 3, 0x00); |
260 EC_WRITE_U16(data + 4, 0x0000); |
260 EC_WRITE_U16(data + 4, 0x0000); |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
261 EC_WRITE_U16(data + 6, 0x0001); // deliver all SDOs! |
262 |
262 |
263 ec_master_queue_datagram(fsm->slave->master, datagram); |
263 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
264 fsm->retries = EC_FSM_RETRIES; |
264 fsm->state = ec_fsm_coe_dict_request; |
265 fsm->state = ec_fsm_coe_dict_request; |
265 } |
266 } |
266 |
267 |
267 /*****************************************************************************/ |
268 /*****************************************************************************/ |
268 |
269 |
269 /** |
270 /** |
270 CoE state: DICT REQUEST. |
271 CoE state: DICT REQUEST. |
|
272 \todo Timeout behavior |
271 */ |
273 */ |
272 |
274 |
273 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
275 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
274 { |
276 { |
275 ec_datagram_t *datagram = fsm->datagram; |
277 ec_datagram_t *datagram = fsm->datagram; |
276 ec_slave_t *slave = fsm->slave; |
278 ec_slave_t *slave = fsm->slave; |
277 |
279 |
278 if (datagram->state != EC_DATAGRAM_RECEIVED |
280 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
279 || datagram->working_counter != 1) { |
281 // FIXME: request again? |
280 fsm->state = ec_fsm_coe_error; |
282 ec_master_queue_datagram(fsm->slave->master, datagram); |
281 EC_ERR("Reception of CoE dictionary request failed on slave %i.\n", |
283 return; |
282 slave->ring_position); |
284 } |
|
285 |
|
286 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
287 fsm->state = ec_fsm_coe_error; |
|
288 EC_ERR("Failed to receive CoE dictionary request datagram for" |
|
289 " slave %i.\n", slave->ring_position); |
|
290 return; |
|
291 } |
|
292 |
|
293 if (datagram->working_counter != 1) { |
|
294 fsm->state = ec_fsm_coe_error; |
|
295 EC_ERR("Reception of CoE dictionary request failed - slave %i did" |
|
296 " not respond.\n", slave->ring_position); |
283 return; |
297 return; |
284 } |
298 } |
285 |
299 |
286 fsm->cycles_start = datagram->cycles_sent; |
300 fsm->cycles_start = datagram->cycles_sent; |
287 |
301 |
288 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
302 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
289 ec_master_queue_datagram(fsm->slave->master, datagram); |
303 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
304 fsm->retries = EC_FSM_RETRIES; |
290 fsm->state = ec_fsm_coe_dict_check; |
305 fsm->state = ec_fsm_coe_dict_check; |
291 } |
306 } |
292 |
307 |
293 /*****************************************************************************/ |
308 /*****************************************************************************/ |
294 |
309 |
299 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
314 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
300 { |
315 { |
301 ec_datagram_t *datagram = fsm->datagram; |
316 ec_datagram_t *datagram = fsm->datagram; |
302 ec_slave_t *slave = fsm->slave; |
317 ec_slave_t *slave = fsm->slave; |
303 |
318 |
304 if (datagram->state != EC_DATAGRAM_RECEIVED |
319 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
305 || datagram->working_counter != 1) { |
320 ec_master_queue_datagram(fsm->slave->master, datagram); |
306 fsm->state = ec_fsm_coe_error; |
321 return; |
307 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n", |
322 } |
|
323 |
|
324 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
325 fsm->state = ec_fsm_coe_error; |
|
326 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
|
327 slave->ring_position); |
|
328 return; |
|
329 } |
|
330 |
|
331 if (datagram->working_counter != 1) { |
|
332 fsm->state = ec_fsm_coe_error; |
|
333 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
334 " not respond.\n", |
308 slave->ring_position); |
335 slave->ring_position); |
309 return; |
336 return; |
310 } |
337 } |
311 |
338 |
312 if (!ec_slave_mbox_check(datagram)) { |
339 if (!ec_slave_mbox_check(datagram)) { |
318 return; |
345 return; |
319 } |
346 } |
320 |
347 |
321 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
348 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
322 ec_master_queue_datagram(fsm->slave->master, datagram); |
349 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
350 fsm->retries = EC_FSM_RETRIES; |
323 return; |
351 return; |
324 } |
352 } |
325 |
353 |
326 // Fetch response |
354 // Fetch response |
327 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
355 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
328 ec_master_queue_datagram(fsm->slave->master, datagram); |
356 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
357 fsm->retries = EC_FSM_RETRIES; |
329 fsm->state = ec_fsm_coe_dict_response; |
358 fsm->state = ec_fsm_coe_dict_response; |
330 } |
359 } |
331 |
360 |
332 /*****************************************************************************/ |
361 /*****************************************************************************/ |
333 |
362 |
334 /** |
363 /** |
335 CoE state: DICT RESPONSE. |
364 CoE state: DICT RESPONSE. |
|
365 \todo Timeout behavior |
336 */ |
366 */ |
337 |
367 |
338 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
368 void ec_fsm_coe_dict_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
339 { |
369 { |
340 ec_datagram_t *datagram = fsm->datagram; |
370 ec_datagram_t *datagram = fsm->datagram; |
343 size_t rec_size; |
373 size_t rec_size; |
344 unsigned int sdo_count, i; |
374 unsigned int sdo_count, i; |
345 uint16_t sdo_index; |
375 uint16_t sdo_index; |
346 ec_sdo_t *sdo; |
376 ec_sdo_t *sdo; |
347 |
377 |
348 if (datagram->state != EC_DATAGRAM_RECEIVED |
378 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
349 || datagram->working_counter != 1) { |
379 // FIXME: request again? |
350 fsm->state = ec_fsm_coe_error; |
380 ec_master_queue_datagram(fsm->slave->master, datagram); |
351 EC_ERR("Reception of CoE dictionary response failed on slave %i.\n", |
381 return; |
352 slave->ring_position); |
382 } |
|
383 |
|
384 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
385 fsm->state = ec_fsm_coe_error; |
|
386 EC_ERR("Failed to receive CoE dictionary response datagram for" |
|
387 " slave %i.\n", slave->ring_position); |
|
388 return; |
|
389 } |
|
390 |
|
391 if (datagram->working_counter != 1) { |
|
392 fsm->state = ec_fsm_coe_error; |
|
393 EC_ERR("Reception of CoE dictionary response failed - slave %i did" |
|
394 " not respond.\n", slave->ring_position); |
353 return; |
395 return; |
354 } |
396 } |
355 |
397 |
356 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
398 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
357 &mbox_prot, &rec_size))) { |
399 &mbox_prot, &rec_size))) { |
418 |
460 |
419 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
461 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
420 fsm->cycles_start = datagram->cycles_sent; |
462 fsm->cycles_start = datagram->cycles_sent; |
421 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
463 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
422 ec_master_queue_datagram(fsm->slave->master, datagram); |
464 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
465 fsm->retries = EC_FSM_RETRIES; |
423 fsm->state = ec_fsm_coe_dict_check; |
466 fsm->state = ec_fsm_coe_dict_check; |
424 return; |
467 return; |
425 } |
468 } |
426 |
469 |
427 if (list_empty(&slave->sdo_dictionary)) { |
470 if (list_empty(&slave->sdo_dictionary)) { |
443 EC_WRITE_U8 (data + 3, 0x00); |
486 EC_WRITE_U8 (data + 3, 0x00); |
444 EC_WRITE_U16(data + 4, 0x0000); |
487 EC_WRITE_U16(data + 4, 0x0000); |
445 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
488 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
446 |
489 |
447 ec_master_queue_datagram(fsm->slave->master, datagram); |
490 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
491 fsm->retries = EC_FSM_RETRIES; |
448 fsm->state = ec_fsm_coe_dict_desc_request; |
492 fsm->state = ec_fsm_coe_dict_desc_request; |
449 } |
493 } |
450 |
494 |
451 /*****************************************************************************/ |
495 /*****************************************************************************/ |
452 |
496 |
453 /** |
497 /** |
454 CoE state: DICT DESC REQUEST. |
498 CoE state: DICT DESC REQUEST. |
|
499 \todo Timeout behavior |
455 */ |
500 */ |
456 |
501 |
457 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
502 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
458 { |
503 { |
459 ec_datagram_t *datagram = fsm->datagram; |
504 ec_datagram_t *datagram = fsm->datagram; |
460 ec_slave_t *slave = fsm->slave; |
505 ec_slave_t *slave = fsm->slave; |
461 |
506 |
462 if (datagram->state != EC_DATAGRAM_RECEIVED |
507 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
463 || datagram->working_counter != 1) { |
508 // FIXME: check for response first? |
464 fsm->state = ec_fsm_coe_error; |
509 ec_master_queue_datagram(fsm->slave->master, datagram); |
465 EC_ERR("Reception of CoE SDO description" |
510 return; |
466 " request failed on slave %i.\n", slave->ring_position); |
511 } |
|
512 |
|
513 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
514 fsm->state = ec_fsm_coe_error; |
|
515 EC_ERR("Failed to receive CoE SDO description request datagram for" |
|
516 " slave %i.\n", slave->ring_position); |
|
517 return; |
|
518 } |
|
519 |
|
520 if (datagram->working_counter != 1) { |
|
521 fsm->state = ec_fsm_coe_error; |
|
522 EC_ERR("Reception of CoE SDO description request failed - slave %i did" |
|
523 " not respond.\n", slave->ring_position); |
467 return; |
524 return; |
468 } |
525 } |
469 |
526 |
470 fsm->cycles_start = datagram->cycles_sent; |
527 fsm->cycles_start = datagram->cycles_sent; |
471 |
528 |
472 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
529 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
473 ec_master_queue_datagram(fsm->slave->master, datagram); |
530 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
531 fsm->retries = EC_FSM_RETRIES; |
474 fsm->state = ec_fsm_coe_dict_desc_check; |
532 fsm->state = ec_fsm_coe_dict_desc_check; |
475 } |
533 } |
476 |
534 |
477 /*****************************************************************************/ |
535 /*****************************************************************************/ |
478 |
536 |
483 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
541 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
484 { |
542 { |
485 ec_datagram_t *datagram = fsm->datagram; |
543 ec_datagram_t *datagram = fsm->datagram; |
486 ec_slave_t *slave = fsm->slave; |
544 ec_slave_t *slave = fsm->slave; |
487 |
545 |
488 if (datagram->state != EC_DATAGRAM_RECEIVED |
546 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
489 || datagram->working_counter != 1) { |
547 ec_master_queue_datagram(fsm->slave->master, datagram); |
490 fsm->state = ec_fsm_coe_error; |
548 return; |
491 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n", |
549 } |
|
550 |
|
551 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
552 fsm->state = ec_fsm_coe_error; |
|
553 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
492 slave->ring_position); |
554 slave->ring_position); |
|
555 return; |
|
556 } |
|
557 |
|
558 if (datagram->working_counter != 1) { |
|
559 fsm->state = ec_fsm_coe_error; |
|
560 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
561 " not respond.\n", slave->ring_position); |
493 return; |
562 return; |
494 } |
563 } |
495 |
564 |
496 if (!ec_slave_mbox_check(datagram)) { |
565 if (!ec_slave_mbox_check(datagram)) { |
497 if (datagram->cycles_received |
566 if (datagram->cycles_received |
502 return; |
571 return; |
503 } |
572 } |
504 |
573 |
505 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
574 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
506 ec_master_queue_datagram(fsm->slave->master, datagram); |
575 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
576 fsm->retries = EC_FSM_RETRIES; |
507 return; |
577 return; |
508 } |
578 } |
509 |
579 |
510 // Fetch response |
580 // Fetch response |
511 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
581 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
512 ec_master_queue_datagram(fsm->slave->master, datagram); |
582 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
583 fsm->retries = EC_FSM_RETRIES; |
513 fsm->state = ec_fsm_coe_dict_desc_response; |
584 fsm->state = ec_fsm_coe_dict_desc_response; |
514 } |
585 } |
515 |
586 |
516 /*****************************************************************************/ |
587 /*****************************************************************************/ |
517 |
588 |
518 /** |
589 /** |
519 CoE state: DICT DESC RESPONSE. |
590 CoE state: DICT DESC RESPONSE. |
|
591 \todo Timeout behavior |
520 */ |
592 */ |
521 |
593 |
522 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm |
594 void ec_fsm_coe_dict_desc_response(ec_fsm_coe_t *fsm |
523 /**< finite state machine */) |
595 /**< finite state machine */) |
524 { |
596 { |
526 ec_slave_t *slave = fsm->slave; |
598 ec_slave_t *slave = fsm->slave; |
527 ec_sdo_t *sdo = fsm->sdo; |
599 ec_sdo_t *sdo = fsm->sdo; |
528 uint8_t *data, mbox_prot; |
600 uint8_t *data, mbox_prot; |
529 size_t rec_size, name_size; |
601 size_t rec_size, name_size; |
530 |
602 |
531 if (datagram->state != EC_DATAGRAM_RECEIVED |
603 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
532 || datagram->working_counter != 1) { |
604 // FIXME: request again? |
533 fsm->state = ec_fsm_coe_error; |
605 ec_master_queue_datagram(fsm->slave->master, datagram); |
534 EC_ERR("Reception of CoE SDO description" |
606 return; |
535 "response failed on slave %i.\n", slave->ring_position); |
607 } |
|
608 |
|
609 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
610 fsm->state = ec_fsm_coe_error; |
|
611 EC_ERR("Failed to receive CoE SDO description response datagram from" |
|
612 " slave %i.\n", slave->ring_position); |
|
613 return; |
|
614 } |
|
615 |
|
616 if (datagram->working_counter != 1) { |
|
617 fsm->state = ec_fsm_coe_error; |
|
618 EC_ERR("Reception of CoE SDO description response failed - slave %i" |
|
619 " did not respond.\n", slave->ring_position); |
536 return; |
620 return; |
537 } |
621 } |
538 |
622 |
539 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
623 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
540 &mbox_prot, &rec_size))) { |
624 &mbox_prot, &rec_size))) { |
613 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
697 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
614 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
698 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
615 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
699 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
616 |
700 |
617 ec_master_queue_datagram(fsm->slave->master, datagram); |
701 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
702 fsm->retries = EC_FSM_RETRIES; |
618 fsm->state = ec_fsm_coe_dict_entry_request; |
703 fsm->state = ec_fsm_coe_dict_entry_request; |
619 } |
704 } |
620 |
705 |
621 /*****************************************************************************/ |
706 /*****************************************************************************/ |
622 |
707 |
623 /** |
708 /** |
624 CoE state: DICT ENTRY REQUEST. |
709 CoE state: DICT ENTRY REQUEST. |
|
710 \todo Timeout behavior |
625 */ |
711 */ |
626 |
712 |
627 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm |
713 void ec_fsm_coe_dict_entry_request(ec_fsm_coe_t *fsm |
628 /**< finite state machine */) |
714 /**< finite state machine */) |
629 { |
715 { |
630 ec_datagram_t *datagram = fsm->datagram; |
716 ec_datagram_t *datagram = fsm->datagram; |
631 ec_slave_t *slave = fsm->slave; |
717 ec_slave_t *slave = fsm->slave; |
632 |
718 |
633 if (datagram->state != EC_DATAGRAM_RECEIVED |
719 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
634 || datagram->working_counter != 1) { |
720 // FIXME: check for response first? |
635 fsm->state = ec_fsm_coe_error; |
721 ec_master_queue_datagram(fsm->slave->master, datagram); |
636 EC_ERR("Reception of CoE SDO entry request failed on slave %i.\n", |
722 return; |
637 slave->ring_position); |
723 } |
|
724 |
|
725 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
726 fsm->state = ec_fsm_coe_error; |
|
727 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
|
728 " slave %i.\n", slave->ring_position); |
|
729 return; |
|
730 } |
|
731 |
|
732 if (datagram->working_counter != 1) { |
|
733 fsm->state = ec_fsm_coe_error; |
|
734 EC_ERR("Reception of CoE SDO entry request failed - slave %i did" |
|
735 " not respond.\n", slave->ring_position); |
638 return; |
736 return; |
639 } |
737 } |
640 |
738 |
641 fsm->cycles_start = datagram->cycles_sent; |
739 fsm->cycles_start = datagram->cycles_sent; |
642 |
740 |
643 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
741 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
644 ec_master_queue_datagram(fsm->slave->master, datagram); |
742 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
743 fsm->retries = EC_FSM_RETRIES; |
645 fsm->state = ec_fsm_coe_dict_entry_check; |
744 fsm->state = ec_fsm_coe_dict_entry_check; |
646 } |
745 } |
647 |
746 |
648 /*****************************************************************************/ |
747 /*****************************************************************************/ |
649 |
748 |
655 /**< finite state machine */) |
754 /**< finite state machine */) |
656 { |
755 { |
657 ec_datagram_t *datagram = fsm->datagram; |
756 ec_datagram_t *datagram = fsm->datagram; |
658 ec_slave_t *slave = fsm->slave; |
757 ec_slave_t *slave = fsm->slave; |
659 |
758 |
660 if (datagram->state != EC_DATAGRAM_RECEIVED |
759 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
661 || datagram->working_counter != 1) { |
760 ec_master_queue_datagram(fsm->slave->master, datagram); |
662 fsm->state = ec_fsm_coe_error; |
761 return; |
663 EC_ERR("Reception of CoE mailbox check datagram failed on slave %i.\n", |
762 } |
|
763 |
|
764 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
765 fsm->state = ec_fsm_coe_error; |
|
766 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
664 slave->ring_position); |
767 slave->ring_position); |
|
768 return; |
|
769 } |
|
770 |
|
771 if (datagram->working_counter != 1) { |
|
772 fsm->state = ec_fsm_coe_error; |
|
773 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
774 " not respond.\n", slave->ring_position); |
665 return; |
775 return; |
666 } |
776 } |
667 |
777 |
668 if (!ec_slave_mbox_check(datagram)) { |
778 if (!ec_slave_mbox_check(datagram)) { |
669 if (datagram->cycles_received |
779 if (datagram->cycles_received |
674 return; |
784 return; |
675 } |
785 } |
676 |
786 |
677 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
787 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
678 ec_master_queue_datagram(fsm->slave->master, datagram); |
788 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
789 fsm->retries = EC_FSM_RETRIES; |
679 return; |
790 return; |
680 } |
791 } |
681 |
792 |
682 // Fetch response |
793 // Fetch response |
683 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
794 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
684 ec_master_queue_datagram(fsm->slave->master, datagram); |
795 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
796 fsm->retries = EC_FSM_RETRIES; |
685 fsm->state = ec_fsm_coe_dict_entry_response; |
797 fsm->state = ec_fsm_coe_dict_entry_response; |
686 } |
798 } |
687 |
799 |
688 /*****************************************************************************/ |
800 /*****************************************************************************/ |
689 |
801 |
690 /** |
802 /** |
691 CoE state: DICT ENTRY RESPONSE. |
803 CoE state: DICT ENTRY RESPONSE. |
|
804 \todo Timeout behavior |
692 */ |
805 */ |
693 |
806 |
694 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm |
807 void ec_fsm_coe_dict_entry_response(ec_fsm_coe_t *fsm |
695 /**< finite state machine */) |
808 /**< finite state machine */) |
696 { |
809 { |
699 ec_sdo_t *sdo = fsm->sdo; |
812 ec_sdo_t *sdo = fsm->sdo; |
700 uint8_t *data, mbox_prot; |
813 uint8_t *data, mbox_prot; |
701 size_t rec_size, data_size; |
814 size_t rec_size, data_size; |
702 ec_sdo_entry_t *entry; |
815 ec_sdo_entry_t *entry; |
703 |
816 |
704 if (datagram->state != EC_DATAGRAM_RECEIVED |
817 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
705 || datagram->working_counter != 1) { |
818 // FIXME: request again? |
706 fsm->state = ec_fsm_coe_error; |
819 ec_master_queue_datagram(fsm->slave->master, datagram); |
707 EC_ERR("Reception of CoE SDO description" |
820 return; |
708 " response failed on slave %i.\n", slave->ring_position); |
821 } |
|
822 |
|
823 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
824 fsm->state = ec_fsm_coe_error; |
|
825 EC_ERR("Failed to receive CoE SDO description response datagram from" |
|
826 " slave %i.\n", slave->ring_position); |
|
827 return; |
|
828 } |
|
829 |
|
830 if (datagram->working_counter != 1) { |
|
831 fsm->state = ec_fsm_coe_error; |
|
832 EC_ERR("Reception of CoE SDO description response failed - slave %i" |
|
833 " did not respond.\n", slave->ring_position); |
709 return; |
834 return; |
710 } |
835 } |
711 |
836 |
712 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
837 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
713 &mbox_prot, &rec_size))) { |
838 &mbox_prot, &rec_size))) { |
797 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
922 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
798 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
923 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
799 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
924 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
800 |
925 |
801 ec_master_queue_datagram(fsm->slave->master, datagram); |
926 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
927 fsm->retries = EC_FSM_RETRIES; |
802 fsm->state = ec_fsm_coe_dict_entry_request; |
928 fsm->state = ec_fsm_coe_dict_entry_request; |
803 return; |
929 return; |
804 } |
930 } |
805 |
931 |
806 // another SDO description to fetch? |
932 // another SDO description to fetch? |
817 EC_WRITE_U8 (data + 3, 0x00); |
943 EC_WRITE_U8 (data + 3, 0x00); |
818 EC_WRITE_U16(data + 4, 0x0000); |
944 EC_WRITE_U16(data + 4, 0x0000); |
819 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
945 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
820 |
946 |
821 ec_master_queue_datagram(fsm->slave->master, datagram); |
947 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
948 fsm->retries = EC_FSM_RETRIES; |
822 fsm->state = ec_fsm_coe_dict_desc_request; |
949 fsm->state = ec_fsm_coe_dict_desc_request; |
823 return; |
950 return; |
824 } |
951 } |
825 |
952 |
826 fsm->state = ec_fsm_coe_end; |
953 fsm->state = ec_fsm_coe_end; |
864 EC_WRITE_U8 (data + 5, sdodata->subindex); |
991 EC_WRITE_U8 (data + 5, sdodata->subindex); |
865 EC_WRITE_U32(data + 6, sdodata->size); |
992 EC_WRITE_U32(data + 6, sdodata->size); |
866 memcpy(data + 10, sdodata->data, sdodata->size); |
993 memcpy(data + 10, sdodata->data, sdodata->size); |
867 |
994 |
868 ec_master_queue_datagram(fsm->slave->master, datagram); |
995 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
996 fsm->retries = EC_FSM_RETRIES; |
869 fsm->state = ec_fsm_coe_down_request; |
997 fsm->state = ec_fsm_coe_down_request; |
870 } |
998 } |
871 |
999 |
872 /*****************************************************************************/ |
1000 /*****************************************************************************/ |
873 |
1001 |
874 /** |
1002 /** |
875 CoE state: DOWN REQUEST. |
1003 CoE state: DOWN REQUEST. |
|
1004 \todo Timeout behavior |
876 */ |
1005 */ |
877 |
1006 |
878 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1007 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
879 { |
1008 { |
880 ec_datagram_t *datagram = fsm->datagram; |
1009 ec_datagram_t *datagram = fsm->datagram; |
881 ec_slave_t *slave = fsm->slave; |
1010 ec_slave_t *slave = fsm->slave; |
882 |
1011 |
883 if (datagram->state != EC_DATAGRAM_RECEIVED |
1012 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
884 || datagram->working_counter != 1) { |
1013 // FIXME: check for response first? |
885 fsm->state = ec_fsm_coe_error; |
1014 ec_master_queue_datagram(fsm->slave->master, datagram); |
886 EC_ERR("Reception of CoE download request failed.\n"); |
1015 return; |
|
1016 } |
|
1017 |
|
1018 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1019 fsm->state = ec_fsm_coe_error; |
|
1020 EC_ERR("Failed to receive CoE download request datagram for" |
|
1021 " slave %i.\n", slave->ring_position); |
|
1022 return; |
|
1023 } |
|
1024 |
|
1025 if (datagram->working_counter != 1) { |
|
1026 fsm->state = ec_fsm_coe_error; |
|
1027 EC_ERR("Reception of CoE download request failed - slave %i did not" |
|
1028 " respond.\n", slave->ring_position); |
887 return; |
1029 return; |
888 } |
1030 } |
889 |
1031 |
890 fsm->cycles_start = datagram->cycles_sent; |
1032 fsm->cycles_start = datagram->cycles_sent; |
891 |
1033 |
892 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1034 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
893 ec_master_queue_datagram(fsm->slave->master, datagram); |
1035 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1036 fsm->retries = EC_FSM_RETRIES; |
894 fsm->state = ec_fsm_coe_down_check; |
1037 fsm->state = ec_fsm_coe_down_check; |
895 } |
1038 } |
896 |
1039 |
897 /*****************************************************************************/ |
1040 /*****************************************************************************/ |
898 |
1041 |
903 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1046 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
904 { |
1047 { |
905 ec_datagram_t *datagram = fsm->datagram; |
1048 ec_datagram_t *datagram = fsm->datagram; |
906 ec_slave_t *slave = fsm->slave; |
1049 ec_slave_t *slave = fsm->slave; |
907 |
1050 |
908 if (datagram->state != EC_DATAGRAM_RECEIVED |
1051 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
909 || datagram->working_counter != 1) { |
1052 ec_master_queue_datagram(fsm->slave->master, datagram); |
910 fsm->state = ec_fsm_coe_error; |
1053 return; |
911 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
1054 } |
|
1055 |
|
1056 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1057 fsm->state = ec_fsm_coe_error; |
|
1058 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
|
1059 slave->ring_position); |
|
1060 return; |
|
1061 } |
|
1062 |
|
1063 if (datagram->working_counter != 1) { |
|
1064 fsm->state = ec_fsm_coe_error; |
|
1065 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
1066 " not respond.\n", slave->ring_position); |
912 return; |
1067 return; |
913 } |
1068 } |
914 |
1069 |
915 if (!ec_slave_mbox_check(datagram)) { |
1070 if (!ec_slave_mbox_check(datagram)) { |
916 if (datagram->cycles_received |
1071 if (datagram->cycles_received |
921 return; |
1076 return; |
922 } |
1077 } |
923 |
1078 |
924 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1079 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
925 ec_master_queue_datagram(fsm->slave->master, datagram); |
1080 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1081 fsm->retries = EC_FSM_RETRIES; |
926 return; |
1082 return; |
927 } |
1083 } |
928 |
1084 |
929 // Fetch response |
1085 // Fetch response |
930 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1086 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
931 ec_master_queue_datagram(fsm->slave->master, datagram); |
1087 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1088 fsm->retries = EC_FSM_RETRIES; |
932 fsm->state = ec_fsm_coe_down_response; |
1089 fsm->state = ec_fsm_coe_down_response; |
933 } |
1090 } |
934 |
1091 |
935 /*****************************************************************************/ |
1092 /*****************************************************************************/ |
936 |
1093 |
937 /** |
1094 /** |
938 CoE state: DOWN RESPONSE. |
1095 CoE state: DOWN RESPONSE. |
|
1096 \todo Timeout behavior |
939 */ |
1097 */ |
940 |
1098 |
941 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1099 void ec_fsm_coe_down_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
942 { |
1100 { |
943 ec_datagram_t *datagram = fsm->datagram; |
1101 ec_datagram_t *datagram = fsm->datagram; |
944 ec_slave_t *slave = fsm->slave; |
1102 ec_slave_t *slave = fsm->slave; |
945 uint8_t *data, mbox_prot; |
1103 uint8_t *data, mbox_prot; |
946 size_t rec_size; |
1104 size_t rec_size; |
947 ec_sdo_data_t *sdodata = fsm->sdodata; |
1105 ec_sdo_data_t *sdodata = fsm->sdodata; |
948 |
1106 |
949 if (datagram->state != EC_DATAGRAM_RECEIVED |
1107 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
950 || datagram->working_counter != 1) { |
1108 // FIXME: request again? |
951 fsm->state = ec_fsm_coe_error; |
1109 ec_master_queue_datagram(fsm->slave->master, datagram); |
952 EC_ERR("Reception of CoE download response failed.\n"); |
1110 return; |
|
1111 } |
|
1112 |
|
1113 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1114 fsm->state = ec_fsm_coe_error; |
|
1115 EC_ERR("Failed to receive CoE download response datagram from" |
|
1116 " slave %i.\n", slave->ring_position); |
|
1117 return; |
|
1118 } |
|
1119 |
|
1120 if (datagram->working_counter != 1) { |
|
1121 fsm->state = ec_fsm_coe_error; |
|
1122 EC_ERR("Reception of CoE download response failed - slave %i did not" |
|
1123 " respond.\n", slave->ring_position); |
953 return; |
1124 return; |
954 } |
1125 } |
955 |
1126 |
956 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1127 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
957 &mbox_prot, &rec_size))) { |
1128 &mbox_prot, &rec_size))) { |
1038 EC_DBG("Upload request:\n"); |
1209 EC_DBG("Upload request:\n"); |
1039 ec_print_data(data, 10); |
1210 ec_print_data(data, 10); |
1040 } |
1211 } |
1041 |
1212 |
1042 ec_master_queue_datagram(fsm->slave->master, datagram); |
1213 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1214 fsm->retries = EC_FSM_RETRIES; |
1043 fsm->state = ec_fsm_coe_up_request; |
1215 fsm->state = ec_fsm_coe_up_request; |
1044 } |
1216 } |
1045 |
1217 |
1046 /*****************************************************************************/ |
1218 /*****************************************************************************/ |
1047 |
1219 |
1048 /** |
1220 /** |
1049 CoE state: UP REQUEST. |
1221 CoE state: UP REQUEST. |
|
1222 \todo Timeout behavior |
1050 */ |
1223 */ |
1051 |
1224 |
1052 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1225 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1053 { |
1226 { |
1054 ec_datagram_t *datagram = fsm->datagram; |
1227 ec_datagram_t *datagram = fsm->datagram; |
1055 ec_slave_t *slave = fsm->slave; |
1228 ec_slave_t *slave = fsm->slave; |
1056 |
1229 |
1057 if (datagram->state != EC_DATAGRAM_RECEIVED |
1230 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1058 || datagram->working_counter != 1) { |
1231 // FIXME: check for response first? |
1059 fsm->state = ec_fsm_coe_error; |
1232 ec_master_queue_datagram(fsm->slave->master, datagram); |
1060 EC_ERR("Reception of CoE upload request failed.\n"); |
1233 return; |
|
1234 } |
|
1235 |
|
1236 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1237 fsm->state = ec_fsm_coe_error; |
|
1238 EC_ERR("Failed to receive CoE upload request for slave %i.\n", |
|
1239 slave->ring_position); |
|
1240 return; |
|
1241 } |
|
1242 |
|
1243 if (datagram->working_counter != 1) { |
|
1244 fsm->state = ec_fsm_coe_error; |
|
1245 EC_ERR("Reception of CoE upload request failed - slave %i did not" |
|
1246 " respond.\n", slave->ring_position); |
1061 return; |
1247 return; |
1062 } |
1248 } |
1063 |
1249 |
1064 fsm->cycles_start = datagram->cycles_sent; |
1250 fsm->cycles_start = datagram->cycles_sent; |
1065 |
1251 |
1066 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1252 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1067 ec_master_queue_datagram(fsm->slave->master, datagram); |
1253 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1254 fsm->retries = EC_FSM_RETRIES; |
1068 fsm->state = ec_fsm_coe_up_check; |
1255 fsm->state = ec_fsm_coe_up_check; |
1069 } |
1256 } |
1070 |
1257 |
1071 /*****************************************************************************/ |
1258 /*****************************************************************************/ |
1072 |
1259 |
1077 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1264 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1078 { |
1265 { |
1079 ec_datagram_t *datagram = fsm->datagram; |
1266 ec_datagram_t *datagram = fsm->datagram; |
1080 ec_slave_t *slave = fsm->slave; |
1267 ec_slave_t *slave = fsm->slave; |
1081 |
1268 |
1082 if (datagram->state != EC_DATAGRAM_RECEIVED |
1269 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1083 || datagram->working_counter != 1) { |
1270 ec_master_queue_datagram(fsm->slave->master, datagram); |
1084 fsm->state = ec_fsm_coe_error; |
1271 return; |
1085 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
1272 } |
|
1273 |
|
1274 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1275 fsm->state = ec_fsm_coe_error; |
|
1276 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i.\n", |
|
1277 slave->ring_position); |
|
1278 return; |
|
1279 } |
|
1280 |
|
1281 if (datagram->working_counter != 1) { |
|
1282 fsm->state = ec_fsm_coe_error; |
|
1283 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
1284 " not respond.\n", slave->ring_position); |
1086 return; |
1285 return; |
1087 } |
1286 } |
1088 |
1287 |
1089 if (!ec_slave_mbox_check(datagram)) { |
1288 if (!ec_slave_mbox_check(datagram)) { |
1090 if (datagram->cycles_received |
1289 if (datagram->cycles_received |
1095 return; |
1294 return; |
1096 } |
1295 } |
1097 |
1296 |
1098 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1297 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1099 ec_master_queue_datagram(fsm->slave->master, datagram); |
1298 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1299 fsm->retries = EC_FSM_RETRIES; |
1100 return; |
1300 return; |
1101 } |
1301 } |
1102 |
1302 |
1103 // Fetch response |
1303 // Fetch response |
1104 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1304 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1105 ec_master_queue_datagram(fsm->slave->master, datagram); |
1305 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1306 fsm->retries = EC_FSM_RETRIES; |
1106 fsm->state = ec_fsm_coe_up_response; |
1307 fsm->state = ec_fsm_coe_up_response; |
1107 } |
1308 } |
1108 |
1309 |
1109 /*****************************************************************************/ |
1310 /*****************************************************************************/ |
1110 |
1311 |
1111 /** |
1312 /** |
1112 CoE state: UP RESPONSE. |
1313 CoE state: UP RESPONSE. |
|
1314 \todo Timeout behavior |
1113 */ |
1315 */ |
1114 |
1316 |
1115 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1317 void ec_fsm_coe_up_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1116 { |
1318 { |
1117 ec_datagram_t *datagram = fsm->datagram; |
1319 ec_datagram_t *datagram = fsm->datagram; |
1123 ec_sdo_t *sdo = request->sdo; |
1325 ec_sdo_t *sdo = request->sdo; |
1124 ec_sdo_entry_t *entry = request->entry; |
1326 ec_sdo_entry_t *entry = request->entry; |
1125 uint32_t complete_size; |
1327 uint32_t complete_size; |
1126 unsigned int expedited, size_specified; |
1328 unsigned int expedited, size_specified; |
1127 |
1329 |
1128 if (datagram->state != EC_DATAGRAM_RECEIVED |
1330 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1129 || datagram->working_counter != 1) { |
1331 // FIXME: request again? |
1130 fsm->state = ec_fsm_coe_error; |
1332 ec_master_queue_datagram(fsm->slave->master, datagram); |
1131 EC_ERR("Reception of CoE upload response failed.\n"); |
1333 return; |
|
1334 } |
|
1335 |
|
1336 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1337 fsm->state = ec_fsm_coe_error; |
|
1338 EC_ERR("Failed to receive CoE upload response datagram for" |
|
1339 " slave %i.\n", slave->ring_position); |
|
1340 return; |
|
1341 } |
|
1342 |
|
1343 if (datagram->working_counter != 1) { |
|
1344 fsm->state = ec_fsm_coe_error; |
|
1345 EC_ERR("Reception of CoE upload response failed - slave %i did not" |
|
1346 " respond.\n", slave->ring_position); |
1132 return; |
1347 return; |
1133 } |
1348 } |
1134 |
1349 |
1135 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1350 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1136 &mbox_prot, &rec_size))) { |
1351 &mbox_prot, &rec_size))) { |
1249 |
1465 |
1250 /*****************************************************************************/ |
1466 /*****************************************************************************/ |
1251 |
1467 |
1252 /** |
1468 /** |
1253 CoE state: UP REQUEST. |
1469 CoE state: UP REQUEST. |
|
1470 \todo Timeout behavior |
1254 */ |
1471 */ |
1255 |
1472 |
1256 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1473 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1257 { |
1474 { |
1258 ec_datagram_t *datagram = fsm->datagram; |
1475 ec_datagram_t *datagram = fsm->datagram; |
1259 ec_slave_t *slave = fsm->slave; |
1476 ec_slave_t *slave = fsm->slave; |
1260 |
1477 |
1261 if (datagram->state != EC_DATAGRAM_RECEIVED |
1478 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1262 || datagram->working_counter != 1) { |
1479 // FIXME: check for response first? |
1263 fsm->state = ec_fsm_coe_error; |
1480 ec_master_queue_datagram(fsm->slave->master, datagram); |
1264 EC_ERR("Reception of CoE upload segment request failed.\n"); |
1481 return; |
|
1482 } |
|
1483 |
|
1484 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1485 fsm->state = ec_fsm_coe_error; |
|
1486 EC_ERR("Failed to receive CoE upload segment request datagram for" |
|
1487 " slave %i.\n", slave->ring_position); |
|
1488 return; |
|
1489 } |
|
1490 |
|
1491 if (datagram->working_counter != 1) { |
|
1492 fsm->state = ec_fsm_coe_error; |
|
1493 EC_ERR("Reception of CoE upload segment request failed - slave %i did" |
|
1494 " not respond.\n", slave->ring_position); |
1265 return; |
1495 return; |
1266 } |
1496 } |
1267 |
1497 |
1268 fsm->cycles_start = datagram->cycles_sent; |
1498 fsm->cycles_start = datagram->cycles_sent; |
1269 |
1499 |
1270 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1500 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1271 ec_master_queue_datagram(fsm->slave->master, datagram); |
1501 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1502 fsm->retries = EC_FSM_RETRIES; |
1272 fsm->state = ec_fsm_coe_up_seg_check; |
1503 fsm->state = ec_fsm_coe_up_seg_check; |
1273 } |
1504 } |
1274 |
1505 |
1275 /*****************************************************************************/ |
1506 /*****************************************************************************/ |
1276 |
1507 |
1281 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1512 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1282 { |
1513 { |
1283 ec_datagram_t *datagram = fsm->datagram; |
1514 ec_datagram_t *datagram = fsm->datagram; |
1284 ec_slave_t *slave = fsm->slave; |
1515 ec_slave_t *slave = fsm->slave; |
1285 |
1516 |
1286 if (datagram->state != EC_DATAGRAM_RECEIVED |
1517 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1287 || datagram->working_counter != 1) { |
1518 ec_master_queue_datagram(fsm->slave->master, datagram); |
1288 fsm->state = ec_fsm_coe_error; |
1519 return; |
1289 EC_ERR("Reception of CoE mailbox check datagram failed.\n"); |
1520 } |
|
1521 |
|
1522 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1523 fsm->state = ec_fsm_coe_error; |
|
1524 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i.\n", |
|
1525 slave->ring_position); |
|
1526 return; |
|
1527 } |
|
1528 |
|
1529 if (datagram->working_counter != 1) { |
|
1530 fsm->state = ec_fsm_coe_error; |
|
1531 EC_ERR("Reception of CoE mailbox check datagram failed - slave %i did" |
|
1532 " not respond.\n", slave->ring_position); |
1290 return; |
1533 return; |
1291 } |
1534 } |
1292 |
1535 |
1293 if (!ec_slave_mbox_check(datagram)) { |
1536 if (!ec_slave_mbox_check(datagram)) { |
1294 if (datagram->cycles_received |
1537 if (datagram->cycles_received |
1299 return; |
1542 return; |
1300 } |
1543 } |
1301 |
1544 |
1302 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1545 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1303 ec_master_queue_datagram(fsm->slave->master, datagram); |
1546 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1547 fsm->retries = EC_FSM_RETRIES; |
1304 return; |
1548 return; |
1305 } |
1549 } |
1306 |
1550 |
1307 // Fetch response |
1551 // Fetch response |
1308 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1552 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1309 ec_master_queue_datagram(fsm->slave->master, datagram); |
1553 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1554 fsm->retries = EC_FSM_RETRIES; |
1310 fsm->state = ec_fsm_coe_up_seg_response; |
1555 fsm->state = ec_fsm_coe_up_seg_response; |
1311 } |
1556 } |
1312 |
1557 |
1313 /*****************************************************************************/ |
1558 /*****************************************************************************/ |
1314 |
1559 |
1315 /** |
1560 /** |
1316 CoE state: UP RESPONSE. |
1561 CoE state: UP RESPONSE. |
|
1562 \todo Timeout behavior |
1317 */ |
1563 */ |
1318 |
1564 |
1319 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1565 void ec_fsm_coe_up_seg_response(ec_fsm_coe_t *fsm /**< finite state machine */) |
1320 { |
1566 { |
1321 ec_datagram_t *datagram = fsm->datagram; |
1567 ec_datagram_t *datagram = fsm->datagram; |
1327 ec_sdo_t *sdo = request->sdo; |
1573 ec_sdo_t *sdo = request->sdo; |
1328 ec_sdo_entry_t *entry = request->entry; |
1574 ec_sdo_entry_t *entry = request->entry; |
1329 uint32_t seg_size; |
1575 uint32_t seg_size; |
1330 unsigned int last_segment; |
1576 unsigned int last_segment; |
1331 |
1577 |
1332 if (datagram->state != EC_DATAGRAM_RECEIVED |
1578 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1333 || datagram->working_counter != 1) { |
1579 // FIXME: request again? |
1334 fsm->state = ec_fsm_coe_error; |
1580 ec_master_queue_datagram(fsm->slave->master, datagram); |
1335 EC_ERR("Reception of CoE upload segment response failed.\n"); |
1581 return; |
|
1582 } |
|
1583 |
|
1584 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
1585 fsm->state = ec_fsm_coe_error; |
|
1586 EC_ERR("Failed to receive CoE upload segment response datagram for" |
|
1587 " slave %i.\n", slave->ring_position); |
|
1588 return; |
|
1589 } |
|
1590 |
|
1591 if (datagram->working_counter != 1) { |
|
1592 fsm->state = ec_fsm_coe_error; |
|
1593 EC_ERR("Reception of CoE upload segment response failed - slave %i" |
|
1594 " did not respond.\n", slave->ring_position); |
1336 return; |
1595 return; |
1337 } |
1596 } |
1338 |
1597 |
1339 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1598 if (!(data = ec_slave_mbox_fetch(slave, datagram, |
1340 &mbox_prot, &rec_size))) { |
1599 &mbox_prot, &rec_size))) { |