diff --git a/configure b/configure index 73d1867..70a0a32 100755 --- a/configure +++ b/configure @@ -737,7 +737,7 @@ if [ $feat_timestamping = "1" ] && [ $try_timestamping = "1" ] && &val, sizeof (val));' then add_def HAVE_LINUX_TIMESTAMPING - EXTRA_OBJECTS="$EXTRA_OBJECTS hwclock.o ntp_io_linux.o" + EXTRA_OBJECTS="$EXTRA_OBJECTS hwclock.o ntp_io_linux.o quantiles.o" if test_code 'other timestamping options' \ 'sys/types.h sys/socket.h linux/net_tstamp.h' '' '' ' @@ -830,7 +830,7 @@ if [ $feat_refclock = "1" ] && [ $feat_phc = "1" ] && [ $try_phc = "1" ] && \ 'ioctl(1, PTP_CLOCK_GETCAPS + PTP_SYS_OFFSET, 0);' then grep 'HAVE_LINUX_TIMESTAMPING' config.h > /dev/null || - EXTRA_OBJECTS="$EXTRA_OBJECTS hwclock.o" + EXTRA_OBJECTS="$EXTRA_OBJECTS hwclock.o quantiles.o" add_def FEAT_PHC fi diff --git a/hwclock.c b/hwclock.c index a982bcc..c014667 100644 --- a/hwclock.c +++ b/hwclock.c @@ -33,6 +33,7 @@ #include "local.h" #include "logging.h" #include "memory.h" +#include "quantiles.h" #include "regress.h" #include "util.h" @@ -43,6 +44,13 @@ /* Maximum acceptable frequency offset of the clock */ #define MAX_FREQ_OFFSET (2.0 / 3.0) +/* Quantiles for filtering readings by delay */ +#define DELAY_QUANT_MIN_K 1 +#define DELAY_QUANT_MAX_K 2 +#define DELAY_QUANT_Q 10 +#define DELAY_QUANT_REPEAT 7 +#define DELAY_QUANT_MIN_STEP 1.0e-9 + struct HCL_Instance_Record { /* HW and local reference timestamp */ struct timespec hw_ref; @@ -73,6 +81,9 @@ struct HCL_Instance_Record { /* Estimated offset and frequency of HW clock relative to local clock */ double offset; double frequency; + + /* Estimated quantiles of reading delay */ + QNT_Instance delay_quants; }; /* ================================================== */ @@ -114,6 +125,9 @@ HCL_CreateInstance(int min_samples, int max_samples, double min_separation, doub clock->valid_coefs = 0; clock->min_separation = min_separation; clock->precision = precision; + clock->delay_quants = QNT_CreateInstance(DELAY_QUANT_MIN_K, DELAY_QUANT_MAX_K, + DELAY_QUANT_Q, DELAY_QUANT_REPEAT, + DELAY_QUANT_MIN_STEP); LCL_AddParameterChangeHandler(handle_slew, clock); @@ -125,6 +139,7 @@ HCL_CreateInstance(int min_samples, int max_samples, double min_separation, doub void HCL_DestroyInstance(HCL_Instance clock) { LCL_RemoveParameterChangeHandler(handle_slew, clock); + QNT_DestroyInstance(clock->delay_quants); Free(clock->y_data); Free(clock->x_data); Free(clock); @@ -148,14 +163,25 @@ 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; + double delay, raw_delay, min_delay, low_delay, high_delay, e, pred_err; + double delay_sum, hw_sum, local_sum, local_prec, freq; + int i, min_reading, combined; + struct timespec ts1, ts2; if (n_readings < 1) return 0; + /* Work out the current correction multiplier needed to get cooked delays */ + LCL_CookTime(&tss[0][0], &ts1, NULL); + LCL_CookTime(&tss[n_readings - 1][2], &ts2, NULL); + if (UTI_CompareTimespecs(&tss[0][0], &tss[n_readings - 1][2]) < 0) + freq = UTI_DiffTimespecsToDouble(&ts1, &ts2) / + UTI_DiffTimespecsToDouble(&tss[0][0], &tss[n_readings - 1][2]); + else + freq = 1.0; + for (i = 0; i < n_readings; i++) { - delay = UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]); + delay = freq * UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]); if (delay < 0.0) { /* Step in the middle of a reading? */ @@ -163,30 +189,60 @@ HCL_ProcessReadings(HCL_Instance clock, int n_readings, struct timespec tss[][3] return 0; } - if (i == 0 || min_delay > delay) + if (i == 0 || min_delay > delay) { min_delay = delay; + min_reading = i; + } + + QNT_Accumulate(clock->delay_quants, 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)) + low_delay = QNT_GetQuantile(clock->delay_quants, DELAY_QUANT_MIN_K); + high_delay = QNT_GetQuantile(clock->delay_quants, DELAY_QUANT_MAX_K); + low_delay = MIN(low_delay, high_delay); + high_delay = MAX(high_delay, low_delay + local_prec); + + /* Combine readings with delay in the expected interval */ + for (i = combined = 0, delay_sum = hw_sum = local_sum = 0.0; i < n_readings; i++) { + raw_delay = UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]); + delay = freq * raw_delay; + + if (delay < low_delay || delay > high_delay) continue; + delay_sum += delay; hw_sum += UTI_DiffTimespecsToDouble(&tss[i][1], &tss[0][1]); - local_sum += UTI_DiffTimespecsToDouble(&tss[i][0], &tss[0][0]) + delay / 2.0; + local_sum += UTI_DiffTimespecsToDouble(&tss[i][0], &tss[0][0]) + raw_delay / 2.0; combined++; } - assert(combined); + DEBUG_LOG("Combined %d readings lo=%e hi=%e", combined, low_delay, high_delay); - UTI_AddDoubleToTimespec(&tss[0][1], hw_sum / combined, hw_ts); - UTI_AddDoubleToTimespec(&tss[0][0], local_sum / combined, local_ts); + if (combined > 0) { + UTI_AddDoubleToTimespec(&tss[0][1], hw_sum / combined, hw_ts); + UTI_AddDoubleToTimespec(&tss[0][0], local_sum / combined, local_ts); + *err = MAX(delay_sum / combined / 2.0, clock->precision); + return 1; + } + + /* Accept the reading with minimum delay if its interval does not contain + the current offset predicted from previous samples */ + + *hw_ts = tss[min_reading][1]; + UTI_AddDoubleToTimespec(&tss[min_reading][0], min_delay / freq / 2.0, local_ts); *err = MAX(min_delay / 2.0, clock->precision); - return 1; + pred_err = 0.0; + LCL_CookTime(local_ts, &ts1, NULL); + if (!HCL_CookTime(clock, hw_ts, &ts2, &e) || + ((pred_err = UTI_DiffTimespecsToDouble(&ts1, &ts2)) > *err)) { + DEBUG_LOG("Accepted reading err=%e prerr=%e", *err, pred_err); + return 1; + } + + return 0; } /* ================================================== */ diff --git a/test/unit/hwclock.c b/test/unit/hwclock.c index 1d421f3..eb9ce7c 100644 --- a/test/unit/hwclock.c +++ b/test/unit/hwclock.c @@ -18,9 +18,13 @@ ********************************************************************** */ -#include +#include #include "test.h" +#if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING) + +#include + #define MAX_READINGS 20 void @@ -30,9 +34,10 @@ test_unit(void) struct timespec readings[MAX_READINGS][3]; HCL_Instance clock; double freq, jitter, interval, dj, err, sum; - int i, j, k, l, n_readings, count; + int i, j, k, l, new_sample, n_readings, count; LCL_Initialise(); + TST_RegisterDummyDrivers(); for (i = 1; i <= 8; i++) { clock = HCL_CreateInstance(random() % (1 << i), 1 << i, 1.0, 1e-9); @@ -51,11 +56,14 @@ test_unit(void) clock->n_samples = 0; clock->valid_coefs = 0; + QNT_Reset(clock->delay_quants); + + new_sample = 0; for (k = 0; k < 100; k++) { UTI_AddDoubleToTimespec(&start_hw_ts, k * interval * freq, &hw_ts); UTI_AddDoubleToTimespec(&start_local_ts, k * interval, &local_ts); - if (HCL_CookTime(clock, &hw_ts, &ts, NULL)) { + if (HCL_CookTime(clock, &hw_ts, &ts, NULL) && new_sample) { dj = fabs(UTI_DiffTimespecsToDouble(&ts, &local_ts) / jitter); DEBUG_LOG("delta/jitter %f", dj); if (clock->n_samples >= clock->max_samples / 2) @@ -76,8 +84,12 @@ test_unit(void) UTI_ZeroTimespec(&hw_ts); UTI_ZeroTimespec(&local_ts); - if (HCL_ProcessReadings(clock, n_readings, readings, &hw_ts, &local_ts, &err)) + if (HCL_ProcessReadings(clock, n_readings, readings, &hw_ts, &local_ts, &err)) { HCL_AccumulateSample(clock, &hw_ts, &local_ts, 2.0 * jitter); + new_sample = 1; + } else { + new_sample = 0; + } } TEST_CHECK(clock->valid_coefs == (clock->n_samples >= 2)); @@ -96,3 +108,10 @@ test_unit(void) LCL_Finalise(); } +#else +void +test_unit(void) +{ + TEST_REQUIRE(0); +} +#endif