master/slave.c
changeset 1425 c1322a8793c0
parent 1421 043a518831b2
child 1426 af45bba80d85
--- a/master/slave.c	Wed Apr 29 14:03:01 2009 +0000
+++ b/master/slave.c	Thu Apr 30 14:12:47 2009 +0000
@@ -89,22 +89,24 @@
     slave->base_sync_count = 0;
 
     for (i = 0; i < EC_MAX_PORTS; i++) {
-        slave->base_ports[i] = EC_PORT_NOT_IMPLEMENTED;
-
-        slave->ports[i].dl_link = 0;
-        slave->ports[i].dl_loop = 0;
-        slave->ports[i].dl_signal = 0;
+        slave->ports[i].desc = EC_PORT_NOT_IMPLEMENTED;
+
+        slave->ports[i].link.link_up = 0;
+        slave->ports[i].link.loop_closed = 0;
+        slave->ports[i].link.signal_detected = 0;
         slave->sii.physical_layer[i] = 0xFF;
 
-        slave->dc_receive_times[i] = 0U;
-
-        slave->next_slave[i] = NULL;
+        slave->ports[i].receive_time = 0U;
+
+        slave->ports[i].next_slave = NULL;
+        slave->ports[i].delay_to_next_dc = 0U;
     }
 
     slave->base_fmmu_bit_operation = 0;
     slave->base_dc_supported = 0;
     slave->base_dc_range = EC_DC_32;
     slave->has_dc_system_time = 0;
+    slave->transition_delay = 0U;
 
     slave->sii_words = NULL;
     slave->sii_nwords = 0;
@@ -740,3 +742,126 @@
 }
 
 /*****************************************************************************/
+
+/** Calculates the sum of round-trip-times of connected ports 1-3.
+ */
+uint32_t ec_slave_calc_rtt_sum(
+        ec_slave_t *slave /**< EtherCAT slave. */
+		)
+{
+    uint32_t rtt_sum = 0, rtt;
+    unsigned int i;
+    
+    for (i = 1; i < EC_MAX_PORTS; i++) {
+        if (slave->ports[i].next_slave) {
+            rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
+            rtt_sum += rtt;
+        }
+    }
+
+    return rtt_sum;
+}
+
+/*****************************************************************************/
+
+/** Finds the next slave supporting DC delay measurement.
+ */
+ec_slave_t *ec_slave_find_next_dc_slave(
+        ec_slave_t *slave /**< EtherCAT slave. */
+		)
+{
+    ec_slave_t *dc_slave = NULL;
+
+    if (slave->base_dc_supported) {
+        dc_slave = slave;
+    } else {
+        unsigned int i;
+
+        for (i = 1; i < EC_MAX_PORTS; i++) {
+            ec_slave_t *next = slave->ports[i].next_slave;
+            if (next) {
+                dc_slave = ec_slave_find_next_dc_slave(next);
+                if (dc_slave)
+                    break;
+            }
+        }
+    }
+
+    return dc_slave;
+}
+
+/*****************************************************************************/
+
+/** Calculates the port transition delays.
+ */
+void ec_slave_calc_port_delays(
+        ec_slave_t *slave /**< EtherCAT slave. */
+		)
+{
+    unsigned int i;
+    ec_slave_t *next, *next_dc;
+    uint32_t rtt, next_rtt_sum;
+
+    if (!slave->base_dc_supported)
+        return;
+
+    for (i = 1; i < EC_MAX_PORTS; i++) {
+        next = slave->ports[i].next_slave;
+        if (!next)
+            continue;
+        next_dc = ec_slave_find_next_dc_slave(next);
+        if (!next_dc)
+            continue;
+
+        rtt = slave->ports[i].receive_time - slave->ports[i - 1].receive_time;
+        next_rtt_sum = ec_slave_calc_rtt_sum(next_dc);
+
+        slave->ports[i].delay_to_next_dc = (rtt - next_rtt_sum) / 2; // FIXME
+        next_dc->ports[0].delay_to_next_dc = (rtt - next_rtt_sum) / 2;
+
+#if 0
+        EC_DBG("delay %u:%u rtt=%u next_rtt_sum=%u delay=%u\n",
+                slave->ring_position, i, rtt, next_rtt_sum,
+                slave->ports[i].delay_to_next_dc);
+#endif
+    }
+}
+
+/*****************************************************************************/
+
+/** Calculates the bus topology; recursion function.
+ */
+void ec_slave_calc_transition_delays_rec(
+        ec_slave_t *slave, /**< Current slave. */
+        uint32_t *delay /**< Sum of delays. */
+		)
+{
+    unsigned int i;
+    ec_slave_t *next, *next_dc;
+
+#if 0
+    EC_DBG("%u: %u\n", slave->ring_position, *delay);
+#endif
+
+    slave->transition_delay = *delay;
+
+    for (i = 1; i < EC_MAX_PORTS; i++) {
+        ec_slave_port_t *port = &slave->ports[i];
+        next = port->next_slave;
+        if (!next)
+            continue;
+        next_dc = ec_slave_find_next_dc_slave(next);
+        if (!next_dc)
+            continue;
+
+        *delay = *delay + port->delay_to_next_dc;
+#if 0
+        EC_DBG("%u:%u %u\n", slave->ring_position, i, *delay);
+#endif
+        ec_slave_calc_transition_delays_rec(next_dc, delay);
+    }
+
+    *delay = *delay + slave->ports[0].delay_to_next_dc;
+}
+
+/*****************************************************************************/