65 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
65 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
66 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
67 void ec_fsm_slave_config_state_clear_sync(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
68 void ec_fsm_slave_config_state_dc_clear_assign(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
69 void ec_fsm_slave_config_state_mbox_sync(ec_fsm_slave_config_t *); |
|
70 #ifdef EC_SII_ASSIGN |
|
71 void ec_fsm_slave_config_state_assign_pdi(ec_fsm_slave_config_t *); |
|
72 #endif |
70 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_boot_preop(ec_fsm_slave_config_t *); |
|
74 #ifdef EC_SII_ASSIGN |
|
75 void ec_fsm_slave_config_state_assign_ethercat(ec_fsm_slave_config_t *); |
|
76 #endif |
71 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
77 void ec_fsm_slave_config_state_sdo_conf(ec_fsm_slave_config_t *); |
72 void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *); |
78 void ec_fsm_slave_config_state_soe_conf_preop(ec_fsm_slave_config_t *); |
73 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
79 void ec_fsm_slave_config_state_watchdog_divider(ec_fsm_slave_config_t *); |
74 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
80 void ec_fsm_slave_config_state_watchdog(ec_fsm_slave_config_t *); |
75 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
81 void ec_fsm_slave_config_state_pdo_sync(ec_fsm_slave_config_t *); |
85 |
91 |
86 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
92 void ec_fsm_slave_config_enter_init(ec_fsm_slave_config_t *); |
87 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
93 void ec_fsm_slave_config_enter_clear_sync(ec_fsm_slave_config_t *); |
88 void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); |
94 void ec_fsm_slave_config_enter_dc_clear_assign(ec_fsm_slave_config_t *); |
89 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
95 void ec_fsm_slave_config_enter_mbox_sync(ec_fsm_slave_config_t *); |
|
96 #ifdef EC_SII_ASSIGN |
|
97 void ec_fsm_slave_config_enter_assign_pdi(ec_fsm_slave_config_t *); |
|
98 #endif |
90 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
99 void ec_fsm_slave_config_enter_boot_preop(ec_fsm_slave_config_t *); |
91 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
100 void ec_fsm_slave_config_enter_sdo_conf(ec_fsm_slave_config_t *); |
92 void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *); |
101 void ec_fsm_slave_config_enter_soe_conf_preop(ec_fsm_slave_config_t *); |
93 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
102 void ec_fsm_slave_config_enter_pdo_conf(ec_fsm_slave_config_t *); |
94 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); |
103 void ec_fsm_slave_config_enter_watchdog_divider(ec_fsm_slave_config_t *); |
430 |
439 |
431 if (!slave->sii.mailbox_protocols) { |
440 if (!slave->sii.mailbox_protocols) { |
432 // no mailbox protocols supported |
441 // no mailbox protocols supported |
433 EC_SLAVE_DBG(slave, 1, "Slave does not support" |
442 EC_SLAVE_DBG(slave, 1, "Slave does not support" |
434 " mailbox communication.\n"); |
443 " mailbox communication.\n"); |
|
444 #ifdef EC_SII_ASSIGN |
|
445 ec_fsm_slave_config_enter_assign_pdi(fsm); |
|
446 #else |
435 ec_fsm_slave_config_enter_boot_preop(fsm); |
447 ec_fsm_slave_config_enter_boot_preop(fsm); |
|
448 #endif |
436 return; |
449 return; |
437 } |
450 } |
438 |
451 |
439 EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); |
452 EC_SLAVE_DBG(slave, 1, "Configuring mailbox sync managers...\n"); |
440 |
453 |
593 EC_SLAVE_ERR(slave, "Failed to set sync managers: "); |
606 EC_SLAVE_ERR(slave, "Failed to set sync managers: "); |
594 ec_datagram_print_wc_error(datagram); |
607 ec_datagram_print_wc_error(datagram); |
595 return; |
608 return; |
596 } |
609 } |
597 |
610 |
|
611 #ifdef EC_SII_ASSIGN |
|
612 ec_fsm_slave_config_enter_assign_pdi(fsm); |
|
613 #else |
598 ec_fsm_slave_config_enter_boot_preop(fsm); |
614 ec_fsm_slave_config_enter_boot_preop(fsm); |
599 } |
615 #endif |
|
616 } |
|
617 |
|
618 /*****************************************************************************/ |
|
619 |
|
620 #ifdef EC_SII_ASSIGN |
|
621 |
|
622 /** Assign SII to PDI. |
|
623 */ |
|
624 void ec_fsm_slave_config_enter_assign_pdi( |
|
625 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
626 ) |
|
627 { |
|
628 ec_datagram_t *datagram = fsm->datagram; |
|
629 ec_slave_t *slave = fsm->slave; |
|
630 |
|
631 if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { |
|
632 EC_SLAVE_DBG(slave, 1, "Assigning SII access to PDI.\n"); |
|
633 |
|
634 ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 0x01); |
|
635 EC_WRITE_U8(datagram->data, 0x01); // PDI |
|
636 fsm->retries = EC_FSM_RETRIES; |
|
637 fsm->state = ec_fsm_slave_config_state_assign_pdi; |
|
638 } |
|
639 else { |
|
640 ec_fsm_slave_config_enter_boot_preop(fsm); |
|
641 } |
|
642 } |
|
643 |
|
644 /*****************************************************************************/ |
|
645 |
|
646 /** Slave configuration state: ASSIGN_PDI. |
|
647 */ |
|
648 void ec_fsm_slave_config_state_assign_pdi( |
|
649 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
650 ) |
|
651 { |
|
652 ec_datagram_t *datagram = fsm->datagram; |
|
653 ec_slave_t *slave = fsm->slave; |
|
654 |
|
655 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
|
656 return; |
|
657 } |
|
658 |
|
659 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
660 EC_SLAVE_WARN(slave, "Failed receive SII assignment datagram: "); |
|
661 ec_datagram_print_state(datagram); |
|
662 goto cont_preop; |
|
663 } |
|
664 |
|
665 if (datagram->working_counter != 1) { |
|
666 EC_SLAVE_WARN(slave, "Failed to assign SII to PDI: "); |
|
667 ec_datagram_print_wc_error(datagram); |
|
668 } |
|
669 |
|
670 cont_preop: |
|
671 ec_fsm_slave_config_enter_boot_preop(fsm); |
|
672 } |
|
673 |
|
674 #endif |
600 |
675 |
601 /*****************************************************************************/ |
676 /*****************************************************************************/ |
602 |
677 |
603 /** Request PREOP state. |
678 /** Request PREOP state. |
604 */ |
679 */ |
607 ) |
682 ) |
608 { |
683 { |
609 fsm->state = ec_fsm_slave_config_state_boot_preop; |
684 fsm->state = ec_fsm_slave_config_state_boot_preop; |
610 |
685 |
611 if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { |
686 if (fsm->slave->requested_state != EC_SLAVE_STATE_BOOT) { |
612 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_PREOP); |
687 ec_fsm_change_start(fsm->fsm_change, |
|
688 fsm->slave, EC_SLAVE_STATE_PREOP); |
613 } else { // BOOT |
689 } else { // BOOT |
614 ec_fsm_change_start(fsm->fsm_change, fsm->slave, EC_SLAVE_STATE_BOOT); |
690 ec_fsm_change_start(fsm->fsm_change, |
|
691 fsm->slave, EC_SLAVE_STATE_BOOT); |
615 } |
692 } |
616 |
693 |
617 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
694 ec_fsm_change_exec(fsm->fsm_change); // execute immediately |
618 } |
695 } |
619 |
696 |
625 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
702 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
626 ) |
703 ) |
627 { |
704 { |
628 ec_slave_t *slave = fsm->slave; |
705 ec_slave_t *slave = fsm->slave; |
629 |
706 |
630 if (ec_fsm_change_exec(fsm->fsm_change)) return; |
707 if (ec_fsm_change_exec(fsm->fsm_change)) { |
|
708 return; |
|
709 } |
631 |
710 |
632 if (!ec_fsm_change_success(fsm->fsm_change)) { |
711 if (!ec_fsm_change_success(fsm->fsm_change)) { |
633 if (!fsm->fsm_change->spontaneous_change) |
712 if (!fsm->fsm_change->spontaneous_change) |
634 slave->error_flag = 1; |
713 slave->error_flag = 1; |
635 fsm->state = ec_fsm_slave_config_state_error; |
714 fsm->state = ec_fsm_slave_config_state_error; |
640 slave->jiffies_preop = fsm->datagram->jiffies_received; |
719 slave->jiffies_preop = fsm->datagram->jiffies_received; |
641 |
720 |
642 EC_SLAVE_DBG(slave, 1, "Now in %s.\n", |
721 EC_SLAVE_DBG(slave, 1, "Now in %s.\n", |
643 slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); |
722 slave->requested_state != EC_SLAVE_STATE_BOOT ? "PREOP" : "BOOT"); |
644 |
723 |
|
724 #ifdef EC_SII_ASSIGN |
|
725 EC_SLAVE_DBG(slave, 1, "Assigning SII access back to EtherCAT.\n"); |
|
726 |
|
727 ec_datagram_fpwr(fsm->datagram, slave->station_address, 0x0500, 0x01); |
|
728 EC_WRITE_U8(fsm->datagram->data, 0x00); // EtherCAT |
|
729 fsm->retries = EC_FSM_RETRIES; |
|
730 fsm->state = ec_fsm_slave_config_state_assign_ethercat; |
|
731 #else |
645 if (slave->current_state == slave->requested_state) { |
732 if (slave->current_state == slave->requested_state) { |
646 fsm->state = ec_fsm_slave_config_state_end; // successful |
733 fsm->state = ec_fsm_slave_config_state_end; // successful |
647 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
734 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
648 return; |
735 return; |
649 } |
736 } |
650 |
737 |
651 ec_fsm_slave_config_enter_sdo_conf(fsm); |
738 ec_fsm_slave_config_enter_sdo_conf(fsm); |
652 } |
739 #endif |
|
740 } |
|
741 |
|
742 /*****************************************************************************/ |
|
743 |
|
744 #ifdef EC_SII_ASSIGN |
|
745 |
|
746 /** Slave configuration state: ASSIGN_ETHERCAT. |
|
747 */ |
|
748 void ec_fsm_slave_config_state_assign_ethercat( |
|
749 ec_fsm_slave_config_t *fsm /**< slave state machine */ |
|
750 ) |
|
751 { |
|
752 ec_datagram_t *datagram = fsm->datagram; |
|
753 ec_slave_t *slave = fsm->slave; |
|
754 |
|
755 if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) { |
|
756 return; |
|
757 } |
|
758 |
|
759 if (datagram->state != EC_DATAGRAM_RECEIVED) { |
|
760 EC_SLAVE_WARN(slave, "Failed receive SII assignment datagram: "); |
|
761 ec_datagram_print_state(datagram); |
|
762 goto cont_sdo_conf; |
|
763 } |
|
764 |
|
765 if (datagram->working_counter != 1) { |
|
766 EC_SLAVE_WARN(slave, "Failed to assign SII back to EtherCAT: "); |
|
767 ec_datagram_print_wc_error(datagram); |
|
768 } |
|
769 |
|
770 cont_sdo_conf: |
|
771 if (slave->current_state == slave->requested_state) { |
|
772 fsm->state = ec_fsm_slave_config_state_end; // successful |
|
773 EC_SLAVE_DBG(slave, 1, "Finished configuration.\n"); |
|
774 return; |
|
775 } |
|
776 |
|
777 ec_fsm_slave_config_enter_sdo_conf(fsm); |
|
778 } |
|
779 |
|
780 #endif |
653 |
781 |
654 /*****************************************************************************/ |
782 /*****************************************************************************/ |
655 |
783 |
656 /** Check for SDO configurations to be applied. |
784 /** Check for SDO configurations to be applied. |
657 */ |
785 */ |