diff --git a/lib/master.c b/lib/master.c index d07bb4ac..81d100db 100644 --- a/lib/master.c +++ b/lib/master.c @@ -800,7 +800,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) ret = ioctl(master->fd, EC_IOCTL_REF_CLOCK_TIME, time); if (EC_IOCTL_IS_ERROR(ret)) { ret = EC_IOCTL_ERRNO(ret); - if (ret != EIO && ret != ENXIO) { + if (ret != EIO && ret != ENXIO && ret != EAGAIN) { // do not log if no refclk or no refclk time yet fprintf(stderr, "Failed to get reference clock time: %s\n", strerror(ret)); diff --git a/master/datagram.c b/master/datagram.c index f6163274..da508509 100644 --- a/master/datagram.c +++ b/master/datagram.c @@ -102,6 +102,7 @@ void ec_datagram_init(ec_datagram_t *datagram /**< EtherCAT datagram. */) datagram->cycles_sent = 0; #endif datagram->jiffies_sent = 0; + datagram->app_time_sent = 0; #ifdef EC_HAVE_CYCLES datagram->cycles_received = 0; #endif diff --git a/master/datagram.h b/master/datagram.h index e902fbd4..96af32fa 100644 --- a/master/datagram.h +++ b/master/datagram.h @@ -102,6 +102,7 @@ typedef struct { cycles_t cycles_sent; /**< Time, when the datagram was sent. */ #endif unsigned long jiffies_sent; /**< Jiffies, when the datagram was sent. */ + uint64_t app_time_sent; /**< App time, when the datagram was sent. */ #ifdef EC_HAVE_CYCLES cycles_t cycles_received; /**< Time, when the datagram was received. */ #endif diff --git a/master/fsm_master.c b/master/fsm_master.c index 3a90e335..07703ced 100644 --- a/master/fsm_master.c +++ b/master/fsm_master.c @@ -48,7 +48,7 @@ /** Time difference [ns] to tolerate without setting a new system time offset. */ -#define EC_SYSTEM_TIME_TOLERANCE_NS 10000 +#define EC_SYSTEM_TIME_TOLERANCE_NS 1000 /*****************************************************************************/ @@ -417,6 +417,7 @@ void ec_fsm_master_state_broadcast( // application applied configurations if (master->config_changed) { master->config_changed = 0; + master->dc_offset_valid = 0; EC_MASTER_DBG(master, 1, "Configuration changed.\n"); @@ -805,6 +806,7 @@ void ec_fsm_master_action_configure( if (master->config_changed) { master->config_changed = 0; + master->dc_offset_valid = 0; // abort iterating through slaves, // first compensate DC system time offsets, @@ -1178,6 +1180,7 @@ void ec_fsm_master_state_scan_slave( if (master->slave_count) { master->config_changed = 0; + master->dc_offset_valid = 0; fsm->slave = master->slaves; // begin with first slave ec_fsm_master_enter_write_system_times(fsm); @@ -1233,7 +1236,7 @@ void ec_fsm_master_enter_write_system_times( { ec_master_t *master = fsm->master; - if (master->dc_ref_time) { + if (master->active) { while (fsm->slave < master->slaves + master->slave_count) { if (!fsm->slave->base_dc_supported @@ -1249,19 +1252,16 @@ void ec_fsm_master_enter_write_system_times( // and time offset (0x0920, 64 bit) ec_datagram_fprd(fsm->datagram, fsm->slave->station_address, 0x0910, 24); + ec_datagram_zero(fsm->datagram); fsm->datagram->device_index = fsm->slave->device_index; fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_master_state_dc_read_offset; return; } + master->dc_offset_valid = 1; } else { - if (master->active) { - EC_MASTER_WARN(master, "No application time received up to now," - " but master already active.\n"); - } else { - EC_MASTER_DBG(master, 1, "No app_time received up to now.\n"); - } + EC_MASTER_DBG(master, 1, "No app_time received up to now.\n"); } // scanning and setting system times complete @@ -1279,26 +1279,21 @@ u64 ec_fsm_master_dc_offset32( ec_fsm_master_t *fsm, /**< Master state machine. */ u64 system_time, /**< System time register. */ u64 old_offset, /**< Time offset register. */ - unsigned long jiffies_since_read /**< Jiffies for correction. */ + u64 app_time_sent /**< Master app time by reading. */ ) { ec_slave_t *slave = fsm->slave; - u32 correction, system_time32, old_offset32, new_offset; + u32 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; + time_diff = (u32) app_time_sent - system_time32; EC_SLAVE_DBG(slave, 1, "DC 32 bit system time offset calculation:" - " system_time=%u (corrected with %u)," - " app_time=%llu, diff=%i\n", - system_time32, correction, - slave->master->app_time, time_diff); + " system_time=%u, app_time=%llu, diff=%i\n", + system_time32, app_time_sent, time_diff); if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { new_offset = time_diff + old_offset32; @@ -1321,23 +1316,18 @@ u64 ec_fsm_master_dc_offset64( ec_fsm_master_t *fsm, /**< Master state machine. */ u64 system_time, /**< System time register. */ u64 old_offset, /**< Time offset register. */ - unsigned long jiffies_since_read /**< Jiffies for correction. */ + u64 app_time_sent /**< Master app time by reading. */ ) { 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; - time_diff = fsm->slave->master->app_time - system_time; + time_diff = app_time_sent - system_time; EC_SLAVE_DBG(slave, 1, "DC 64 bit system time offset calculation:" - " system_time=%llu (corrected with %llu)," - " app_time=%llu, diff=%lli\n", - system_time, correction, - slave->master->app_time, time_diff); + " system_time=%llu, app_time=%llu, diff=%lli\n", + system_time, app_time_sent, time_diff); if (EC_ABS(time_diff) > EC_SYSTEM_TIME_TOLERANCE_NS) { new_offset = time_diff + old_offset; @@ -1361,8 +1351,8 @@ void ec_fsm_master_state_dc_read_offset( { ec_datagram_t *datagram = fsm->datagram; ec_slave_t *slave = fsm->slave; + ec_master_t *master = fsm->master; u64 system_time, old_offset, new_offset; - unsigned long jiffies_since_read; if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) return; @@ -1383,16 +1373,24 @@ void ec_fsm_master_state_dc_read_offset( return; } + if (unlikely(!master->dc_ref_time)) { + EC_MASTER_WARN(master, "No app_time received up to now," + " abort DC time offset calculation.\n"); + // scanning and setting system times complete + ec_master_request_op(master); + ec_fsm_master_restart(fsm); + return; + } + 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_master_dc_offset32(fsm, - system_time, old_offset, jiffies_since_read); + system_time, old_offset, datagram->app_time_sent); } else { new_offset = ec_fsm_master_dc_offset64(fsm, - system_time, old_offset, jiffies_since_read); + system_time, old_offset, datagram->app_time_sent); } // set DC system time offset and transmission delay diff --git a/master/fsm_slave_config.c b/master/fsm_slave_config.c index 49d916f4..865daba4 100755 --- a/master/fsm_slave_config.c +++ b/master/fsm_slave_config.c @@ -1372,6 +1372,7 @@ void ec_fsm_slave_config_state_dc_cycle( fsm->jiffies_start = jiffies; ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_config_state_dc_sync_check; } @@ -1430,11 +1431,12 @@ void ec_fsm_slave_config_state_dc_sync_check( if ((diff_ms < last_diff_ms) || (diff_ms >= (last_diff_ms + 100))) { last_diff_ms = diff_ms; EC_SLAVE_DBG(slave, 1, "Sync after %4lu ms: %10u ns\n", - diff_ms, abs_sync_diff); + diff_ms, EC_READ_U32(datagram->data) & 0x80000000 ? -abs_sync_diff: abs_sync_diff); } // check synchrony again ec_datagram_fprd(datagram, slave->station_address, 0x092c, 4); + ec_datagram_zero(datagram); fsm->retries = EC_FSM_RETRIES; return; } diff --git a/master/fsm_slave_scan.c b/master/fsm_slave_scan.c index 708fe891..e24fefe2 100644 --- a/master/fsm_slave_scan.c +++ b/master/fsm_slave_scan.c @@ -957,6 +957,7 @@ void ec_fsm_slave_scan_enter_preop( * sizes. */ ec_datagram_fprd(fsm->datagram, slave->station_address, 0x0800, EC_SYNC_PAGE_SIZE * 2); + ec_datagram_zero(fsm->datagram); fsm->retries = EC_FSM_RETRIES; fsm->state = ec_fsm_slave_scan_state_sync; } diff --git a/master/master.c b/master/master.c index 9ed5241c..748f4368 100644 --- a/master/master.c +++ b/master/master.c @@ -188,6 +188,7 @@ int ec_master_init(ec_master_t *master, /**< EtherCAT master */ master->app_time = 0ULL; master->dc_ref_time = 0ULL; + master->dc_offset_valid = 0; master->scan_busy = 0; master->allow_scan = 1; @@ -1092,6 +1093,7 @@ size_t ec_master_send_datagrams( datagram->cycles_sent = cycles_sent; #endif datagram->jiffies_sent = jiffies_sent; + datagram->app_time_sent = master->app_time; list_del_init(&datagram->sent); // empty list of sent datagrams } @@ -1237,14 +1239,17 @@ void ec_master_receive_datagrams( datagram->working_counter = EC_READ_U16(cur_data); cur_data += EC_DATAGRAM_FOOTER_SIZE; - // dequeue the received datagram - datagram->state = EC_DATAGRAM_RECEIVED; #ifdef EC_HAVE_CYCLES datagram->cycles_received = master->devices[EC_DEVICE_MAIN].cycles_poll; #endif datagram->jiffies_received = master->devices[EC_DEVICE_MAIN].jiffies_poll; + + barrier(); /* reordering might lead to races */ + + // dequeue the received datagram + datagram->state = EC_DATAGRAM_RECEIVED; list_del_init(&datagram->queue); } } @@ -2396,6 +2401,7 @@ int ecrt_master_activate(ec_master_t *master) // notify state machine, that the configuration shall now be applied master->config_changed = 1; + master->dc_offset_valid = 0; return 0; } @@ -2500,6 +2506,13 @@ void ecrt_master_deactivate(ec_master_t *master) master->app_time = 0ULL; master->dc_ref_time = 0ULL; + master->dc_offset_valid = 0; + + /* Disallow scanning to get into the same state like after a master + * request (after ec_master_enter_operation_phase() is called). */ + master->allow_scan = 0; + + master->active = 0; #ifdef EC_EOE if (eoe_was_running) { @@ -2510,12 +2523,6 @@ void ecrt_master_deactivate(ec_master_t *master) "EtherCAT-IDLE")) { EC_MASTER_WARN(master, "Failed to restart master thread!\n"); } - - /* Disallow scanning to get into the same state like after a master - * request (after ec_master_enter_operation_phase() is called). */ - master->allow_scan = 0; - - master->active = 0; } /*****************************************************************************/ @@ -2881,6 +2888,10 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) return -EIO; } + if (!master->dc_offset_valid) { + return -EAGAIN; + } + // Get returned datagram time, transmission delay removed. *time = EC_READ_U32(master->sync_datagram.data) - master->dc_ref_clock->transmission_delay; @@ -2892,7 +2903,7 @@ int ecrt_master_reference_clock_time(ec_master_t *master, uint32_t *time) void ecrt_master_sync_reference_clock(ec_master_t *master) { - if (master->dc_ref_clock) { + if (master->dc_ref_clock && master->dc_offset_valid) { EC_WRITE_U32(master->ref_sync_datagram.data, master->app_time); ec_master_queue_datagram(master, &master->ref_sync_datagram); } @@ -2915,7 +2926,7 @@ void ecrt_master_sync_reference_clock_to( void ecrt_master_sync_slave_clocks(ec_master_t *master) { - if (master->dc_ref_clock) { + if (master->dc_ref_clock && master->dc_offset_valid) { ec_datagram_zero(&master->sync_datagram); ec_master_queue_datagram(master, &master->sync_datagram); } diff --git a/master/master.h b/master/master.h index ca83d01f..abb5b472 100644 --- a/master/master.h +++ b/master/master.h @@ -237,6 +237,7 @@ struct ec_master { u64 app_time; /**< Time of the last ecrt_master_sync() call. */ u64 dc_ref_time; /**< Common reference timestamp for DC start times. */ + u8 dc_offset_valid; /**< DC slaves have valid system time offsets*/ ec_datagram_t ref_sync_datagram; /**< Datagram used for synchronizing the reference clock to the master clock. */ ec_datagram_t sync_datagram; /**< Datagram used for DC drift