Improved DC tome offset calculation:

- Only write time offset, if absolute difference to application time is
      more than 100 ms.
    - Separate handling for 32 bit and 64 bit time registers.

TODO

@@ -52,7 +52,6 @@ Future issues:
 * Override sync manager size?
 * Show Record / Array / List type of SDOs.
 * Distributed clocks:
-    - Check 32/64 bit operations.
     - Use vendor correction factors when calculating transmission delays.
     - Skip setting system time offset when application detached.
     - How to use the SYNC1 shift time?

master/fsm_slave_config.c

@@ -44,6 +44,12 @@
 
 /*****************************************************************************/
 
+/** Time difference [ns] to tolerate without setting a new system time offset.
+ */
+#define EC_SYSTEM_TIME_TOLERANCE_NS 100000000
+
+/*****************************************************************************/
+
 void ec_fsm_slave_config_state_start(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_init(ec_fsm_slave_config_t *);
 void ec_fsm_slave_config_state_clear_fmmus(ec_fsm_slave_config_t *);
@@ -400,7 +406,9 @@ void ec_fsm_slave_config_state_dc_clear_assign(
         ec_datagram_print_wc_error(datagram);
     }
 
-    // read DC system time and system time offset
+    // read DC system time (0x0910, 64 bit)
+    //                         gap (64 bit)
+    //     and time offset (0x0920, 64 bit)
     ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24);
     fsm->retries = EC_FSM_RETRIES;
     fsm->state = ec_fsm_slave_config_state_dc_read_offset;
@@ -408,6 +416,93 @@ void ec_fsm_slave_config_state_dc_clear_assign(
 
 /*****************************************************************************/
 
+#define ABS(X) ((X) >= 0 ? (X) : -(X))
+
+/** Configure 32 bit time offset.
+ */
+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. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    u32 correction, 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;
+    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,
+                (u32) slave->master->app_time, time_diff);
+
+    if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
+        new_offset = time_diff + old_offset32;
+        if (slave->master->debug_level)
+            EC_DBG("Slave %u: Setting time offset to %u (was %u)\n",
+                    slave->ring_position, new_offset, old_offset32);
+        return (u64) new_offset;
+    } else {
+        if (slave->master->debug_level)
+            EC_DBG("Slave %u: Not touching time offset.\n",
+                    slave->ring_position);
+        return old_offset;
+    }
+}
+
+/*****************************************************************************/
+
+/** Configure 64 bit time offset.
+ */
+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. */
+        )
+{
+    ec_slave_t *slave = fsm->slave;
+    u64 new_offset, correction;
+    s64 time_diff;
+
+    // correct read system time by elapsed time since read operation
+    correction = (u64) (jiffies_since_read * 1000 / HZ) * 1000000;
+    system_time += correction;
+    time_diff = fsm->slave->master->app_time - system_time;
+
+    if (slave->master->debug_level)
+        EC_DBG("Slave %u: system_time=%llu (corrected with %llu),"
+                " app_time=%llu, diff=%lli\n",
+                slave->ring_position, system_time, correction,
+                slave->master->app_time, time_diff);
+
+
+    if (ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) {
+        new_offset = time_diff + old_offset;
+        if (slave->master->debug_level)
+            EC_DBG("Slave %u: Setting time offset to %llu (was %llu)\n",
+                    slave->ring_position, new_offset, old_offset);
+    } else {
+        new_offset = old_offset;
+        if (slave->master->debug_level)
+            EC_DBG("Slave %u: Not touching time offset.\n",
+                    slave->ring_position);
+    }
+
+    return new_offset;
+}
+
+/*****************************************************************************/
+
 /** Slave configuration state: DC READ OFFSET.
  */
 void ec_fsm_slave_config_state_dc_read_offset(
@@ -417,6 +512,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;
 
     if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)
         return;
@@ -438,15 +534,17 @@ void ec_fsm_slave_config_state_dc_read_offset(
         return;
     }
 
-    system_time = EC_READ_U64(datagram->data);
+    system_time = EC_READ_U64(datagram->data);     // 0x0910
-    old_offset = EC_READ_U64(datagram->data + 16);
+    old_offset = EC_READ_U64(datagram->data + 16); // 0x0920
-    new_offset = slave->master->app_time - system_time + old_offset;
+    jiffies_since_read = jiffies - datagram->jiffies_sent;
 
-    if (slave->master->debug_level)
+    if (slave->base_dc_range == EC_DC_32) {
-        EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "
+        new_offset = ec_fsm_slave_config_dc_offset32(fsm,
-                "app_time=%llu, new_offset=%llu\n",
+                system_time, old_offset, jiffies_since_read);
-                slave->ring_position, system_time, old_offset,
+    } else {
-                slave->master->app_time, new_offset);
+        new_offset = ec_fsm_slave_config_dc_offset64(fsm,
+                system_time, old_offset, jiffies_since_read);
+    }
 
     // set DC system time offset and transmission delay
     ec_datagram_fpwr(datagram, slave->station_address, 0x0920, 12);