801 } |
801 } |
802 } |
802 } |
803 |
803 |
804 /*****************************************************************************/ |
804 /*****************************************************************************/ |
805 |
805 |
|
806 /** Returns the previous connected port of a given port. |
|
807 */ |
|
808 unsigned int ec_slave_get_previous_port( |
|
809 ec_slave_t *slave, /**< EtherCAT slave. */ |
|
810 unsigned int port_index /**< Port index. */ |
|
811 ) |
|
812 { |
|
813 static const unsigned int prev_table[EC_MAX_PORTS] = { |
|
814 2, 3, 1, 0 |
|
815 }; |
|
816 |
|
817 if (port_index >= EC_MAX_PORTS) { |
|
818 EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n", |
|
819 __func__, port_index); |
|
820 } |
|
821 |
|
822 do { |
|
823 port_index = prev_table[port_index]; |
|
824 if (slave->ports[port_index].next_slave) { |
|
825 return port_index; |
|
826 } |
|
827 } while (port_index); |
|
828 |
|
829 return 0; |
|
830 } |
|
831 |
|
832 /*****************************************************************************/ |
|
833 |
|
834 /** Returns the next connected port of a given port. |
|
835 */ |
|
836 unsigned int ec_slave_get_next_port( |
|
837 ec_slave_t *slave, /**< EtherCAT slave. */ |
|
838 unsigned int port_index /**< Port index. */ |
|
839 ) |
|
840 { |
|
841 static const unsigned int next_table[EC_MAX_PORTS] = { |
|
842 3, 2, 0, 1 |
|
843 }; |
|
844 |
|
845 if (port_index >= EC_MAX_PORTS) { |
|
846 EC_SLAVE_WARN(slave, "%s(port_index=%u): Invalid port index!\n", |
|
847 __func__, port_index); |
|
848 } |
|
849 |
|
850 do { |
|
851 port_index = next_table[port_index]; |
|
852 if (slave->ports[port_index].next_slave) { |
|
853 return port_index; |
|
854 } |
|
855 } while (port_index); |
|
856 |
|
857 return 0; |
|
858 } |
|
859 |
|
860 /*****************************************************************************/ |
|
861 |
806 /** Calculates the sum of round-trip-times of connected ports 1-3. |
862 /** Calculates the sum of round-trip-times of connected ports 1-3. |
807 */ |
863 */ |
808 uint32_t ec_slave_calc_rtt_sum( |
864 uint32_t ec_slave_calc_rtt_sum( |
809 ec_slave_t *slave /**< EtherCAT slave. */ |
865 ec_slave_t *slave /**< EtherCAT slave. */ |
810 ) |
866 ) |
811 { |
867 { |
812 uint32_t rtt_sum = 0, rtt; |
868 uint32_t rtt_sum = 0, rtt; |
813 unsigned int i; |
869 unsigned int port_index = ec_slave_get_next_port(slave, 0); |
814 |
870 |
815 for (i = 1; i < EC_MAX_PORTS; i++) { |
871 while (port_index != 0) { |
816 if (slave->ports[i].next_slave) { |
872 unsigned int prev_index = |
817 rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time; |
873 ec_slave_get_previous_port(slave, port_index); |
818 rtt_sum += rtt; |
874 |
819 } |
875 rtt = slave->ports[port_index].receive_time - |
|
876 slave->ports[prev_index].receive_time; |
|
877 rtt_sum += rtt; |
|
878 port_index = ec_slave_get_next_port(slave, port_index); |
820 } |
879 } |
821 |
880 |
822 return rtt_sum; |
881 return rtt_sum; |
823 } |
882 } |
824 |
883 |
828 */ |
887 */ |
829 ec_slave_t *ec_slave_find_next_dc_slave( |
888 ec_slave_t *ec_slave_find_next_dc_slave( |
830 ec_slave_t *slave /**< EtherCAT slave. */ |
889 ec_slave_t *slave /**< EtherCAT slave. */ |
831 ) |
890 ) |
832 { |
891 { |
|
892 unsigned int port_index; |
833 ec_slave_t *dc_slave = NULL; |
893 ec_slave_t *dc_slave = NULL; |
834 |
894 |
835 if (slave->base_dc_supported) { |
895 if (slave->base_dc_supported) { |
836 dc_slave = slave; |
896 dc_slave = slave; |
837 } else { |
897 } else { |
838 unsigned int i; |
898 port_index = ec_slave_get_next_port(slave, 0); |
839 |
899 |
840 for (i = 1; i < EC_MAX_PORTS; i++) { |
900 while (port_index != 0) { |
841 ec_slave_t *next = slave->ports[i].next_slave; |
901 ec_slave_t *next = slave->ports[port_index].next_slave; |
|
902 |
842 if (next) { |
903 if (next) { |
843 dc_slave = ec_slave_find_next_dc_slave(next); |
904 dc_slave = ec_slave_find_next_dc_slave(next); |
844 if (dc_slave) |
905 |
|
906 if (dc_slave) { |
845 break; |
907 break; |
|
908 } |
846 } |
909 } |
|
910 port_index = ec_slave_get_next_port(slave, port_index); |
847 } |
911 } |
848 } |
912 } |
849 |
913 |
850 return dc_slave; |
914 return dc_slave; |
851 } |
915 } |
856 */ |
920 */ |
857 void ec_slave_calc_port_delays( |
921 void ec_slave_calc_port_delays( |
858 ec_slave_t *slave /**< EtherCAT slave. */ |
922 ec_slave_t *slave /**< EtherCAT slave. */ |
859 ) |
923 ) |
860 { |
924 { |
861 unsigned int i; |
925 unsigned int port_index; |
862 ec_slave_t *next, *next_dc; |
926 ec_slave_t *next_slave, *next_dc; |
863 uint32_t rtt, next_rtt_sum; |
927 uint32_t rtt, next_rtt_sum; |
864 |
928 |
865 if (!slave->base_dc_supported) |
929 if (!slave->base_dc_supported) |
866 return; |
930 return; |
867 |
931 |
868 for (i = 1; i < EC_MAX_PORTS; i++) { |
932 port_index = ec_slave_get_next_port(slave, 0); |
869 next = slave->ports[i].next_slave; |
933 |
870 if (!next) |
934 while (port_index != 0) { |
871 continue; |
935 next_slave = slave->ports[port_index].next_slave; |
872 next_dc = ec_slave_find_next_dc_slave(next); |
936 next_dc = ec_slave_find_next_dc_slave(next_slave); |
873 if (!next_dc) |
937 |
874 continue; |
938 if (next_dc) { |
875 |
939 unsigned int prev_port = |
876 rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time; |
940 ec_slave_get_previous_port(slave, port_index); |
877 next_rtt_sum = ec_slave_calc_rtt_sum(next_dc); |
941 |
878 |
942 rtt = slave->ports[port_index].receive_time - |
879 slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME |
943 slave->ports[prev_port].receive_time; |
880 next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2; |
944 next_rtt_sum = ec_slave_calc_rtt_sum(next_dc); |
|
945 |
|
946 slave->ports[port_index].delay_to_next_dc = |
|
947 (rtt - next_rtt_sum) / 2; // FIXME |
|
948 next_dc->ports[0].delay_to_next_dc = |
|
949 (rtt - next_rtt_sum) / 2; |
881 |
950 |
882 #if 0 |
951 #if 0 |
883 EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u" |
952 EC_SLAVE_DBG(slave, 1, "delay %u:%u rtt=%u" |
884 " next_rtt_sum=%u delay=%u\n", |
953 " next_rtt_sum=%u delay=%u\n", |
885 slave->ring_position, i, rtt, next_rtt_sum, |
954 slave->ring_position, port_index, rtt, next_rtt_sum, |
886 slave->ports[i].delay_to_next_dc); |
955 slave->ports[port_index].delay_to_next_dc); |
887 #endif |
956 #endif |
|
957 } |
|
958 |
|
959 port_index = ec_slave_get_next_port(slave, port_index); |
888 } |
960 } |
889 } |
961 } |
890 |
962 |
891 /*****************************************************************************/ |
963 /*****************************************************************************/ |
892 |
964 |
896 ec_slave_t *slave, /**< Current slave. */ |
968 ec_slave_t *slave, /**< Current slave. */ |
897 uint32_t *delay /**< Sum of delays. */ |
969 uint32_t *delay /**< Sum of delays. */ |
898 ) |
970 ) |
899 { |
971 { |
900 unsigned int i; |
972 unsigned int i; |
901 ec_slave_t *next, *next_dc; |
973 ec_slave_t *next_dc; |
902 |
974 |
|
975 EC_SLAVE_DBG(slave, 1, "%s(delay = %u ns)\n", __func__, *delay); |
|
976 |
|
977 slave->transmission_delay = *delay; |
|
978 |
|
979 i = ec_slave_get_next_port(slave, 0); |
|
980 |
|
981 while (i != 0) { |
|
982 ec_slave_port_t *port = &slave->ports[i]; |
|
983 next_dc = ec_slave_find_next_dc_slave(port->next_slave); |
|
984 if (next_dc) { |
|
985 *delay = *delay + port->delay_to_next_dc; |
903 #if 0 |
986 #if 0 |
904 EC_SLAVE_DBG(slave, 1, "%u\n", *delay); |
987 EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", |
|
988 slave->ring_position, i, *delay); |
905 #endif |
989 #endif |
906 |
990 ec_slave_calc_transmission_delays_rec(next_dc, delay); |
907 slave->transmission_delay = *delay; |
991 } |
908 |
992 |
909 for (i = 1; i < EC_MAX_PORTS; i++) { |
993 i = ec_slave_get_next_port(slave, i); |
910 ec_slave_port_t *port = &slave->ports[i]; |
|
911 next = port->next_slave; |
|
912 if (!next) |
|
913 continue; |
|
914 next_dc = ec_slave_find_next_dc_slave(next); |
|
915 if (!next_dc) |
|
916 continue; |
|
917 |
|
918 *delay = *delay + port->delay_to_next_dc; |
|
919 #if 0 |
|
920 EC_SLAVE_DBG(slave, 1, "%u:%u %u\n", slave->ring_position, i, *delay); |
|
921 #endif |
|
922 ec_slave_calc_transmission_delays_rec(next_dc, delay); |
|
923 } |
994 } |
924 |
995 |
925 *delay = *delay + slave->ports[0].delay_to_next_dc; |
996 *delay = *delay + slave->ports[0].delay_to_next_dc; |
926 } |
997 } |
927 |
998 |