equal
deleted
inserted
replaced
258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
258 EC_WRITE_U8 (data + 2, 0x01); // Get OD List Request |
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); |
|
264 fsm->retries = EC_FSM_RETRIES; |
263 fsm->retries = EC_FSM_RETRIES; |
265 fsm->state = ec_fsm_coe_dict_request; |
264 fsm->state = ec_fsm_coe_dict_request; |
266 } |
265 } |
267 |
266 |
268 /*****************************************************************************/ |
267 /*****************************************************************************/ |
275 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
274 void ec_fsm_coe_dict_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
276 { |
275 { |
277 ec_datagram_t *datagram = fsm->datagram; |
276 ec_datagram_t *datagram = fsm->datagram; |
278 ec_slave_t *slave = fsm->slave; |
277 ec_slave_t *slave = fsm->slave; |
279 |
278 |
280 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
279 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
281 // FIXME: request again? |
280 return; // FIXME: request again? |
282 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
283 return; |
|
284 } |
|
285 |
281 |
286 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
282 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
287 fsm->state = ec_fsm_coe_error; |
283 fsm->state = ec_fsm_coe_error; |
288 EC_ERR("Failed to receive CoE dictionary request datagram for" |
284 EC_ERR("Failed to receive CoE dictionary request datagram for" |
289 " slave %i (datagram state %i).\n", |
285 " slave %i (datagram state %i).\n", |
299 } |
295 } |
300 |
296 |
301 fsm->cycles_start = datagram->cycles_sent; |
297 fsm->cycles_start = datagram->cycles_sent; |
302 |
298 |
303 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
299 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
304 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
305 fsm->retries = EC_FSM_RETRIES; |
300 fsm->retries = EC_FSM_RETRIES; |
306 fsm->state = ec_fsm_coe_dict_check; |
301 fsm->state = ec_fsm_coe_dict_check; |
307 } |
302 } |
308 |
303 |
309 /*****************************************************************************/ |
304 /*****************************************************************************/ |
315 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
310 void ec_fsm_coe_dict_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
316 { |
311 { |
317 ec_datagram_t *datagram = fsm->datagram; |
312 ec_datagram_t *datagram = fsm->datagram; |
318 ec_slave_t *slave = fsm->slave; |
313 ec_slave_t *slave = fsm->slave; |
319 |
314 |
320 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
315 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
321 ec_master_queue_datagram(fsm->slave->master, datagram); |
316 return; |
322 return; |
|
323 } |
|
324 |
317 |
325 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
318 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
326 fsm->state = ec_fsm_coe_error; |
319 fsm->state = ec_fsm_coe_error; |
327 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
320 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
328 " (datagram state %i).\n", |
321 " (datagram state %i).\n", |
346 slave->ring_position); |
339 slave->ring_position); |
347 return; |
340 return; |
348 } |
341 } |
349 |
342 |
350 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
343 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
351 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
352 fsm->retries = EC_FSM_RETRIES; |
344 fsm->retries = EC_FSM_RETRIES; |
353 return; |
345 return; |
354 } |
346 } |
355 |
347 |
356 // Fetch response |
348 // Fetch response |
357 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
349 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
358 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
359 fsm->retries = EC_FSM_RETRIES; |
350 fsm->retries = EC_FSM_RETRIES; |
360 fsm->state = ec_fsm_coe_dict_response; |
351 fsm->state = ec_fsm_coe_dict_response; |
361 } |
352 } |
362 |
353 |
363 /*****************************************************************************/ |
354 /*****************************************************************************/ |
375 size_t rec_size; |
366 size_t rec_size; |
376 unsigned int sdo_count, i; |
367 unsigned int sdo_count, i; |
377 uint16_t sdo_index; |
368 uint16_t sdo_index; |
378 ec_sdo_t *sdo; |
369 ec_sdo_t *sdo; |
379 |
370 |
380 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
371 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
381 // FIXME: request again? |
372 return; // FIXME: request again? |
382 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
383 return; |
|
384 } |
|
385 |
373 |
386 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
374 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
387 fsm->state = ec_fsm_coe_error; |
375 fsm->state = ec_fsm_coe_error; |
388 EC_ERR("Failed to receive CoE dictionary response datagram for" |
376 EC_ERR("Failed to receive CoE dictionary response datagram for" |
389 " slave %i (datagram state %i).\n", |
377 " slave %i (datagram state %i).\n", |
462 } |
450 } |
463 |
451 |
464 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
452 if (EC_READ_U8(data + 2) & 0x80) { // more messages waiting. check again. |
465 fsm->cycles_start = datagram->cycles_sent; |
453 fsm->cycles_start = datagram->cycles_sent; |
466 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
454 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
467 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
468 fsm->retries = EC_FSM_RETRIES; |
455 fsm->retries = EC_FSM_RETRIES; |
469 fsm->state = ec_fsm_coe_dict_check; |
456 fsm->state = ec_fsm_coe_dict_check; |
470 return; |
457 return; |
471 } |
458 } |
472 |
459 |
488 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
475 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
489 EC_WRITE_U8 (data + 3, 0x00); |
476 EC_WRITE_U8 (data + 3, 0x00); |
490 EC_WRITE_U16(data + 4, 0x0000); |
477 EC_WRITE_U16(data + 4, 0x0000); |
491 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
478 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
492 |
479 |
493 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
494 fsm->retries = EC_FSM_RETRIES; |
480 fsm->retries = EC_FSM_RETRIES; |
495 fsm->state = ec_fsm_coe_dict_desc_request; |
481 fsm->state = ec_fsm_coe_dict_desc_request; |
496 } |
482 } |
497 |
483 |
498 /*****************************************************************************/ |
484 /*****************************************************************************/ |
505 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
491 void ec_fsm_coe_dict_desc_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
506 { |
492 { |
507 ec_datagram_t *datagram = fsm->datagram; |
493 ec_datagram_t *datagram = fsm->datagram; |
508 ec_slave_t *slave = fsm->slave; |
494 ec_slave_t *slave = fsm->slave; |
509 |
495 |
510 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
496 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
511 // FIXME: check for response first? |
497 return; // FIXME: check for response first? |
512 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
513 return; |
|
514 } |
|
515 |
498 |
516 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
499 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
517 fsm->state = ec_fsm_coe_error; |
500 fsm->state = ec_fsm_coe_error; |
518 EC_ERR("Failed to receive CoE SDO description request datagram for" |
501 EC_ERR("Failed to receive CoE SDO description request datagram for" |
519 " slave %i (datagram state %i).\n", |
502 " slave %i (datagram state %i).\n", |
529 } |
512 } |
530 |
513 |
531 fsm->cycles_start = datagram->cycles_sent; |
514 fsm->cycles_start = datagram->cycles_sent; |
532 |
515 |
533 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
516 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
534 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
535 fsm->retries = EC_FSM_RETRIES; |
517 fsm->retries = EC_FSM_RETRIES; |
536 fsm->state = ec_fsm_coe_dict_desc_check; |
518 fsm->state = ec_fsm_coe_dict_desc_check; |
537 } |
519 } |
538 |
520 |
539 /*****************************************************************************/ |
521 /*****************************************************************************/ |
545 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
527 void ec_fsm_coe_dict_desc_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
546 { |
528 { |
547 ec_datagram_t *datagram = fsm->datagram; |
529 ec_datagram_t *datagram = fsm->datagram; |
548 ec_slave_t *slave = fsm->slave; |
530 ec_slave_t *slave = fsm->slave; |
549 |
531 |
550 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
532 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
551 ec_master_queue_datagram(fsm->slave->master, datagram); |
533 return; |
552 return; |
|
553 } |
|
554 |
534 |
555 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
535 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
556 fsm->state = ec_fsm_coe_error; |
536 fsm->state = ec_fsm_coe_error; |
557 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
537 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
558 " (datagram state %i).\n", |
538 " (datagram state %i).\n", |
575 slave->ring_position); |
555 slave->ring_position); |
576 return; |
556 return; |
577 } |
557 } |
578 |
558 |
579 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
559 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
580 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
581 fsm->retries = EC_FSM_RETRIES; |
560 fsm->retries = EC_FSM_RETRIES; |
582 return; |
561 return; |
583 } |
562 } |
584 |
563 |
585 // Fetch response |
564 // Fetch response |
586 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
565 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
587 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
588 fsm->retries = EC_FSM_RETRIES; |
566 fsm->retries = EC_FSM_RETRIES; |
589 fsm->state = ec_fsm_coe_dict_desc_response; |
567 fsm->state = ec_fsm_coe_dict_desc_response; |
590 } |
568 } |
591 |
569 |
592 /*****************************************************************************/ |
570 /*****************************************************************************/ |
603 ec_slave_t *slave = fsm->slave; |
581 ec_slave_t *slave = fsm->slave; |
604 ec_sdo_t *sdo = fsm->sdo; |
582 ec_sdo_t *sdo = fsm->sdo; |
605 uint8_t *data, mbox_prot; |
583 uint8_t *data, mbox_prot; |
606 size_t rec_size, name_size; |
584 size_t rec_size, name_size; |
607 |
585 |
608 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
586 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
609 // FIXME: request again? |
587 return; // FIXME: request again? |
610 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
611 return; |
|
612 } |
|
613 |
588 |
614 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
589 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
615 fsm->state = ec_fsm_coe_error; |
590 fsm->state = ec_fsm_coe_error; |
616 EC_ERR("Failed to receive CoE SDO description response datagram from" |
591 EC_ERR("Failed to receive CoE SDO description response datagram from" |
617 " slave %i (datagram state %i).\n", |
592 " slave %i (datagram state %i).\n", |
702 EC_WRITE_U16(data + 4, 0x0000); |
677 EC_WRITE_U16(data + 4, 0x0000); |
703 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
678 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
704 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
679 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
705 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
680 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
706 |
681 |
707 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
708 fsm->retries = EC_FSM_RETRIES; |
682 fsm->retries = EC_FSM_RETRIES; |
709 fsm->state = ec_fsm_coe_dict_entry_request; |
683 fsm->state = ec_fsm_coe_dict_entry_request; |
710 } |
684 } |
711 |
685 |
712 /*****************************************************************************/ |
686 /*****************************************************************************/ |
720 /**< finite state machine */) |
694 /**< finite state machine */) |
721 { |
695 { |
722 ec_datagram_t *datagram = fsm->datagram; |
696 ec_datagram_t *datagram = fsm->datagram; |
723 ec_slave_t *slave = fsm->slave; |
697 ec_slave_t *slave = fsm->slave; |
724 |
698 |
725 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
699 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
726 // FIXME: check for response first? |
700 return; // FIXME: check for response first? |
727 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
728 return; |
|
729 } |
|
730 |
701 |
731 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
702 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
732 fsm->state = ec_fsm_coe_error; |
703 fsm->state = ec_fsm_coe_error; |
733 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
704 EC_ERR("Failed to receive CoE SDO entry request datagram for" |
734 " slave %i (datagram state %i).\n", |
705 " slave %i (datagram state %i).\n", |
744 } |
715 } |
745 |
716 |
746 fsm->cycles_start = datagram->cycles_sent; |
717 fsm->cycles_start = datagram->cycles_sent; |
747 |
718 |
748 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
719 ec_slave_mbox_prepare_check(slave, datagram); // can not fail |
749 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
750 fsm->retries = EC_FSM_RETRIES; |
720 fsm->retries = EC_FSM_RETRIES; |
751 fsm->state = ec_fsm_coe_dict_entry_check; |
721 fsm->state = ec_fsm_coe_dict_entry_check; |
752 } |
722 } |
753 |
723 |
754 /*****************************************************************************/ |
724 /*****************************************************************************/ |
761 /**< finite state machine */) |
731 /**< finite state machine */) |
762 { |
732 { |
763 ec_datagram_t *datagram = fsm->datagram; |
733 ec_datagram_t *datagram = fsm->datagram; |
764 ec_slave_t *slave = fsm->slave; |
734 ec_slave_t *slave = fsm->slave; |
765 |
735 |
766 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
736 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
767 ec_master_queue_datagram(fsm->slave->master, datagram); |
737 return; |
768 return; |
|
769 } |
|
770 |
738 |
771 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
739 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
772 fsm->state = ec_fsm_coe_error; |
740 fsm->state = ec_fsm_coe_error; |
773 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
741 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
774 " (datagram state %i).\n", |
742 " (datagram state %i).\n", |
791 slave->ring_position); |
759 slave->ring_position); |
792 return; |
760 return; |
793 } |
761 } |
794 |
762 |
795 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
763 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
796 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
797 fsm->retries = EC_FSM_RETRIES; |
764 fsm->retries = EC_FSM_RETRIES; |
798 return; |
765 return; |
799 } |
766 } |
800 |
767 |
801 // Fetch response |
768 // Fetch response |
802 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
769 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
803 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
804 fsm->retries = EC_FSM_RETRIES; |
770 fsm->retries = EC_FSM_RETRIES; |
805 fsm->state = ec_fsm_coe_dict_entry_response; |
771 fsm->state = ec_fsm_coe_dict_entry_response; |
806 } |
772 } |
807 |
773 |
808 /*****************************************************************************/ |
774 /*****************************************************************************/ |
820 ec_sdo_t *sdo = fsm->sdo; |
786 ec_sdo_t *sdo = fsm->sdo; |
821 uint8_t *data, mbox_prot; |
787 uint8_t *data, mbox_prot; |
822 size_t rec_size, data_size; |
788 size_t rec_size, data_size; |
823 ec_sdo_entry_t *entry; |
789 ec_sdo_entry_t *entry; |
824 |
790 |
825 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
791 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
826 // FIXME: request again? |
792 return; // FIXME: request again? |
827 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
828 return; |
|
829 } |
|
830 |
793 |
831 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
794 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
832 fsm->state = ec_fsm_coe_error; |
795 fsm->state = ec_fsm_coe_error; |
833 EC_ERR("Failed to receive CoE SDO description response datagram from" |
796 EC_ERR("Failed to receive CoE SDO description response datagram from" |
834 " slave %i (datagram state %i).\n", |
797 " slave %i (datagram state %i).\n", |
930 EC_WRITE_U16(data + 4, 0x0000); |
893 EC_WRITE_U16(data + 4, 0x0000); |
931 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
894 EC_WRITE_U16(data + 6, sdo->index); // SDO index |
932 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
895 EC_WRITE_U8 (data + 8, fsm->subindex); // SDO subindex |
933 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
896 EC_WRITE_U8 (data + 9, 0x00); // value info (no values) |
934 |
897 |
935 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
936 fsm->retries = EC_FSM_RETRIES; |
898 fsm->retries = EC_FSM_RETRIES; |
937 fsm->state = ec_fsm_coe_dict_entry_request; |
899 fsm->state = ec_fsm_coe_dict_entry_request; |
938 return; |
900 return; |
939 } |
901 } |
940 |
902 |
951 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
913 EC_WRITE_U8 (data + 2, 0x03); // Get object description request |
952 EC_WRITE_U8 (data + 3, 0x00); |
914 EC_WRITE_U8 (data + 3, 0x00); |
953 EC_WRITE_U16(data + 4, 0x0000); |
915 EC_WRITE_U16(data + 4, 0x0000); |
954 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
916 EC_WRITE_U16(data + 6, fsm->sdo->index); // SDO index |
955 |
917 |
956 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
957 fsm->retries = EC_FSM_RETRIES; |
918 fsm->retries = EC_FSM_RETRIES; |
958 fsm->state = ec_fsm_coe_dict_desc_request; |
919 fsm->state = ec_fsm_coe_dict_desc_request; |
959 return; |
920 return; |
960 } |
921 } |
961 |
922 |
999 EC_WRITE_U16(data + 3, sdodata->index); |
960 EC_WRITE_U16(data + 3, sdodata->index); |
1000 EC_WRITE_U8 (data + 5, sdodata->subindex); |
961 EC_WRITE_U8 (data + 5, sdodata->subindex); |
1001 EC_WRITE_U32(data + 6, sdodata->size); |
962 EC_WRITE_U32(data + 6, sdodata->size); |
1002 memcpy(data + 10, sdodata->data, sdodata->size); |
963 memcpy(data + 10, sdodata->data, sdodata->size); |
1003 |
964 |
1004 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1005 fsm->retries = EC_FSM_RETRIES; |
965 fsm->retries = EC_FSM_RETRIES; |
1006 fsm->state = ec_fsm_coe_down_request; |
966 fsm->state = ec_fsm_coe_down_request; |
1007 } |
967 } |
1008 |
968 |
1009 /*****************************************************************************/ |
969 /*****************************************************************************/ |
1016 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
976 void ec_fsm_coe_down_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1017 { |
977 { |
1018 ec_datagram_t *datagram = fsm->datagram; |
978 ec_datagram_t *datagram = fsm->datagram; |
1019 ec_slave_t *slave = fsm->slave; |
979 ec_slave_t *slave = fsm->slave; |
1020 |
980 |
1021 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
981 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1022 // FIXME: check for response first? |
982 return; // FIXME: check for response first? |
1023 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1024 return; |
|
1025 } |
|
1026 |
983 |
1027 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
984 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1028 fsm->state = ec_fsm_coe_error; |
985 fsm->state = ec_fsm_coe_error; |
1029 EC_ERR("Failed to receive CoE download request datagram for" |
986 EC_ERR("Failed to receive CoE download request datagram for" |
1030 " slave %i (datagram state %i).\n", |
987 " slave %i (datagram state %i).\n", |
1040 } |
997 } |
1041 |
998 |
1042 fsm->cycles_start = datagram->cycles_sent; |
999 fsm->cycles_start = datagram->cycles_sent; |
1043 |
1000 |
1044 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1001 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1045 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1046 fsm->retries = EC_FSM_RETRIES; |
1002 fsm->retries = EC_FSM_RETRIES; |
1047 fsm->state = ec_fsm_coe_down_check; |
1003 fsm->state = ec_fsm_coe_down_check; |
1048 } |
1004 } |
1049 |
1005 |
1050 /*****************************************************************************/ |
1006 /*****************************************************************************/ |
1056 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1012 void ec_fsm_coe_down_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1057 { |
1013 { |
1058 ec_datagram_t *datagram = fsm->datagram; |
1014 ec_datagram_t *datagram = fsm->datagram; |
1059 ec_slave_t *slave = fsm->slave; |
1015 ec_slave_t *slave = fsm->slave; |
1060 |
1016 |
1061 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1017 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1062 ec_master_queue_datagram(fsm->slave->master, datagram); |
1018 return; |
1063 return; |
|
1064 } |
|
1065 |
1019 |
1066 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1020 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1067 fsm->state = ec_fsm_coe_error; |
1021 fsm->state = ec_fsm_coe_error; |
1068 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1022 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1069 " (datagram state %i).\n", |
1023 " (datagram state %i).\n", |
1086 slave->ring_position); |
1040 slave->ring_position); |
1087 return; |
1041 return; |
1088 } |
1042 } |
1089 |
1043 |
1090 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1044 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1091 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1092 fsm->retries = EC_FSM_RETRIES; |
1045 fsm->retries = EC_FSM_RETRIES; |
1093 return; |
1046 return; |
1094 } |
1047 } |
1095 |
1048 |
1096 // Fetch response |
1049 // Fetch response |
1097 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1050 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1098 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1099 fsm->retries = EC_FSM_RETRIES; |
1051 fsm->retries = EC_FSM_RETRIES; |
1100 fsm->state = ec_fsm_coe_down_response; |
1052 fsm->state = ec_fsm_coe_down_response; |
1101 } |
1053 } |
1102 |
1054 |
1103 /*****************************************************************************/ |
1055 /*****************************************************************************/ |
1113 ec_slave_t *slave = fsm->slave; |
1065 ec_slave_t *slave = fsm->slave; |
1114 uint8_t *data, mbox_prot; |
1066 uint8_t *data, mbox_prot; |
1115 size_t rec_size; |
1067 size_t rec_size; |
1116 ec_sdo_data_t *sdodata = fsm->sdodata; |
1068 ec_sdo_data_t *sdodata = fsm->sdodata; |
1117 |
1069 |
1118 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1070 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1119 // FIXME: request again? |
1071 return; // FIXME: request again? |
1120 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1121 return; |
|
1122 } |
|
1123 |
1072 |
1124 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1073 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1125 fsm->state = ec_fsm_coe_error; |
1074 fsm->state = ec_fsm_coe_error; |
1126 EC_ERR("Failed to receive CoE download response datagram from" |
1075 EC_ERR("Failed to receive CoE download response datagram from" |
1127 " slave %i (datagram state %i).\n", |
1076 " slave %i (datagram state %i).\n", |
1220 if (master->debug_level) { |
1169 if (master->debug_level) { |
1221 EC_DBG("Upload request:\n"); |
1170 EC_DBG("Upload request:\n"); |
1222 ec_print_data(data, 10); |
1171 ec_print_data(data, 10); |
1223 } |
1172 } |
1224 |
1173 |
1225 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1226 fsm->retries = EC_FSM_RETRIES; |
1174 fsm->retries = EC_FSM_RETRIES; |
1227 fsm->state = ec_fsm_coe_up_request; |
1175 fsm->state = ec_fsm_coe_up_request; |
1228 } |
1176 } |
1229 |
1177 |
1230 /*****************************************************************************/ |
1178 /*****************************************************************************/ |
1237 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1185 void ec_fsm_coe_up_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1238 { |
1186 { |
1239 ec_datagram_t *datagram = fsm->datagram; |
1187 ec_datagram_t *datagram = fsm->datagram; |
1240 ec_slave_t *slave = fsm->slave; |
1188 ec_slave_t *slave = fsm->slave; |
1241 |
1189 |
1242 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1190 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1243 // FIXME: check for response first? |
1191 return; // FIXME: check for response first? |
1244 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1245 return; |
|
1246 } |
|
1247 |
1192 |
1248 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1193 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1249 fsm->state = ec_fsm_coe_error; |
1194 fsm->state = ec_fsm_coe_error; |
1250 EC_ERR("Failed to receive CoE upload request for slave %i" |
1195 EC_ERR("Failed to receive CoE upload request for slave %i" |
1251 " (datagram state %i).\n", |
1196 " (datagram state %i).\n", |
1261 } |
1206 } |
1262 |
1207 |
1263 fsm->cycles_start = datagram->cycles_sent; |
1208 fsm->cycles_start = datagram->cycles_sent; |
1264 |
1209 |
1265 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1210 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1266 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1267 fsm->retries = EC_FSM_RETRIES; |
1211 fsm->retries = EC_FSM_RETRIES; |
1268 fsm->state = ec_fsm_coe_up_check; |
1212 fsm->state = ec_fsm_coe_up_check; |
1269 } |
1213 } |
1270 |
1214 |
1271 /*****************************************************************************/ |
1215 /*****************************************************************************/ |
1277 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1221 void ec_fsm_coe_up_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1278 { |
1222 { |
1279 ec_datagram_t *datagram = fsm->datagram; |
1223 ec_datagram_t *datagram = fsm->datagram; |
1280 ec_slave_t *slave = fsm->slave; |
1224 ec_slave_t *slave = fsm->slave; |
1281 |
1225 |
1282 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1226 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1283 ec_master_queue_datagram(fsm->slave->master, datagram); |
1227 return; |
1284 return; |
|
1285 } |
|
1286 |
1228 |
1287 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1229 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1288 fsm->state = ec_fsm_coe_error; |
1230 fsm->state = ec_fsm_coe_error; |
1289 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
1231 EC_ERR("Failed to receive CoE mailbox check datagram from slave %i" |
1290 " (datagram state %i).\n", |
1232 " (datagram state %i).\n", |
1307 slave->ring_position); |
1249 slave->ring_position); |
1308 return; |
1250 return; |
1309 } |
1251 } |
1310 |
1252 |
1311 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1253 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1312 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1313 fsm->retries = EC_FSM_RETRIES; |
1254 fsm->retries = EC_FSM_RETRIES; |
1314 return; |
1255 return; |
1315 } |
1256 } |
1316 |
1257 |
1317 // Fetch response |
1258 // Fetch response |
1318 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1259 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1319 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1320 fsm->retries = EC_FSM_RETRIES; |
1260 fsm->retries = EC_FSM_RETRIES; |
1321 fsm->state = ec_fsm_coe_up_response; |
1261 fsm->state = ec_fsm_coe_up_response; |
1322 } |
1262 } |
1323 |
1263 |
1324 /*****************************************************************************/ |
1264 /*****************************************************************************/ |
1339 ec_sdo_t *sdo = request->sdo; |
1279 ec_sdo_t *sdo = request->sdo; |
1340 ec_sdo_entry_t *entry = request->entry; |
1280 ec_sdo_entry_t *entry = request->entry; |
1341 uint32_t complete_size; |
1281 uint32_t complete_size; |
1342 unsigned int expedited, size_specified; |
1282 unsigned int expedited, size_specified; |
1343 |
1283 |
1344 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1284 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1345 // FIXME: request again? |
1285 return; // FIXME: request again? |
1346 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1347 return; |
|
1348 } |
|
1349 |
1286 |
1350 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1287 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1351 fsm->state = ec_fsm_coe_error; |
1288 fsm->state = ec_fsm_coe_error; |
1352 EC_ERR("Failed to receive CoE upload response datagram for" |
1289 EC_ERR("Failed to receive CoE upload response datagram for" |
1353 " slave %i (datagram state %i).\n", |
1290 " slave %i (datagram state %i).\n", |
1466 if (master->debug_level) { |
1403 if (master->debug_level) { |
1467 EC_DBG("Upload segment request:\n"); |
1404 EC_DBG("Upload segment request:\n"); |
1468 ec_print_data(data, 3); |
1405 ec_print_data(data, 3); |
1469 } |
1406 } |
1470 |
1407 |
1471 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1472 fsm->retries = EC_FSM_RETRIES; |
1408 fsm->retries = EC_FSM_RETRIES; |
1473 fsm->state = ec_fsm_coe_up_seg_request; |
1409 fsm->state = ec_fsm_coe_up_seg_request; |
1474 return; |
1410 return; |
1475 } |
1411 } |
1476 } |
1412 } |
1488 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1424 void ec_fsm_coe_up_seg_request(ec_fsm_coe_t *fsm /**< finite state machine */) |
1489 { |
1425 { |
1490 ec_datagram_t *datagram = fsm->datagram; |
1426 ec_datagram_t *datagram = fsm->datagram; |
1491 ec_slave_t *slave = fsm->slave; |
1427 ec_slave_t *slave = fsm->slave; |
1492 |
1428 |
1493 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1429 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1494 // FIXME: check for response first? |
1430 return; // FIXME: check for response first? |
1495 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1496 return; |
|
1497 } |
|
1498 |
1431 |
1499 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1432 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1500 fsm->state = ec_fsm_coe_error; |
1433 fsm->state = ec_fsm_coe_error; |
1501 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1434 EC_ERR("Failed to receive CoE upload segment request datagram for" |
1502 " slave %i (datagram state %i).\n", |
1435 " slave %i (datagram state %i).\n", |
1512 } |
1445 } |
1513 |
1446 |
1514 fsm->cycles_start = datagram->cycles_sent; |
1447 fsm->cycles_start = datagram->cycles_sent; |
1515 |
1448 |
1516 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1449 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1517 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1518 fsm->retries = EC_FSM_RETRIES; |
1450 fsm->retries = EC_FSM_RETRIES; |
1519 fsm->state = ec_fsm_coe_up_seg_check; |
1451 fsm->state = ec_fsm_coe_up_seg_check; |
1520 } |
1452 } |
1521 |
1453 |
1522 /*****************************************************************************/ |
1454 /*****************************************************************************/ |
1528 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1460 void ec_fsm_coe_up_seg_check(ec_fsm_coe_t *fsm /**< finite state machine */) |
1529 { |
1461 { |
1530 ec_datagram_t *datagram = fsm->datagram; |
1462 ec_datagram_t *datagram = fsm->datagram; |
1531 ec_slave_t *slave = fsm->slave; |
1463 ec_slave_t *slave = fsm->slave; |
1532 |
1464 |
1533 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1465 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1534 ec_master_queue_datagram(fsm->slave->master, datagram); |
1466 return; |
1535 return; |
|
1536 } |
|
1537 |
1467 |
1538 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1468 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1539 fsm->state = ec_fsm_coe_error; |
1469 fsm->state = ec_fsm_coe_error; |
1540 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1470 EC_ERR("Failed to receive CoE mailbox check datagram for slave %i" |
1541 " (datagram state %i).\n", |
1471 " (datagram state %i).\n", |
1558 slave->ring_position); |
1488 slave->ring_position); |
1559 return; |
1489 return; |
1560 } |
1490 } |
1561 |
1491 |
1562 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1492 ec_slave_mbox_prepare_check(slave, datagram); // can not fail. |
1563 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1564 fsm->retries = EC_FSM_RETRIES; |
1493 fsm->retries = EC_FSM_RETRIES; |
1565 return; |
1494 return; |
1566 } |
1495 } |
1567 |
1496 |
1568 // Fetch response |
1497 // Fetch response |
1569 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1498 ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail. |
1570 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1571 fsm->retries = EC_FSM_RETRIES; |
1499 fsm->retries = EC_FSM_RETRIES; |
1572 fsm->state = ec_fsm_coe_up_seg_response; |
1500 fsm->state = ec_fsm_coe_up_seg_response; |
1573 } |
1501 } |
1574 |
1502 |
1575 /*****************************************************************************/ |
1503 /*****************************************************************************/ |
1590 ec_sdo_t *sdo = request->sdo; |
1518 ec_sdo_t *sdo = request->sdo; |
1591 ec_sdo_entry_t *entry = request->entry; |
1519 ec_sdo_entry_t *entry = request->entry; |
1592 uint32_t seg_size; |
1520 uint32_t seg_size; |
1593 unsigned int last_segment; |
1521 unsigned int last_segment; |
1594 |
1522 |
1595 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
1523 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) |
1596 // FIXME: request again? |
1524 return; // FIXME: request again? |
1597 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1598 return; |
|
1599 } |
|
1600 |
1525 |
1601 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1526 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
1602 fsm->state = ec_fsm_coe_error; |
1527 fsm->state = ec_fsm_coe_error; |
1603 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1528 EC_ERR("Failed to receive CoE upload segment response datagram for" |
1604 " slave %i (datagram state %i).\n", |
1529 " slave %i (datagram state %i).\n", |
1684 if (master->debug_level) { |
1609 if (master->debug_level) { |
1685 EC_DBG("Upload segment request:\n"); |
1610 EC_DBG("Upload segment request:\n"); |
1686 ec_print_data(data, 3); |
1611 ec_print_data(data, 3); |
1687 } |
1612 } |
1688 |
1613 |
1689 ec_master_queue_datagram(fsm->slave->master, datagram); |
|
1690 fsm->retries = EC_FSM_RETRIES; |
1614 fsm->retries = EC_FSM_RETRIES; |
1691 fsm->state = ec_fsm_coe_up_seg_request; |
1615 fsm->state = ec_fsm_coe_up_seg_request; |
1692 return; |
1616 return; |
1693 } |
1617 } |
1694 |
1618 |