87 slave->base_build = 0; |
87 slave->base_build = 0; |
88 slave->base_fmmu_count = 0; |
88 slave->base_fmmu_count = 0; |
89 slave->base_sync_count = 0; |
89 slave->base_sync_count = 0; |
90 |
90 |
91 for (i = 0; i < EC_MAX_PORTS; i++) { |
91 for (i = 0; i < EC_MAX_PORTS; i++) { |
92 slave->base_ports[i] = EC_PORT_NOT_IMPLEMENTED; |
92 slave->ports[i].desc = EC_PORT_NOT_IMPLEMENTED; |
93 |
93 |
94 slave->ports[i].dl_link = 0; |
94 slave->ports[i].link.link_up = 0; |
95 slave->ports[i].dl_loop = 0; |
95 slave->ports[i].link.loop_closed = 0; |
96 slave->ports[i].dl_signal = 0; |
96 slave->ports[i].link.signal_detected = 0; |
97 slave->sii.physical_layer[i] = 0xFF; |
97 slave->sii.physical_layer[i] = 0xFF; |
98 |
98 |
99 slave->dc_receive_times[i] = 0U; |
99 slave->ports[i].receive_time = 0U; |
100 |
100 |
101 slave->next_slave[i] = NULL; |
101 slave->ports[i].next_slave = NULL; |
|
102 slave->ports[i].delay_to_next_dc = 0U; |
102 } |
103 } |
103 |
104 |
104 slave->base_fmmu_bit_operation = 0; |
105 slave->base_fmmu_bit_operation = 0; |
105 slave->base_dc_supported = 0; |
106 slave->base_dc_supported = 0; |
106 slave->base_dc_range = EC_DC_32; |
107 slave->base_dc_range = EC_DC_32; |
107 slave->has_dc_system_time = 0; |
108 slave->has_dc_system_time = 0; |
|
109 slave->transition_delay = 0U; |
108 |
110 |
109 slave->sii_words = NULL; |
111 slave->sii_words = NULL; |
110 slave->sii_nwords = 0; |
112 slave->sii_nwords = 0; |
111 |
113 |
112 slave->sii.alias = 0x0000; |
114 slave->sii.alias = 0x0000; |
738 } |
740 } |
739 } |
741 } |
740 } |
742 } |
741 |
743 |
742 /*****************************************************************************/ |
744 /*****************************************************************************/ |
|
745 |
|
746 /** Calculates the sum of round-trip-times of connected ports 1-3. |
|
747 */ |
|
748 uint32_t ec_slave_calc_rtt_sum( |
|
749 ec_slave_t *slave /**< EtherCAT slave. */ |
|
750 ) |
|
751 { |
|
752 uint32_t rtt_sum = 0, rtt; |
|
753 unsigned int i; |
|
754 |
|
755 for (i = 1; i < EC_MAX_PORTS; i++) { |
|
756 if (slave->ports[i].next_slave) { |
|
757 rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time; |
|
758 rtt_sum += rtt; |
|
759 } |
|
760 } |
|
761 |
|
762 return rtt_sum; |
|
763 } |
|
764 |
|
765 /*****************************************************************************/ |
|
766 |
|
767 /** Finds the next slave supporting DC delay measurement. |
|
768 */ |
|
769 ec_slave_t *ec_slave_find_next_dc_slave( |
|
770 ec_slave_t *slave /**< EtherCAT slave. */ |
|
771 ) |
|
772 { |
|
773 ec_slave_t *dc_slave = NULL; |
|
774 |
|
775 if (slave->base_dc_supported) { |
|
776 dc_slave = slave; |
|
777 } else { |
|
778 unsigned int i; |
|
779 |
|
780 for (i = 1; i < EC_MAX_PORTS; i++) { |
|
781 ec_slave_t *next = slave->ports[i].next_slave; |
|
782 if (next) { |
|
783 dc_slave = ec_slave_find_next_dc_slave(next); |
|
784 if (dc_slave) |
|
785 break; |
|
786 } |
|
787 } |
|
788 } |
|
789 |
|
790 return dc_slave; |
|
791 } |
|
792 |
|
793 /*****************************************************************************/ |
|
794 |
|
795 /** Calculates the port transition delays. |
|
796 */ |
|
797 void ec_slave_calc_port_delays( |
|
798 ec_slave_t *slave /**< EtherCAT slave. */ |
|
799 ) |
|
800 { |
|
801 unsigned int i; |
|
802 ec_slave_t *next, *next_dc; |
|
803 uint32_t rtt, next_rtt_sum; |
|
804 |
|
805 if (!slave->base_dc_supported) |
|
806 return; |
|
807 |
|
808 for (i = 1; i < EC_MAX_PORTS; i++) { |
|
809 next = slave->ports[i].next_slave; |
|
810 if (!next) |
|
811 continue; |
|
812 next_dc = ec_slave_find_next_dc_slave(next); |
|
813 if (!next_dc) |
|
814 continue; |
|
815 |
|
816 rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time; |
|
817 next_rtt_sum = ec_slave_calc_rtt_sum(next_dc); |
|
818 |
|
819 slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME |
|
820 next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2; |
|
821 |
|
822 #if 0 |
|
823 EC_DBG("delay %u:%u rtt=%u next_rtt_sum=%u delay=%u\n", |
|
824 slave->ring_position, i, rtt, next_rtt_sum, |
|
825 slave->ports[i].delay_to_next_dc); |
|
826 #endif |
|
827 } |
|
828 } |
|
829 |
|
830 /*****************************************************************************/ |
|
831 |
|
832 /** Calculates the bus topology; recursion function. |
|
833 */ |
|
834 void ec_slave_calc_transition_delays_rec( |
|
835 ec_slave_t *slave, /**< Current slave. */ |
|
836 uint32_t *delay /**< Sum of delays. */ |
|
837 ) |
|
838 { |
|
839 unsigned int i; |
|
840 ec_slave_t *next, *next_dc; |
|
841 |
|
842 #if 0 |
|
843 EC_DBG("%u: %u\n", slave->ring_position, *delay); |
|
844 #endif |
|
845 |
|
846 slave->transition_delay = *delay; |
|
847 |
|
848 for (i = 1; i < EC_MAX_PORTS; i++) { |
|
849 ec_slave_port_t *port = &slave->ports[i]; |
|
850 next = port->next_slave; |
|
851 if (!next) |
|
852 continue; |
|
853 next_dc = ec_slave_find_next_dc_slave(next); |
|
854 if (!next_dc) |
|
855 continue; |
|
856 |
|
857 *delay = *delay + port->delay_to_next_dc; |
|
858 #if 0 |
|
859 EC_DBG("%u:%u %u\n", slave->ring_position, i, *delay); |
|
860 #endif |
|
861 ec_slave_calc_transition_delays_rec(next_dc, delay); |
|
862 } |
|
863 |
|
864 *delay = *delay + slave->ports[0].delay_to_next_dc; |
|
865 } |
|
866 |
|
867 /*****************************************************************************/ |