From 09b7f77f9a61a3906ad621737ecafd429fe64a99 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Tue, 7 Jun 2022 15:03:14 +0200 Subject: [PATCH] hwclock: refactor processing of PHC readings Move processing of PHC readings from sys_linux to hwclock, where statistics can be collected and filtering improved. In the PHC refclock driver accumulate the samples even if not in the external timestamping mode to update the context which will be needed for improved filtering. --- hwclock.c | 53 ++++++++++++++++++++- hwclock.h | 7 ++- ntp_io_linux.c | 18 +++---- refclock_phc.c | 28 +++++++---- sys_linux.c | 112 +++++++++++++------------------------------- sys_linux.h | 4 +- test/unit/hwclock.c | 26 +++++++--- 7 files changed, 141 insertions(+), 107 deletions(-) diff --git a/hwclock.c b/hwclock.c index 8e13e7e..a982bcc 100644 --- a/hwclock.c +++ b/hwclock.c @@ -64,6 +64,9 @@ struct HCL_Instance_Record { /* Minimum interval between samples */ double min_separation; + /* Expected precision of readings */ + double precision; + /* Flag indicating the offset and frequency values are valid */ int valid_coefs; @@ -92,7 +95,7 @@ handle_slew(struct timespec *raw, struct timespec *cooked, double dfreq, /* ================================================== */ HCL_Instance -HCL_CreateInstance(int min_samples, int max_samples, double min_separation) +HCL_CreateInstance(int min_samples, int max_samples, double min_separation, double precision) { HCL_Instance clock; @@ -110,6 +113,7 @@ HCL_CreateInstance(int min_samples, int max_samples, double min_separation) clock->n_samples = 0; clock->valid_coefs = 0; clock->min_separation = min_separation; + clock->precision = precision; LCL_AddParameterChangeHandler(handle_slew, clock); @@ -140,6 +144,53 @@ HCL_NeedsNewSample(HCL_Instance clock, struct timespec *now) /* ================================================== */ +int +HCL_ProcessReadings(HCL_Instance clock, int n_readings, struct timespec tss[][3], + struct timespec *hw_ts, struct timespec *local_ts, double *err) +{ + double delay, min_delay = 0.0, hw_sum, local_sum, local_prec; + int i, combined; + + if (n_readings < 1) + return 0; + + for (i = 0; i < n_readings; i++) { + delay = UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]); + + if (delay < 0.0) { + /* Step in the middle of a reading? */ + DEBUG_LOG("Bad reading delay=%e", delay); + return 0; + } + + if (i == 0 || min_delay > delay) + min_delay = delay; + } + + local_prec = LCL_GetSysPrecisionAsQuantum(); + + /* Combine best readings */ + for (i = combined = 0, hw_sum = local_sum = 0.0; i < n_readings; i++) { + delay = UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]); + if (delay > min_delay + MAX(local_prec, clock->precision)) + continue; + + hw_sum += UTI_DiffTimespecsToDouble(&tss[i][1], &tss[0][1]); + local_sum += UTI_DiffTimespecsToDouble(&tss[i][0], &tss[0][0]) + delay / 2.0; + combined++; + } + + assert(combined); + + UTI_AddDoubleToTimespec(&tss[0][1], hw_sum / combined, hw_ts); + UTI_AddDoubleToTimespec(&tss[0][0], local_sum / combined, local_ts); + *err = MAX(min_delay / 2.0, clock->precision); + + return 1; +} + +/* ================================================== */ + void HCL_AccumulateSample(HCL_Instance clock, struct timespec *hw_ts, struct timespec *local_ts, double err) diff --git a/hwclock.h b/hwclock.h index 3005bae..c3415ad 100644 --- a/hwclock.h +++ b/hwclock.h @@ -30,7 +30,7 @@ typedef struct HCL_Instance_Record *HCL_Instance; /* Create a new HW clock instance */ extern HCL_Instance HCL_CreateInstance(int min_samples, int max_samples, - double min_separation); + double min_separation, double precision); /* Destroy a HW clock instance */ extern void HCL_DestroyInstance(HCL_Instance clock); @@ -38,6 +38,11 @@ extern void HCL_DestroyInstance(HCL_Instance clock); /* Check if a new sample should be accumulated at this time */ extern int HCL_NeedsNewSample(HCL_Instance clock, struct timespec *now); +/* Process new readings of the HW clock in form of (sys, hw, sys) triplets and + produce a sample which can be accumulated */ +extern int HCL_ProcessReadings(HCL_Instance clock, int n_readings, struct timespec tss[][3], + struct timespec *hw_ts, struct timespec *local_ts, double *err); + /* Accumulate a new sample */ extern void HCL_AccumulateSample(HCL_Instance clock, struct timespec *hw_ts, struct timespec *local_ts, double err); diff --git a/ntp_io_linux.c b/ntp_io_linux.c index acb41fa..2c94fb7 100644 --- a/ntp_io_linux.c +++ b/ntp_io_linux.c @@ -59,8 +59,6 @@ struct Interface { /* Start of UDP data at layer 2 for IPv4 and IPv6 */ int l2_udp4_ntp_start; int l2_udp6_ntp_start; - /* Precision of PHC readings */ - double precision; /* Compensation of errors in TX and RX timestamping */ double tx_comp; double rx_comp; @@ -68,7 +66,7 @@ struct Interface { }; /* Number of PHC readings per HW clock sample */ -#define PHC_READINGS 10 +#define PHC_READINGS 25 /* Minimum interval between PHC readings */ #define MIN_PHC_POLL -6 @@ -244,12 +242,12 @@ add_interface(CNF_HwTsInterface *conf_iface) iface->l2_udp4_ntp_start = 42; iface->l2_udp6_ntp_start = 62; - iface->precision = conf_iface->precision; iface->tx_comp = conf_iface->tx_comp; iface->rx_comp = conf_iface->rx_comp; iface->clock = HCL_CreateInstance(conf_iface->min_samples, conf_iface->max_samples, - UTI_Log2ToDouble(MAX(conf_iface->minpoll, MIN_PHC_POLL))); + UTI_Log2ToDouble(MAX(conf_iface->minpoll, MIN_PHC_POLL)), + conf_iface->precision); LOG(LOGS_INFO, "Enabled HW timestamping %son %s", ts_config.rx_filter == HWTSTAMP_FILTER_NONE ? "(TX only) " : "", iface->name); @@ -566,12 +564,16 @@ process_hw_timestamp(struct Interface *iface, struct timespec *hw_ts, int l2_length) { struct timespec sample_phc_ts, sample_sys_ts, sample_local_ts, ts; + struct timespec phc_readings[PHC_READINGS][3]; double rx_correction, ts_delay, phc_err, local_err; + int n_readings; if (HCL_NeedsNewSample(iface->clock, &local_ts->ts)) { - if (SYS_Linux_GetPHCSample(iface->phc_fd, iface->phc_nocrossts, iface->precision, - &iface->phc_mode, &sample_phc_ts, &sample_sys_ts, - &phc_err)) { + n_readings = SYS_Linux_GetPHCReadings(iface->phc_fd, iface->phc_nocrossts, + &iface->phc_mode, PHC_READINGS, phc_readings); + if (n_readings > 0 && + HCL_ProcessReadings(iface->clock, n_readings, phc_readings, + &sample_phc_ts, &sample_sys_ts, &phc_err)) { LCL_CookTime(&sample_sys_ts, &sample_local_ts, &local_err); HCL_AccumulateSample(iface->clock, &sample_phc_ts, &sample_local_ts, phc_err + local_err); diff --git a/refclock_phc.c b/refclock_phc.c index 3efc667..e0e206e 100644 --- a/refclock_phc.c +++ b/refclock_phc.c @@ -75,13 +75,15 @@ static int phc_initialise(RCL_Instance instance) phc->nocrossts = RCL_GetDriverOption(instance, "nocrossts") ? 1 : 0; phc->extpps = RCL_GetDriverOption(instance, "extpps") ? 1 : 0; + phc->clock = HCL_CreateInstance(0, 16, UTI_Log2ToDouble(RCL_GetDriverPoll(instance)), + RCL_GetPrecision(instance)); + if (phc->extpps) { s = RCL_GetDriverOption(instance, "pin"); phc->pin = s ? atoi(s) : 0; s = RCL_GetDriverOption(instance, "channel"); phc->channel = s ? atoi(s) : 0; rising_edge = RCL_GetDriverOption(instance, "clear") ? 0 : 1; - phc->clock = HCL_CreateInstance(0, 16, UTI_Log2ToDouble(RCL_GetDriverPoll(instance))); if (!SYS_Linux_SetPHCExtTimestamping(phc->fd, phc->pin, phc->channel, rising_edge, !rising_edge, 1)) @@ -90,7 +92,6 @@ static int phc_initialise(RCL_Instance instance) SCH_AddFileHandler(phc->fd, SCH_FILE_INPUT, read_ext_pulse, instance); } else { phc->pin = phc->channel = 0; - phc->clock = NULL; } RCL_SetDriverData(instance, phc); @@ -106,9 +107,9 @@ static void phc_finalise(RCL_Instance instance) if (phc->extpps) { SCH_RemoveFileHandler(phc->fd); SYS_Linux_SetPHCExtTimestamping(phc->fd, phc->pin, phc->channel, 0, 0, 0); - HCL_DestroyInstance(phc->clock); } + HCL_DestroyInstance(phc->clock); close(phc->fd); Free(phc); } @@ -139,23 +140,30 @@ static void read_ext_pulse(int fd, int event, void *anything) UTI_DiffTimespecsToDouble(&phc_ts, &local_ts)); } +#define PHC_READINGS 25 + static int phc_poll(RCL_Instance instance) { + struct timespec phc_ts, sys_ts, local_ts, readings[PHC_READINGS][3]; struct phc_instance *phc; - struct timespec phc_ts, sys_ts, local_ts; double phc_err, local_err; + int n_readings; phc = (struct phc_instance *)RCL_GetDriverData(instance); - if (!SYS_Linux_GetPHCSample(phc->fd, phc->nocrossts, RCL_GetPrecision(instance), - &phc->mode, &phc_ts, &sys_ts, &phc_err)) + n_readings = SYS_Linux_GetPHCReadings(phc->fd, phc->nocrossts, &phc->mode, + PHC_READINGS, readings); + if (n_readings < 1) return 0; - if (phc->extpps) { - LCL_CookTime(&sys_ts, &local_ts, &local_err); - HCL_AccumulateSample(phc->clock, &phc_ts, &local_ts, phc_err + local_err); + if (!HCL_ProcessReadings(phc->clock, n_readings, readings, &phc_ts, &sys_ts, &phc_err)) + return 0; + + LCL_CookTime(&sys_ts, &local_ts, &local_err); + HCL_AccumulateSample(phc->clock, &phc_ts, &local_ts, phc_err + local_err); + + if (phc->extpps) return 0; - } DEBUG_LOG("PHC offset: %+.9f err: %.9f", UTI_DiffTimespecsToDouble(&phc_ts, &sys_ts), phc_err); diff --git a/sys_linux.c b/sys_linux.c index fd818e3..f2baab1 100644 --- a/sys_linux.c +++ b/sys_linux.c @@ -794,73 +794,25 @@ SYS_Linux_CheckKernelVersion(int req_major, int req_minor) #if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING) -#define PHC_READINGS 25 - static int -process_phc_readings(struct timespec ts[][3], int n, double precision, - struct timespec *phc_ts, struct timespec *sys_ts, double *err) +get_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3]) { - double min_delay = 0.0, delays[PTP_MAX_SAMPLES], phc_sum, sys_sum, sys_prec; - int i, combined; - - if (n > PTP_MAX_SAMPLES) - return 0; - - for (i = 0; i < n; i++) { - delays[i] = UTI_DiffTimespecsToDouble(&ts[i][2], &ts[i][0]); - - if (delays[i] < 0.0) { - /* Step in the middle of a PHC reading? */ - DEBUG_LOG("Bad PTP_SYS_OFFSET sample delay=%e", delays[i]); - return 0; - } - - if (!i || delays[i] < min_delay) - min_delay = delays[i]; - } - - sys_prec = LCL_GetSysPrecisionAsQuantum(); - - /* Combine best readings */ - for (i = combined = 0, phc_sum = sys_sum = 0.0; i < n; i++) { - if (delays[i] > min_delay + MAX(sys_prec, precision)) - continue; - - phc_sum += UTI_DiffTimespecsToDouble(&ts[i][1], &ts[0][1]); - sys_sum += UTI_DiffTimespecsToDouble(&ts[i][0], &ts[0][0]) + delays[i] / 2.0; - combined++; - } - - assert(combined); - - UTI_AddDoubleToTimespec(&ts[0][1], phc_sum / combined, phc_ts); - UTI_AddDoubleToTimespec(&ts[0][0], sys_sum / combined, sys_ts); - *err = MAX(min_delay / 2.0, precision); - - return 1; -} - -/* ================================================== */ - -static int -get_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, - struct timespec *sys_ts, double *err) -{ - struct timespec ts[PHC_READINGS][3]; struct ptp_sys_offset sys_off; int i; + max_samples = CLAMP(0, max_samples, PTP_MAX_SAMPLES); + /* Silence valgrind */ memset(&sys_off, 0, sizeof (sys_off)); - sys_off.n_samples = PHC_READINGS; + sys_off.n_samples = max_samples; if (ioctl(phc_fd, PTP_SYS_OFFSET, &sys_off)) { DEBUG_LOG("ioctl(%s) failed : %s", "PTP_SYS_OFFSET", strerror(errno)); return 0; } - for (i = 0; i < PHC_READINGS; i++) { + for (i = 0; i < max_samples; i++) { ts[i][0].tv_sec = sys_off.ts[i * 2].sec; ts[i][0].tv_nsec = sys_off.ts[i * 2].nsec; ts[i][1].tv_sec = sys_off.ts[i * 2 + 1].sec; @@ -869,31 +821,31 @@ get_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, ts[i][2].tv_nsec = sys_off.ts[i * 2 + 2].nsec; } - return process_phc_readings(ts, PHC_READINGS, precision, phc_ts, sys_ts, err); + return max_samples; } /* ================================================== */ static int -get_extended_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, - struct timespec *sys_ts, double *err) +get_extended_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3]) { #ifdef PTP_SYS_OFFSET_EXTENDED - struct timespec ts[PHC_READINGS][3]; struct ptp_sys_offset_extended sys_off; int i; + max_samples = CLAMP(0, max_samples, PTP_MAX_SAMPLES); + /* Silence valgrind */ memset(&sys_off, 0, sizeof (sys_off)); - sys_off.n_samples = PHC_READINGS; + sys_off.n_samples = max_samples; if (ioctl(phc_fd, PTP_SYS_OFFSET_EXTENDED, &sys_off)) { DEBUG_LOG("ioctl(%s) failed : %s", "PTP_SYS_OFFSET_EXTENDED", strerror(errno)); return 0; } - for (i = 0; i < PHC_READINGS; i++) { + for (i = 0; i < max_samples; i++) { ts[i][0].tv_sec = sys_off.ts[i][0].sec; ts[i][0].tv_nsec = sys_off.ts[i][0].nsec; ts[i][1].tv_sec = sys_off.ts[i][1].sec; @@ -902,7 +854,7 @@ get_extended_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, ts[i][2].tv_nsec = sys_off.ts[i][2].nsec; } - return process_phc_readings(ts, PHC_READINGS, precision, phc_ts, sys_ts, err); + return max_samples; #else return 0; #endif @@ -911,12 +863,14 @@ get_extended_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, /* ================================================== */ static int -get_precise_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, - struct timespec *sys_ts, double *err) +get_precise_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3]) { #ifdef PTP_SYS_OFFSET_PRECISE struct ptp_sys_offset_precise sys_off; + if (max_samples < 1) + return 0; + /* Silence valgrind */ memset(&sys_off, 0, sizeof (sys_off)); @@ -926,11 +880,11 @@ get_precise_phc_sample(int phc_fd, double precision, struct timespec *phc_ts, return 0; } - phc_ts->tv_sec = sys_off.device.sec; - phc_ts->tv_nsec = sys_off.device.nsec; - sys_ts->tv_sec = sys_off.sys_realtime.sec; - sys_ts->tv_nsec = sys_off.sys_realtime.nsec; - *err = MAX(LCL_GetSysPrecisionAsQuantum(), precision); + ts[0][0].tv_sec = sys_off.sys_realtime.sec; + ts[0][0].tv_nsec = sys_off.sys_realtime.nsec; + ts[0][1].tv_sec = sys_off.device.sec; + ts[0][1].tv_nsec = sys_off.device.nsec; + ts[0][2] = ts[0][0]; return 1; #else @@ -974,23 +928,23 @@ SYS_Linux_OpenPHC(const char *path, int phc_index) /* ================================================== */ int -SYS_Linux_GetPHCSample(int fd, int nocrossts, double precision, int *reading_mode, - struct timespec *phc_ts, struct timespec *sys_ts, double *err) +SYS_Linux_GetPHCReadings(int fd, int nocrossts, int *reading_mode, int max_readings, + struct timespec tss[][3]) { - if ((*reading_mode == 2 || !*reading_mode) && !nocrossts && - get_precise_phc_sample(fd, precision, phc_ts, sys_ts, err)) { + int r = 0; + + if ((*reading_mode == 2 || *reading_mode == 0) && !nocrossts && + (r = get_precise_phc_readings(fd, max_readings, tss)) > 0) { *reading_mode = 2; - return 1; - } else if ((*reading_mode == 3 || !*reading_mode) && - get_extended_phc_sample(fd, precision, phc_ts, sys_ts, err)) { + } else if ((*reading_mode == 3 || *reading_mode == 0) && + (r = get_extended_phc_readings(fd, max_readings, tss)) > 0) { *reading_mode = 3; - return 1; - } else if ((*reading_mode == 1 || !*reading_mode) && - get_phc_sample(fd, precision, phc_ts, sys_ts, err)) { + } else if ((*reading_mode == 1 || *reading_mode == 0) && + (r = get_phc_readings(fd, max_readings, tss)) > 0) { *reading_mode = 1; - return 1; } - return 0; + + return r; } /* ================================================== */ diff --git a/sys_linux.h b/sys_linux.h index b09ec31..4741624 100644 --- a/sys_linux.h +++ b/sys_linux.h @@ -41,8 +41,8 @@ extern int SYS_Linux_CheckKernelVersion(int req_major, int req_minor); extern int SYS_Linux_OpenPHC(const char *path, int phc_index); -extern int SYS_Linux_GetPHCSample(int fd, int nocrossts, double precision, int *reading_mode, - struct timespec *phc_ts, struct timespec *sys_ts, double *err); +extern int SYS_Linux_GetPHCReadings(int fd, int nocrossts, int *reading_mode, int max_readings, + struct timespec tss[][3]); extern int SYS_Linux_SetPHCExtTimestamping(int fd, int pin, int channel, int rising, int falling, int enable); diff --git a/test/unit/hwclock.c b/test/unit/hwclock.c index 6462c5c..1d421f3 100644 --- a/test/unit/hwclock.c +++ b/test/unit/hwclock.c @@ -21,18 +21,21 @@ #include #include "test.h" +#define MAX_READINGS 20 + void test_unit(void) { struct timespec start_hw_ts, start_local_ts, hw_ts, local_ts, ts; + struct timespec readings[MAX_READINGS][3]; HCL_Instance clock; - double freq, jitter, interval, dj, sum; - int i, j, k, count; + double freq, jitter, interval, dj, err, sum; + int i, j, k, l, n_readings, count; LCL_Initialise(); for (i = 1; i <= 8; i++) { - clock = HCL_CreateInstance(random() % (1 << i), 1 << i, 1.0); + clock = HCL_CreateInstance(random() % (1 << i), 1 << i, 1.0, 1e-9); for (j = 0, count = 0, sum = 0.0; j < 100; j++) { UTI_ZeroTimespec(&start_hw_ts); @@ -63,10 +66,21 @@ test_unit(void) UTI_AddDoubleToTimespec(&start_hw_ts, k * interval * freq + TST_GetRandomDouble(-jitter, jitter), &hw_ts); - if (HCL_NeedsNewSample(clock, &local_ts)) - HCL_AccumulateSample(clock, &hw_ts, &local_ts, 2.0 * jitter); + if (HCL_NeedsNewSample(clock, &local_ts)) { + n_readings = random() % MAX_READINGS + 1; + for (l = 0; l < n_readings; l++) { + UTI_AddDoubleToTimespec(&local_ts, -TST_GetRandomDouble(0.0, jitter / 10.0), &readings[l][0]); + readings[l][1] = hw_ts; + UTI_AddDoubleToTimespec(&local_ts, TST_GetRandomDouble(0.0, jitter / 10.0), &readings[l][2]); + } - TEST_CHECK(clock->valid_coefs || clock->n_samples < 2); + UTI_ZeroTimespec(&hw_ts); + UTI_ZeroTimespec(&local_ts); + if (HCL_ProcessReadings(clock, n_readings, readings, &hw_ts, &local_ts, &err)) + HCL_AccumulateSample(clock, &hw_ts, &local_ts, 2.0 * jitter); + } + + TEST_CHECK(clock->valid_coefs == (clock->n_samples >= 2)); if (!clock->valid_coefs) continue;