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