--- a/master/fsm_slave_config.c Mon Mar 29 15:45:44 2010 +0200
+++ b/master/fsm_slave_config.c Mon Mar 29 18:04:17 2010 +0200
@@ -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 @@
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 @@
/*****************************************************************************/
+#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 @@
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 @@
return;
}
- system_time = EC_READ_U64(datagram->data);
- old_offset = EC_READ_U64(datagram->data + 16);
- new_offset = slave->master->app_time - system_time + old_offset;
-
- if (slave->master->debug_level)
- EC_DBG("Slave %u: DC system_time=%llu old_offset=%llu, "
- "app_time=%llu, new_offset=%llu\n",
- slave->ring_position, system_time, old_offset,
- slave->master->app_time, new_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;
+
+ if (slave->base_dc_range == EC_DC_32) {
+ new_offset = ec_fsm_slave_config_dc_offset32(fsm,
+ system_time, old_offset, jiffies_since_read);
+ } else {
+ 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);