bkinnightskytw
/
ethercat
mirror of https://gitlab.com/bkinnightskytw/ethercat.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Applied 0002-Distributed-Clock-fixes-from-Jun-Yuan.patch

This commit is contained in:
Florian Pose 2022-06-23 10:12:12 +02:00
parent b4f8c0e750
commit 17eddce68d
8 changed files with 59 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
lib/master.c
View File

@ -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));

1
master/datagram.c
View File

@ -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

1
master/datagram.h
View File

@ -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

62
master/fsm_master.c
View File

@ -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

4
master/fsm_slave_config.c
View File

@ -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;
}

1
master/fsm_slave_scan.c
View File

@ -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;
}

31
master/master.c
View File

@ -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);
}

1
master/master.h
View File

@ -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