From ef375c0935cd7fd9944a03103d26611f551886d9 Mon Sep 17 00:00:00 2001 From: Martin Troxler Date: Fri, 7 May 2010 15:25:55 +0200 Subject: [PATCH] Improved distributed clock offset calculation to speed-up initial drift compensation --- master/fsm_slave_config.c | 39 +++++++++++++++++++++++---------------- master/master.c | 10 +++++++++- master/master.h | 6 +++++- 3 files changed, 37 insertions(+), 18 deletions(-) diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index ebbd8fa1..6bb27c4e 100644 --- a/master/fsm_slave_config.c +++ b/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 diff --git a/master/master.c b/master/master.c index 7fe49943..c0c5d944 100644 --- a/master/master.c +++ b/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; diff --git a/master/master.h b/master/master.h index 1f02fed9..3e088c53 100644 --- a/master/master.h +++ b/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