ntp: move PHC-specific code to sys_linux

This will allow sharing of the code with the PHC refclock driver.

ntp_io_linux.c

@@ -61,6 +61,7 @@ struct Interface {
   char name[IF_NAMESIZE];
   int if_index;
   int phc_fd;
+  int phc_mode;
   /* Link speed in mbit/s */
   int link_speed;
   /* Start of UDP data at layer 2 for IPv4 and IPv6 */
@@ -101,10 +102,9 @@ add_interface(CNF_HwTsInterface *conf_iface)
   struct ethtool_ts_info ts_info;
   struct hwtstamp_config ts_config;
   struct ifreq req;
-  int sock_fd, if_index, phc_index, phc_fd;
+  int sock_fd, if_index, phc_fd;
   unsigned int i;
   struct Interface *iface;
-  char phc_path[64];
 
   /* Check if the interface was not already added */
   for (i = 0; i < ARR_GetSize(interfaces); i++) {
@@ -154,24 +154,17 @@ add_interface(CNF_HwTsInterface *conf_iface)
   }
 
   close(sock_fd);
-  phc_index = ts_info.phc_index;
 
-  if (snprintf(phc_path, sizeof (phc_path), "/dev/ptp%d", phc_index) >= sizeof (phc_path))
+  phc_fd = SYS_Linux_OpenPHC(NULL, ts_info.phc_index);
+  if (phc_fd < 0)
     return 0;
 
-  phc_fd = open(phc_path, O_RDONLY);
-  if (phc_fd < 0) {
-    LOG(LOGS_ERR, LOGF_NtpIOLinux, "Could not open %s : %s", phc_path, strerror(errno));
-    return 0;
-  }
-
-  UTI_FdSetCloexec(phc_fd);
-
   iface = ARR_GetNewElement(interfaces);
 
   snprintf(iface->name, sizeof (iface->name), "%s", conf_iface->name);
   iface->if_index = if_index;
   iface->phc_fd = phc_fd;
+  iface->phc_mode = 0;
 
   /* Start with 1 gbit and no VLANs or IPv4/IPv6 options */
   iface->link_speed = 1000;
@@ -347,69 +340,6 @@ NIO_Linux_SetTimestampSocketOptions(int sock_fd, int client_only, int *events)
 
 /* ================================================== */
 
-static int
-get_phc_sample(struct Interface *iface, struct timespec *phc_ts, struct timespec *local_ts,
-               double *err)
-{
-  struct ptp_sys_offset sys_off;
-  struct timespec ts1, ts2, ts3, phc_tss[PHC_READINGS], sys_tss[PHC_READINGS];
-  double min_delay = 0.0, delays[PHC_READINGS], phc_sum, local_sum, local_prec;
-  int i, n;
-
-  /* Silence valgrind */
-  memset(&sys_off, 0, sizeof (sys_off));
-
-  sys_off.n_samples = PHC_READINGS;
-
-  if (ioctl(iface->phc_fd, PTP_SYS_OFFSET, &sys_off)) {
-    DEBUG_LOG(LOGF_NtpIOLinux, "ioctl(%s) failed : %s", "PTP_SYS_OFFSET", strerror(errno));
-    return 0;
-  }
-
-  for (i = 0; i < PHC_READINGS; i++) {
-    ts1.tv_sec = sys_off.ts[i * 2].sec;
-    ts1.tv_nsec = sys_off.ts[i * 2].nsec;
-    ts2.tv_sec = sys_off.ts[i * 2 + 1].sec;
-    ts2.tv_nsec = sys_off.ts[i * 2 + 1].nsec;
-    ts3.tv_sec = sys_off.ts[i * 2 + 2].sec;
-    ts3.tv_nsec = sys_off.ts[i * 2 + 2].nsec;
-
-    sys_tss[i] = ts1;
-    phc_tss[i] = ts2;
-    delays[i] = UTI_DiffTimespecsToDouble(&ts3, &ts1);
-
-    if (delays[i] <= 0.0)
-      /* Step in the middle of a PHC reading? */
-      return 0;
-
-    if (!i || delays[i] < min_delay)
-      min_delay = delays[i];
-  }
-
-  local_prec = LCL_GetSysPrecisionAsQuantum();
-
-  /* Combine best readings */
-  for (i = n = 0, phc_sum = local_sum = 0.0; i < PHC_READINGS; i++) {
-    if (delays[i] > min_delay + MAX(local_prec, iface->precision))
-      continue;
-
-    phc_sum += UTI_DiffTimespecsToDouble(&phc_tss[i], &phc_tss[0]);
-    local_sum += UTI_DiffTimespecsToDouble(&sys_tss[i], &sys_tss[0]) + delays[i] / 2.0;
-    n++;
-  }
-
-  assert(n);
-
-  UTI_AddDoubleToTimespec(&phc_tss[0], phc_sum / n, phc_ts);
-  UTI_AddDoubleToTimespec(&sys_tss[0], local_sum / n, &ts1);
-  LCL_CookTime(&ts1, local_ts, NULL);
-  *err = MAX(min_delay / 2.0, iface->precision);
-
-  return 1;
-}
-
-/* ================================================== */
-
 static struct Interface *
 get_interface(int if_index)
 {
@@ -433,14 +363,16 @@ static void
 process_hw_timestamp(struct Interface *iface, struct timespec *hw_ts,
                      NTP_Local_Timestamp *local_ts, int rx_ntp_length, int family)
 {
-  struct timespec sample_phc_ts, sample_local_ts, ts;
+  struct timespec sample_phc_ts, sample_sys_ts, sample_local_ts, ts;
   double rx_correction, ts_delay, err;
   int l2_length;
 
   if (HCL_NeedsNewSample(iface->clock, &local_ts->ts)) {
-    if (!get_phc_sample(iface, &sample_phc_ts, &sample_local_ts, &err))
+    if (!SYS_Linux_GetPHCSample(iface->phc_fd, iface->precision, &iface->phc_mode,
+                                &sample_phc_ts, &sample_sys_ts, &err))
       return;
 
+    LCL_CookTime(&sample_sys_ts, &sample_local_ts, NULL);
     HCL_AccumulateSample(iface->clock, &sample_phc_ts, &sample_local_ts, err);
 
     update_interface_speed(iface);

sys_linux.c

@@ -47,13 +47,14 @@
 #include <sys/capability.h>
 #endif
 
+#if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING)
+#include <linux/ptp_clock.h>
+#endif
+
 #ifdef FEAT_SCFILTER
 #include <sys/prctl.h>
 #include <seccomp.h>
 #include <termios.h>
-#if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING)
-#include <linux/ptp_clock.h>
-#endif
 #ifdef FEAT_PPS
 #include <linux/pps.h>
 #endif
@@ -68,6 +69,7 @@
 #include "sys_linux.h"
 #include "sys_timex.h"
 #include "conf.h"
+#include "local.h"
 #include "logging.h"
 #include "privops.h"
 #include "util.h"
@@ -661,3 +663,118 @@ SYS_Linux_CheckKernelVersion(int req_major, int req_minor)
 
   return kernelvercmp(req_major, req_minor, 0, major, minor, patch) <= 0;
 }
+
+/* ================================================== */
+
+#if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING)
+
+#define PHC_READINGS 10
+
+static int
+get_phc_sample(int phc_fd, double precision, struct timespec *phc_ts,
+               struct timespec *sys_ts, double *err)
+{
+  struct ptp_sys_offset sys_off;
+  struct timespec ts1, ts2, ts3, phc_tss[PHC_READINGS], sys_tss[PHC_READINGS];
+  double min_delay = 0.0, delays[PHC_READINGS], phc_sum, sys_sum, sys_prec;
+  int i, n;
+
+  /* Silence valgrind */
+  memset(&sys_off, 0, sizeof (sys_off));
+
+  sys_off.n_samples = PHC_READINGS;
+
+  if (ioctl(phc_fd, PTP_SYS_OFFSET, &sys_off)) {
+    DEBUG_LOG(LOGF_SysLinux, "ioctl(%s) failed : %s", "PTP_SYS_OFFSET", strerror(errno));
+    return 0;
+  }
+
+  for (i = 0; i < PHC_READINGS; i++) {
+    ts1.tv_sec = sys_off.ts[i * 2].sec;
+    ts1.tv_nsec = sys_off.ts[i * 2].nsec;
+    ts2.tv_sec = sys_off.ts[i * 2 + 1].sec;
+    ts2.tv_nsec = sys_off.ts[i * 2 + 1].nsec;
+    ts3.tv_sec = sys_off.ts[i * 2 + 2].sec;
+    ts3.tv_nsec = sys_off.ts[i * 2 + 2].nsec;
+
+    sys_tss[i] = ts1;
+    phc_tss[i] = ts2;
+    delays[i] = UTI_DiffTimespecsToDouble(&ts3, &ts1);
+
+    if (delays[i] <= 0.0)
+      /* Step in the middle of a PHC reading? */
+      return 0;
+
+    if (!i || delays[i] < min_delay)
+      min_delay = delays[i];
+  }
+
+  sys_prec = LCL_GetSysPrecisionAsQuantum();
+
+  /* Combine best readings */
+  for (i = n = 0, phc_sum = sys_sum = 0.0; i < PHC_READINGS; i++) {
+    if (delays[i] > min_delay + MAX(sys_prec, precision))
+      continue;
+
+    phc_sum += UTI_DiffTimespecsToDouble(&phc_tss[i], &phc_tss[0]);
+    sys_sum += UTI_DiffTimespecsToDouble(&sys_tss[i], &sys_tss[0]) + delays[i] / 2.0;
+    n++;
+  }
+
+  assert(n);
+
+  UTI_AddDoubleToTimespec(&phc_tss[0], phc_sum / n, phc_ts);
+  UTI_AddDoubleToTimespec(&sys_tss[0], sys_sum / n, sys_ts);
+  *err = MAX(min_delay / 2.0, precision);
+
+  return 1;
+}
+
+/* ================================================== */
+
+int
+SYS_Linux_OpenPHC(const char *path, int phc_index)
+{
+  struct ptp_clock_caps caps;
+  char phc_path[64];
+  int phc_fd;
+
+  if (!path) {
+    if (snprintf(phc_path, sizeof (phc_path), "/dev/ptp%d", phc_index) >= sizeof (phc_path))
+      return -1;
+    path = phc_path;
+  }
+
+  phc_fd = open(path, O_RDONLY);
+  if (phc_fd < 0) {
+    LOG(LOGS_ERR, LOGF_SysLinux, "Could not open %s : %s", path, strerror(errno));
+    return -1;
+  }
+
+  /* Make sure it is a PHC */
+  if (ioctl(phc_fd, PTP_CLOCK_GETCAPS, &caps)) {
+    LOG(LOGS_ERR, LOGF_SysLinux, "ioctl(%s) failed : %s", "PTP_CLOCK_GETCAPS", strerror(errno));
+    close(phc_fd);
+    return -1;
+  }
+
+  UTI_FdSetCloexec(phc_fd);
+
+  return phc_fd;
+}
+
+/* ================================================== */
+
+int
+SYS_Linux_GetPHCSample(int fd, double precision, int *reading_mode,
+                       struct timespec *phc_ts, struct timespec *sys_ts, double *err)
+{
+  if ((*reading_mode == 1 || !*reading_mode) &&
+      get_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
+    *reading_mode = 1;
+    return 1;
+  }
+  return 0;
+}
+
+#endif

sys_linux.h

@@ -41,4 +41,9 @@ extern void SYS_Linux_SetScheduler(int SchedPriority);
 
 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, double precision, int *reading_mode,
+                                  struct timespec *phc_ts, struct timespec *sys_ts, double *err);
+
 #endif  /* GOT_SYS_LINUX_H */