Improved distributed clock offset calculation to speed-up initial drift compensation

master/fsm_slave_config.c

@@ -422,25 +422,25 @@ u64 ec_fsm_slave_config_dc_offset32(
         ec_fsm_slave_config_t *fsm, /**< slave state machine */
         u64 system_time, /**< System time register. */
         u64 old_offset, /**< Time offset register. */
-        unsigned long jiffies_since_read /**< Jiffies for correction. */
+        u64 correction /**< Correction. */
         )
 {
     ec_slave_t *slave = fsm->slave;
-    u32 correction, system_time32, old_offset32, new_offset;
+    u32 correction32, system_time32, old_offset32, new_offset;
     s32 time_diff;
 
     system_time32 = (u32) system_time;
     old_offset32 = (u32) old_offset;
 
     // correct read system time by elapsed time since read operation
-    correction = jiffies_since_read * 1000 / HZ * 1000000;
-    system_time32 += correction;
+    correction32 = (u32)correction;
+    system_time32 -= correction32;
     time_diff = (u32) slave->master->app_time - system_time32;
 
     if (slave->master->debug_level)
         EC_DBG("Slave %u: system_time=%u (corrected with %u),"
                 " app_time=%u, diff=%i\n",
-                slave->ring_position, system_time32, correction,
+                slave->ring_position, system_time32, correction32,
                 (u32) slave->master->app_time, time_diff);
 
     if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
@@ -465,16 +465,14 @@ u64 ec_fsm_slave_config_dc_offset64(
         ec_fsm_slave_config_t *fsm, /**< slave state machine */
         u64 system_time, /**< System time register. */
         u64 old_offset, /**< Time offset register. */
-        unsigned long jiffies_since_read /**< Jiffies for correction. */
+        u64 correction /**< Correction. */
         )
 {
     ec_slave_t *slave = fsm->slave;
-    u64 new_offset, correction;
+    u64 new_offset;
     s64 time_diff;
 
-    // correct read system time by elapsed time since read operation
-    correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;
-    system_time += correction;
+    system_time -= correction;
     time_diff = fsm->slave->master->app_time - system_time;
 
     if (slave->master->debug_level)
@@ -509,9 +507,7 @@ void ec_fsm_slave_config_state_dc_read_offset(
 {
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
-    u64 system_time, old_offset, new_offset;
-    unsigned long jiffies_since_read;
-
+    u64 system_time, old_offset, new_offset, correction;
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return;
 
@@ -534,14 +530,25 @@ void ec_fsm_slave_config_state_dc_read_offset(
 
     system_time = EC_READ_U64(datagram->data);     // 0x0910
     old_offset = EC_READ_U64(datagram->data + 16); // 0x0920
-    jiffies_since_read = jiffies - datagram->jiffies_sent;
+    /* correct read system time by elapsed time since read operation
+       and the app_time set time */
+#ifdef EC_HAVE_CYCLES
+    correction =
+            (datagram->cycles_sent - slave->master->dc_cycles_app_time)
+            * 1000000LL;
+    do_div(correction,cpu_khz);
+#else
+    correction =
+            (u64) ((datagram->jiffies_sent-slave->master->dc_jiffies_app_time) * 1000 / HZ)
+            * 1000000;
+#endif
 
     if (slave->base_dc_range == EC_DC_32) {
         new_offset = ec_fsm_slave_config_dc_offset32(fsm,
-                system_time, old_offset, jiffies_since_read);
+                system_time, old_offset, correction);
     } else {
         new_offset = ec_fsm_slave_config_dc_offset64(fsm,
-                system_time, old_offset, jiffies_since_read);
+                system_time, old_offset, correction);
     }
 
     // set DC system time offset and transmission delay

master/master.c

@@ -144,10 +144,14 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */
 
     master->slaves = NULL;
     master->slave_count = 0;
-    
+
     INIT_LIST_HEAD(&master->configs);
 
     master->app_time = 0ULL;
+#ifdef EC_HAVE_CYCLES
+    master->dc_cycles_app_time = 0;
+#endif
+    master->dc_jiffies_app_time = 0;
     master->app_start_time = 0ULL;
     master->has_start_time = 0;
 
@@ -2381,6 +2385,10 @@ void ecrt_master_configured_slaves_state(const ec_master_t *master, ec_master_st
 void ecrt_master_application_time(ec_master_t *master, uint64_t app_time)
 {
     master->app_time = app_time;
+#ifdef EC_HAVE_CYCLES
+    master->dc_cycles_app_time = get_cycles();
+#endif
+    master->dc_jiffies_app_time = jiffies;
 
     if (unlikely(!master->has_start_time)) {
         master->app_start_time = app_time;

master/master.h

@@ -130,7 +130,11 @@ struct ec_master {
     ec_datagram_t sync_mon_datagram; /**< Datagram used for DC synchronisation
                                        monitoring. */
     ec_slave_t *dc_ref_clock; /**< DC reference clock slave. */
-    
+#ifdef EC_HAVE_CYCLES
+    cycles_t dc_cycles_app_time; /** cycles at last ecrt_master_sync() call.*/
+#endif
+    unsigned long dc_jiffies_app_time;/** jiffies at last
+                                            ecrt_master_sync() call.*/
     unsigned int scan_busy; /**< Current scan state. */
     unsigned int allow_scan; /**< \a True, if slave scanning is allowed. */
     struct semaphore scan_sem; /**< Semaphore protecting the \a scan_busy