refactor: caculate systime offset after clearing receive time

Signed-off-by: sakumisu <1203593632@qq.com>

include/ec_slave.h

@@ -83,6 +83,7 @@ typedef struct ec_slave {
     uint8_t base_dc_supported;         /**< Distributed clocks are supported. */
     ec_slave_dc_range_t base_dc_range; /**< DC range. */
     uint32_t transmission_delay;       /**< DC system time transmission delay (offset from reference clock). */
+    uint64_t system_time_offset;       /**< Offset to master system time [ns]. */
 
     uint32_t logical_start_address;
     uint32_t odata_size;

src/ec_cmd.c

@@ -211,8 +211,11 @@ static void ec_cmd_show_slave_detail(ec_master_t *master, uint32_t slave_idx)
         } else {
             EC_LOG_RAW("32 bit\n");
         }
+        EC_LOG_RAW("  DC system time offset: %lld ns\n",
+                   slave_data.system_time_offset);
         EC_LOG_RAW("  DC system time transmission delay: %d ns\n",
                    slave_data.transmission_delay);
+
     } else {
         EC_LOG_RAW("no\n");
     }

src/ec_slave.c

@@ -251,7 +251,7 @@ static void ec_slave_calc_port_delays(ec_slave_t *slave)
     }
 }
 
-void ec_slave_calc_transmission_delays(ec_slave_t *slave, uint32_t *delay)
+static void ec_slave_calc_transmission_delays(ec_slave_t *slave, uint32_t *delay)
 {
     unsigned int i;
     ec_slave_t *next_dc;
@@ -423,54 +423,6 @@ static inline void ec_slave_fmmu_config(ec_sm_info_t *sm, uint8_t *data)
     EC_WRITE_U16(data + 14, 0x0000); // reserved
 }
 
-static int ec_slave_config_dc_systime_and_delay(ec_slave_t *slave)
-{
-    ec_datagram_t *datagram;
-    uint64_t system_time, old_system_time_offset, new_system_time_offset;
-    uint64_t time_diff;
-    int ret;
-
-    datagram = &slave->master->main_datagram;
-
-    if (slave->base_dc_supported) {
-        ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->SYS_TIME), 24);
-        datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
-        if (ret < 0) {
-            return ret;
-        }
-        system_time = EC_READ_U64(datagram->data);
-        old_system_time_offset = EC_READ_U64(datagram->data + 16);
-
-        if (slave->base_dc_range == EC_DC_32) {
-            system_time = (uint32_t)system_time + (jiffies - datagram->jiffies_sent) * 1000;
-            old_system_time_offset = (uint32_t)old_system_time_offset;
-        } else {
-            system_time = system_time + (jiffies - datagram->jiffies_sent) * 1000;
-        }
-
-        time_diff = ec_timestamp_get_time_ns() - system_time;
-
-        if (time_diff > 1000000) { // 1ms
-            new_system_time_offset = time_diff + old_system_time_offset;
-        } else {
-            new_system_time_offset = old_system_time_offset;
-        }
-
-        // set DC system time offset and transmission delay
-        ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->SYS_TIME_OFFSET), 12);
-        EC_WRITE_U64(datagram->data, new_system_time_offset);
-        EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);
-        datagram->netdev_idx = slave->netdev_idx;
-        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
-        if (ret < 0) {
-            return ret;
-        }
-    }
-
-    return 0;
-}
-
 static int ec_slave_config(ec_slave_t *slave)
 {
     ec_datagram_t *datagram;
@@ -739,7 +691,15 @@ static int ec_slave_config(ec_slave_t *slave)
     if (slave->config && slave->config->dc_assign_activate) {
         EC_ASSERT_MSG(slave->base_dc_supported, "Slave %u does not support DC", slave->index);
 
-        ec_slave_config_dc_systime_and_delay(slave);
+        // set DC system time offset and transmission delay
+        ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->SYS_TIME_OFFSET), 12);
+        EC_WRITE_U64(datagram->data, slave->system_time_offset);
+        EC_WRITE_U32(datagram->data + 8, slave->transmission_delay);
+        datagram->netdev_idx = slave->netdev_idx;
+        ret = ec_master_queue_ext_datagram(slave->master, datagram, true, true);
+        if (ret < 0) {
+            return ret;
+        }
 
         // set DC cycle times
         ec_datagram_fpwr(datagram, slave->station_address, ESCREG_OF(ESCREG->SYNC0_CYC_TIME), 8);
@@ -846,7 +806,7 @@ static void ec_master_clear_slaves(ec_master_t *master)
     master->slave_count = 0;
 }
 
-static int ec_master_calc_topology_rec(ec_master_t *master, ec_slave_t *port0_slave, unsigned int *slave_position)
+static int ec_master_calc_topology(ec_master_t *master, ec_slave_t *port0_slave, unsigned int *slave_position)
 {
     ec_slave_t *slave = master->slaves + *slave_position;
     unsigned int port_index;
@@ -865,8 +825,8 @@ static int ec_master_calc_topology_rec(ec_master_t *master, ec_slave_t *port0_sl
             if (*slave_position < master->slave_count) {
                 slave->ports[port_index].next_slave =
                     master->slaves + *slave_position;
-                ret = ec_master_calc_topology_rec(master,
-                                                  slave, slave_position);
+                ret = ec_master_calc_topology(master,
+                                              slave, slave_position);
                 if (ret) {
                     return ret;
                 }
@@ -909,17 +869,6 @@ static void ec_master_find_dc_ref_clock(ec_master_t *master)
                      ref ? ref->station_address : 0xffff, ESCREG_OF(ESCREG->SYS_TIME), ref ? (ref->base_dc_range == EC_DC_64 ? 8 : 4) : 4);
 }
 
-static void ec_master_calc_topology(ec_master_t *master)
-{
-    unsigned int slave_position = 0;
-
-    if (master->slave_count == 0)
-        return;
-
-    EC_ASSERT_MSG(ec_master_calc_topology_rec(master, NULL, &slave_position) == 0,
-                  "Failed to calculate bus topology\n");
-}
-
 static void ec_master_calc_transmission_delays(ec_master_t *master)
 {
     ec_slave_t *slave;
@@ -938,12 +887,16 @@ static void ec_master_calc_transmission_delays(ec_master_t *master)
 
 static void ec_master_calc_dc(ec_master_t *master)
 {
+    unsigned int slave_position = 0;
+
     // find DC reference clock
     ec_master_find_dc_ref_clock(master);
 
     // calculate bus topology
-    ec_master_calc_topology(master);
+    EC_ASSERT_MSG(ec_master_calc_topology(master, NULL, &slave_position) == 0,
+                  "Failed to calculate bus topology\n");
 
+    // calculate transmission delay
     ec_master_calc_transmission_delays(master);
 }
 
@@ -997,6 +950,7 @@ void ec_slaves_scanning(ec_master_t *master)
     uint8_t netdev_idx;
     bool rescan_required = false;
     unsigned int scan_jiffies;
+    uint64_t ref_time[CONFIG_EC_MAX_NETDEVS] = { 0 };
     int ret;
 
     datagram = &master->main_datagram;
@@ -1128,6 +1082,8 @@ void ec_slaves_scanning(ec_master_t *master)
                 step = 4;
                 goto mutex_unlock;
             }
+
+            ref_time[netdev_idx] = datagram->jiffies_sent;
         }
 
         for (uint32_t slave_index = 0; slave_index < master->slave_count; slave_index++) {
@@ -1214,6 +1170,17 @@ void ec_slaves_scanning(ec_master_t *master)
                 for (uint8_t i = 0; i < EC_MAX_PORTS; i++) {
                     slave->ports[i].receive_time = EC_READ_U32(datagram->data + 4 * i);
                 }
+
+                ec_datagram_fprd(datagram, slave->station_address, ESCREG_OF(ESCREG->RCVT_ECAT_PU), 8);
+                ec_datagram_zero(datagram);
+                datagram->netdev_idx = slave->netdev_idx;
+                ret = ec_master_queue_ext_datagram(master, datagram, true, true);
+                if (ret < 0) {
+                    step = 9;
+                    goto mutex_unlock;
+                }
+
+                slave->system_time_offset = ref_time[netdev_idx] - EC_READ_U64(datagram->data);
             } else {
             }