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;