master/slave.c
branchredundancy
changeset 2360 622a7faa07df
parent 1924 2917b262554b
child 2374 e898451c054a
equal deleted inserted replaced
2359:dce13f854817 2360:622a7faa07df
   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