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

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

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

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,
+    n_readings = SYS_Linux_GetPHCReadings(iface->phc_fd, iface->phc_nocrossts,
-                               &iface->phc_mode, &sample_phc_ts, &sample_sys_ts,
+                                          &iface->phc_mode, PHC_READINGS, phc_readings);
-                               &phc_err)) {
+    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);

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),
+  n_readings = SYS_Linux_GetPHCReadings(phc->fd, phc->nocrossts, &phc->mode,
-                              &phc->mode, &phc_ts, &sys_ts, &phc_err))
+                                        PHC_READINGS, readings);
+  if (n_readings < 1)
+    return 0;
+
+  if (!HCL_ProcessReadings(phc->clock, n_readings, readings, &phc_ts, &sys_ts, &phc_err))
     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 (phc->extpps)
     return 0;
-  }
 
   DEBUG_LOG("PHC offset: %+.9f err: %.9f",
             UTI_DiffTimespecsToDouble(&phc_ts, &sys_ts), phc_err);

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,
+get_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3])
-                     struct timespec *phc_ts, struct timespec *sys_ts, double *err)
 {
-  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,
+get_extended_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3])
-                        struct timespec *sys_ts, double *err)
 {
 #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,
+get_precise_phc_readings(int phc_fd, int max_samples, struct timespec ts[][3])
-		       struct timespec *sys_ts, double *err)
 {
 #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;
+  ts[0][0].tv_sec = sys_off.sys_realtime.sec;
-  phc_ts->tv_nsec = sys_off.device.nsec;
+  ts[0][0].tv_nsec = sys_off.sys_realtime.nsec;
-  sys_ts->tv_sec = sys_off.sys_realtime.sec;
+  ts[0][1].tv_sec = sys_off.device.sec;
-  sys_ts->tv_nsec = sys_off.sys_realtime.nsec;
+  ts[0][1].tv_nsec = sys_off.device.nsec;
-  *err = MAX(LCL_GetSysPrecisionAsQuantum(), precision);
+  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,
+SYS_Linux_GetPHCReadings(int fd, int nocrossts, int *reading_mode, int max_readings,
-                       struct timespec *phc_ts, struct timespec *sys_ts, double *err)
+                         struct timespec tss[][3])
 {
-  if ((*reading_mode == 2 || !*reading_mode) && !nocrossts &&
+  int r = 0;
-      get_precise_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
+
+  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 == 0) &&
-  } else if ((*reading_mode == 3 || !*reading_mode) &&
+             (r = get_extended_phc_readings(fd, max_readings, tss)) > 0) {
-      get_extended_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
     *reading_mode = 3;
-    return 1;
+  } else if ((*reading_mode == 1 || *reading_mode == 0) &&
-  } else if ((*reading_mode == 1 || !*reading_mode) &&
+             (r = get_phc_readings(fd, max_readings, tss)) > 0) {
-      get_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
     *reading_mode = 1;
-    return 1;
   }
-  return 0;
+
+  return r;
 }
 
 /* ================================================== */

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,
+extern int SYS_Linux_GetPHCReadings(int fd, int nocrossts, int *reading_mode, int max_readings,
-                                  struct timespec *phc_ts, struct timespec *sys_ts, double *err);
+                                    struct timespec tss[][3]);
 
 extern int SYS_Linux_SetPHCExtTimestamping(int fd, int pin, int channel,
                                            int rising, int falling, int enable);

test/unit/hwclock.c

@@ -21,18 +21,21 @@
 #include <hwclock.c>
 #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;
+  double freq, jitter, interval, dj, err, sum;
-  int i, j, k, count;
+  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))
+        if (HCL_NeedsNewSample(clock, &local_ts)) {
-          HCL_AccumulateSample(clock, &hw_ts, &local_ts, 2.0 * jitter);
+          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;