ntp: add support for HW timestamping on Linux

Add a new directive to specify interfaces which should be used for HW
timestamping. Extend the Linux ntp_io initialization to enable HW
timestamping, configure the RX filter using the SIOCSHWTSTAMP ioctl,
open their PHC devices, and track them as hwclock instances. When
messages with HW timestamps are received, use the PTP_SYS_OFFSET ioctl
to make PHC samples for hwclock.

conf.c

@@ -57,6 +57,7 @@ static void parse_bindcmdaddress(char *);
 static void parse_broadcast(char *);
 static void parse_clientloglimit(char *);
 static void parse_fallbackdrift(char *);
+static void parse_hwtimestamp(char *);
 static void parse_include(char *);
 static void parse_initstepslew(char *);
 static void parse_leapsecmode(char *);
@@ -221,6 +222,9 @@ static char *leapsec_tz = NULL;
 /* Name of the user to which will be dropped root privileges. */
 static char *user;
 
+/* Array of strings for interfaces with HW timestamping */
+static ARR_Instance hwts_interfaces;
+
 typedef struct {
   NTP_Source_Type type;
   int pool;
@@ -322,6 +326,8 @@ CNF_Initialise(int r)
 {
   restarted = r;
 
+  hwts_interfaces = ARR_CreateInstance(sizeof (char *));
+
   init_sources = ARR_CreateInstance(sizeof (IPAddr));
   ntp_sources = ARR_CreateInstance(sizeof (NTP_Source));
   refclock_sources = ARR_CreateInstance(sizeof (RefclockParameters));
@@ -346,6 +352,10 @@ CNF_Finalise(void)
 {
   unsigned int i;
 
+  for (i = 0; i < ARR_GetSize(hwts_interfaces); i++)
+    Free(*(char **)ARR_GetElement(hwts_interfaces, i));
+  ARR_DestroyInstance(hwts_interfaces);
+
   for (i = 0; i < ARR_GetSize(ntp_sources); i++)
     Free(((NTP_Source *)ARR_GetElement(ntp_sources, i))->params.name);
 
@@ -462,6 +472,8 @@ CNF_ParseLine(const char *filename, int number, char *line)
     parse_fallbackdrift(p);
   } else if (!strcasecmp(command, "hwclockfile")) {
     parse_string(p, &hwclock_file);
+  } else if (!strcasecmp(command, "hwtimestamp")) {
+    parse_hwtimestamp(p);
   } else if (!strcasecmp(command, "include")) {
     parse_include(p);
   } else if (!strcasecmp(command, "initstepslew")) {
@@ -1221,6 +1233,15 @@ parse_tempcomp(char *line)
 
 /* ================================================== */
 
+static void
+parse_hwtimestamp(char *line)
+{
+  check_number_of_args(line, 1);
+  *(char **)ARR_GetNewElement(hwts_interfaces) = Strdup(line);
+}
+
+/* ================================================== */
+
 static void
 parse_include(char *line)
 {
@@ -1889,3 +1910,11 @@ CNF_GetInitStepThreshold(void)
 {
   return init_slew_threshold;
 }
+
+/* ================================================== */
+
+ARR_Instance
+CNF_GetHwTsInterfaces(void)
+{
+  return hwts_interfaces;
+}

conf.h

@@ -29,6 +29,7 @@
 #define GOT_CONF_H
 
 #include "addressing.h"
+#include "array.h"
 #include "reference.h"
 
 extern void CNF_Initialise(int restarted);
@@ -118,4 +119,6 @@ extern char *CNF_GetHwclockFile(void);
 extern int CNF_GetInitSources(void);
 extern double CNF_GetInitStepThreshold(void);
 
+extern ARR_Instance CNF_GetHwTsInterfaces(void);
+
 #endif /* GOT_CONF_H */

configure

@@ -101,7 +101,7 @@ For better control, use the options below.
   --disable-asyncdns     Disable asynchronous name resolving
   --disable-forcednsretry Don't retry on permanent DNS error
   --without-clock-gettime Don't use clock_gettime() even if it is available
-  --disable-timestamping Disable support for SW timestamping
+  --disable-timestamping Disable support for SW/HW timestamping
   --enable-ntp-signd     Enable support for MS-SNTP authentication in Samba
   --with-ntp-era=SECONDS Specify earliest assumed NTP time in seconds
                          since 1970-01-01 [50*365 days ago]
@@ -642,16 +642,16 @@ else
 fi
 
 if [ $feat_timestamping = "1" ] && [ $try_timestamping = "1" ] &&
-  test_code 'SW timestamping' 'sys/types.h sys/socket.h linux/net_tstamp.h
-                               linux/errqueue.h' '' '' '
+  test_code 'SW/HW timestamping' 'sys/types.h sys/socket.h linux/net_tstamp.h
+                                  linux/errqueue.h linux/ptp_clock.h' '' '' '
     int val = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE |
-              SOF_TIMESTAMPING_OPT_CMSG;
-    return sizeof (struct scm_timestamping) + SCM_TSTAMP_SND +
+              SOF_TIMESTAMPING_RAW_HARDWARE | SOF_TIMESTAMPING_OPT_CMSG;
+    return sizeof (struct scm_timestamping) + SCM_TSTAMP_SND + PTP_SYS_OFFSET +
            setsockopt(0, SOL_SOCKET, SO_SELECT_ERR_QUEUE + SO_TIMESTAMPING,
                       &val, sizeof (val));'
 then
   add_def HAVE_LINUX_TIMESTAMPING
-  EXTRA_OBJECTS="$EXTRA_OBJECTS ntp_io_linux.o"
+  EXTRA_OBJECTS="$EXTRA_OBJECTS hwclock.o ntp_io_linux.o"
 fi
 
 timepps_h=""

ntp_io.c

@@ -363,6 +363,9 @@ NIO_Initialise(int family)
 
 #ifdef HAVE_LINUX_TIMESTAMPING
   NIO_Linux_Initialise();
+#else
+  if (ARR_GetSize(CNF_GetHwTsInterfaces()))
+    LOG_FATAL(LOGF_NtpIO, "HW timestamping not supported");
 #endif
 
   recv_messages = ARR_CreateInstance(sizeof (struct Message));

ntp_io_linux.c

@@ -31,10 +31,13 @@
 #include <linux/errqueue.h>
 #include <linux/ethtool.h>
 #include <linux/net_tstamp.h>
+#include <linux/ptp_clock.h>
 #include <linux/sockios.h>
+#include <net/if.h>
 
 #include "array.h"
 #include "conf.h"
+#include "hwclock.h"
 #include "local.h"
 #include "logging.h"
 #include "ntp_core.h"
@@ -53,6 +56,18 @@ union sockaddr_in46 {
   struct sockaddr u;
 };
 
+struct Interface {
+  int if_index;
+  int phc_fd;
+  HCL_Instance clock;
+};
+
+/* Number of PHC readings per HW clock sample */
+#define PHC_READINGS 10
+
+/* Array of Interfaces */
+static ARR_Instance interfaces;
+
 /* RX/TX and TX-specific timestamping socket options */
 static int ts_flags;
 static int ts_tx_flags;
@@ -62,11 +77,105 @@ static int permanent_ts_options;
 
 /* ================================================== */
 
+static int
+add_interface(const char *name)
+{
+  struct ethtool_ts_info ts_info;
+  struct hwtstamp_config ts_config;
+  struct ifreq req;
+  int sock_fd, if_index, phc_index, phc_fd;
+  struct Interface *iface;
+  char phc_path[64];
+
+  sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (sock_fd < 0)
+    return 0;
+
+  memset(&req, 0, sizeof (req));
+  memset(&ts_info, 0, sizeof (ts_info));
+
+  if (snprintf(req.ifr_name, sizeof (req.ifr_name), "%s", name) >= sizeof (req.ifr_name)) {
+    close(sock_fd);
+    return 0;
+  }
+
+  if (ioctl(sock_fd, SIOCGIFINDEX, &req)) {
+    DEBUG_LOG(LOGF_NtpIOLinux, "ioctl(%s) failed : %s", "SIOCGIFINDEX", strerror(errno));
+    close(sock_fd);
+    return 0;
+  }
+
+  if_index = req.ifr_ifindex;
+
+  ts_info.cmd = ETHTOOL_GET_TS_INFO;
+  req.ifr_data = (char *)&ts_info;
+
+  if (ioctl(sock_fd, SIOCETHTOOL, &req)) {
+    DEBUG_LOG(LOGF_NtpIOLinux, "ioctl(%s) failed : %s", "SIOCETHTOOL", strerror(errno));
+    close(sock_fd);
+    return 0;
+  }
+
+  ts_config.flags = 0;
+  ts_config.tx_type = HWTSTAMP_TX_ON;
+  ts_config.rx_filter = HWTSTAMP_FILTER_ALL;
+  req.ifr_data = (char *)&ts_config;
+
+  if (ioctl(sock_fd, SIOCSHWTSTAMP, &req)) {
+    DEBUG_LOG(LOGF_NtpIOLinux, "ioctl(%s) failed : %s", "SIOCSHWTSTAMP", strerror(errno));
+    close(sock_fd);
+    return 0;
+  }
+
+  close(sock_fd);
+  phc_index = ts_info.phc_index;
+
+  if (snprintf(phc_path, sizeof (phc_path), "/dev/ptp%d", phc_index) >= sizeof (phc_path))
+    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);
+  iface->if_index = if_index;
+  iface->phc_fd = phc_fd;
+  iface->clock = HCL_CreateInstance();
+
+  return 1;
+}
+
+/* ================================================== */
+
 void
 NIO_Linux_Initialise(void)
 {
-  ts_flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE;
-  ts_tx_flags = SOF_TIMESTAMPING_TX_SOFTWARE;
+  ARR_Instance config_hwts_ifaces;
+  char *if_name;
+  unsigned int i;
+
+  interfaces = ARR_CreateInstance(sizeof (struct Interface));
+
+  config_hwts_ifaces = CNF_GetHwTsInterfaces();
+
+  /* Enable HW timestamping on all specified interfaces.  If no interface was
+     specified, use SW timestamping. */
+  if (ARR_GetSize(config_hwts_ifaces)) {
+    for (i = 0; i < ARR_GetSize(config_hwts_ifaces); i++) {
+      if_name = *(char **)ARR_GetElement(config_hwts_ifaces, i);
+      if (!add_interface(if_name))
+        LOG_FATAL(LOGF_NtpIO, "Could not enable HW timestamping on %s", if_name);
+    }
+    ts_flags = SOF_TIMESTAMPING_RAW_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE;
+    ts_tx_flags = SOF_TIMESTAMPING_TX_HARDWARE;
+  } else {
+    ts_flags = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE;
+    ts_tx_flags = SOF_TIMESTAMPING_TX_SOFTWARE;
+  }
 
   /* Enable IP_PKTINFO in messages looped back to the error queue */
   ts_flags |= SOF_TIMESTAMPING_OPT_CMSG;
@@ -80,6 +189,16 @@ NIO_Linux_Initialise(void)
 void
 NIO_Linux_Finalise(void)
 {
+  struct Interface *iface;
+  unsigned int i;
+
+  for (i = 0; i < ARR_GetSize(interfaces); i++) {
+    iface = ARR_GetElement(interfaces, i);
+    HCL_DestroyInstance(iface->clock);
+    close(iface->phc_fd);
+  }
+
+  ARR_DestroyInstance(interfaces);
 }
 
 /* ================================================== */
@@ -117,6 +236,99 @@ NIO_Linux_SetTimestampSocketOptions(int sock_fd, int client_only, int *events)
   return 1;
 }
 
+/* ================================================== */
+
+static int
+get_phc_sample(int phc_fd, struct timespec *phc_ts, struct timespec *local_ts, double *p_delay)
+{
+  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(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 + local_prec)
+      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], phc_sum / n, &ts1);
+  LCL_CookTime(&ts1, local_ts, NULL);
+  *p_delay = min_delay;
+
+  return 1;
+}
+
+/* ================================================== */
+
+static int
+process_hw_timestamp(int if_index, struct timespec *hw_ts, NTP_Local_Timestamp *local_ts)
+{
+  struct timespec sample_phc_ts, sample_local_ts;
+  double sample_delay;
+  struct Interface *iface;
+  unsigned int i;
+
+  for (i = 0; i < ARR_GetSize(interfaces); i++) {
+    iface = ARR_GetElement(interfaces, i);
+    if (iface->if_index != if_index)
+      continue;
+
+    if (HCL_NeedsNewSample(iface->clock, &local_ts->ts)) {
+      if (!get_phc_sample(iface->phc_fd, &sample_phc_ts, &sample_local_ts, &sample_delay))
+        return 0;
+
+      HCL_AccumulateSample(iface->clock, &sample_phc_ts, &sample_local_ts,
+                           sample_delay / 2.0);
+    }
+
+    return HCL_CookTime(iface->clock, hw_ts, &local_ts->ts, &local_ts->err);
+  }
+
+  DEBUG_LOG(LOGF_NtpIOLinux, "HW clock not found for interface %d", if_index);
+
+  return 0;
+}
+
 /* ================================================== */
 /* Extract UDP data from a layer 2 message.  Supported is Ethernet
    with optional VLAN tags. */
@@ -198,6 +410,8 @@ NIO_Linux_ProcessMessage(NTP_Remote_Address *remote_addr, NTP_Local_Address *loc
       if (!UTI_IsZeroTimespec(&ts3.ts[0])) {
         LCL_CookTime(&ts3.ts[0], &local_ts->ts, &local_ts->err);
         local_ts->source = NTP_TS_KERNEL;
+      } else if (process_hw_timestamp(if_index, &ts3.ts[2], local_ts)) {
+        local_ts->source = NTP_TS_HARDWARE;
       }
     }
 
@@ -229,6 +443,12 @@ NIO_Linux_ProcessMessage(NTP_Remote_Address *remote_addr, NTP_Local_Address *loc
             length, UTI_IPToString(&remote_addr->ip_addr), remote_addr->port,
             sock_fd, if_index, local_ts->source);
 
+  /* Drop the message if HW timestamp is missing or its processing failed */
+  if ((ts_flags & SOF_TIMESTAMPING_RAW_HARDWARE) && local_ts->source != NTP_TS_HARDWARE) {
+    DEBUG_LOG(LOGF_NtpIOLinux, "Missing HW timestamp");
+    return 1;
+  }
+
   if (length < NTP_NORMAL_PACKET_LENGTH)
     return 1;