hwclock: improve filtering of readings

Estimate the 1st and 2nd 10-quantile of the reading delay and accept
only readings between them unless the error of the offset predicted from
previous samples is larger than the minimum reading error. With the 25
PHC readings per ioctl it should combine about 2-3 readings.

This should improve hwclock tracking and synchronization stability when
a PHC reading delay occasionally falls below the normal expected
minimum, or all readings in the batch are delayed significantly (e.g.
due to high PCIe load).

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
 

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;
+  double delay, raw_delay, min_delay, low_delay, high_delay, e, pred_err;
-  int i, combined;
+  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,32 +189,62 @@ 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 */
+  low_delay = QNT_GetQuantile(clock->delay_quants, DELAY_QUANT_MIN_K);
-  for (i = combined = 0, hw_sum = local_sum = 0.0; i < n_readings; i++) {
+  high_delay = QNT_GetQuantile(clock->delay_quants, DELAY_QUANT_MAX_K);
-    delay = UTI_DiffTimespecsToDouble(&tss[i][2], &tss[i][0]);
+  low_delay = MIN(low_delay, high_delay);
-    if (delay > min_delay + MAX(local_prec, clock->precision))
+  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);
 
+  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);
 
+  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;
+}
+
 /* ================================================== */
 
 void

test/unit/hwclock.c

@@ -18,9 +18,13 @@
  **********************************************************************
  */
 
-#include <hwclock.c>
+#include <config.h>
 #include "test.h"
 
+#if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING)
+
+#include <hwclock.c>
+
 #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