ntp: add support for software timestamping on Linux

Enable SCM_TIMESTAMPING control messages and the socket's error queue in order to receive our transmitted packets with a more accurate transmit timestamp. Add a new file for Linux-specific NTP I/O and implement processing of these messages there.
2016-10-07 17:03:09 +02:00 · 2016-10-07 17:03:09 +02:00 · 997406fe47
commit 997406fe47
parent 14c8f07629
7 changed files with 374 additions and 10 deletions
--- a/21
+++ b/21
@ -101,6 +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
  --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]
@ -219,6 +220,8 @@ try_lockmem=0
 feat_asyncdns=1
 feat_forcednsretry=1
 try_clock_gettime=1
+feat_timestamping=1
+try_timestamping=0
 feat_ntp_signd=0
 ntp_era_split=""
 default_user="root"
@ -329,6 +332,9 @@ do
    --without-clock-gettime)
      try_clock_gettime=0
    ;;
+    --disable-timestamping)
+      feat_timestamping=0
+    ;;
    --enable-ntp-signd)
      feat_ntp_signd=1
    ;;
@ -387,6 +393,7 @@ case $OPERATINGSYSTEM in
        [ $try_libcap != "0" ] && try_libcap=1
        try_rtc=1
        [ $try_seccomp != "0" ] && try_seccomp=1
+        try_timestamping=1
        try_setsched=1
        try_lockmem=1
        try_phc=1
@ -459,6 +466,7 @@ if [ $feat_ntp = "1" ]; then
  fi
 else
  feat_asyncdns=0
+  feat_timestamping=0
 fi

 if [ "$feat_cmdmon" = "1" ] || [ $feat_ntp = "1" ]; then
@ -633,6 +641,19 @@ else
  fi
 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' '' '' '
+    int val = SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RX_SOFTWARE |
+              SOF_TIMESTAMPING_OPT_CMSG;
+    return sizeof (struct scm_timestamping) + SCM_TSTAMP_SND +
+           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"
+fi
+
 timepps_h=""
 if [ $feat_refclock = "1" ] && [ $feat_pps = "1" ]; then
  if test_code '<sys/timepps.h>' 'sys/timepps.h' '' '' ''; then
--- a/logging.h
+++ b/logging.h
@ -82,6 +82,7 @@ typedef enum {
 typedef enum {
  LOGF_Reference,
  LOGF_NtpIO,
+  LOGF_NtpIOLinux,
  LOGF_NtpCore,
  LOGF_NtpSignd,
  LOGF_NtpSources,
--- a/ntp_io.c
+++ b/ntp_io.c
@ -41,6 +41,10 @@
 #include "privops.h"
 #include "util.h"

+#ifdef HAVE_LINUX_TIMESTAMPING
+#include "ntp_io_linux.h"
+#endif
+
 #define INVALID_SOCK_FD -1
 #define CMSGBUF_SIZE 256

@ -118,8 +122,8 @@ prepare_socket(int family, int port_number, int client_only)
  socklen_t my_addr_len;
  int sock_fd;
  IPAddr bind_address;
-  int on_off = 1;
-  
+  int events = SCH_FILE_INPUT, on_off = 1;
+
  /* Open Internet domain UDP socket for NTP message transmissions */

  sock_fd = socket(family, SOCK_DGRAM, 0);
@ -212,6 +216,10 @@ prepare_socket(int family, int port_number, int client_only)
  }
 #endif

+#ifdef HAVE_LINUX_TIMESTAMPING
+  NIO_Linux_SetTimestampSocketOptions(sock_fd, client_only, &events);
+#endif
+
 #ifdef IP_FREEBIND
  /* Allow binding to address that doesn't exist yet */
  if (my_addr_len > 0 &&
@ -260,8 +268,8 @@ prepare_socket(int family, int port_number, int client_only)
    return INVALID_SOCK_FD;
  }

-  /* Register handler for read events on the socket */
-  SCH_AddFileHandler(sock_fd, SCH_FILE_INPUT, read_from_socket, NULL);
+  /* Register handler for read and possibly exception events on the socket */
+  SCH_AddFileHandler(sock_fd, events, read_from_socket, NULL);

  return sock_fd;
 }
@ -353,6 +361,10 @@ NIO_Initialise(int family)
  assert(!initialised);
  initialised = 1;

+#ifdef HAVE_LINUX_TIMESTAMPING
+  NIO_Linux_Initialise();
+#endif
+
  recv_messages = ARR_CreateInstance(sizeof (struct Message));
  ARR_SetSize(recv_messages, MAX_RECV_MESSAGES);
  recv_headers = ARR_CreateInstance(sizeof (struct MessageHeader));
@ -433,6 +445,11 @@ NIO_Finalise(void)
 #endif
  ARR_DestroyInstance(recv_headers);
  ARR_DestroyInstance(recv_messages);
+
+#ifdef HAVE_LINUX_TIMESTAMPING
+  NIO_Linux_Finalise();
+#endif
+
  initialised = 0;
 }

@ -621,6 +638,12 @@ process_message(struct msghdr *hdr, int length, int sock_fd)
 #endif
  }

+#ifdef HAVE_LINUX_TIMESTAMPING
+  if (NIO_Linux_ProcessMessage(&remote_addr, &local_addr, &local_ts,
+                               hdr, length, sock_fd))
+    return;
+#endif
+
  DEBUG_LOG(LOGF_NtpIO, "Received %d bytes from %s:%d to %s fd=%d tss=%d delay=%.9f",
            length, UTI_IPToString(&remote_addr.ip_addr), remote_addr.port,
            UTI_IPToString(&local_addr.ip_addr), local_addr.sock_fd, local_ts.source,
@ -644,19 +667,27 @@ read_from_socket(int sock_fd, int event, void *anything)

  struct MessageHeader *hdr;
  unsigned int i, n;
-  int status;
+  int status, flags = 0;

  hdr = ARR_GetElements(recv_headers);
  n = ARR_GetSize(recv_headers);
  assert(n >= 1);

+  if (event == SCH_FILE_EXCEPTION) {
+#ifdef HAVE_LINUX_TIMESTAMPING
+    flags |= MSG_ERRQUEUE;
+#else
+    assert(0);
+#endif
+  }
+
 #ifdef HAVE_RECVMMSG
-  status = recvmmsg(sock_fd, hdr, n, MSG_DONTWAIT, NULL);
+  status = recvmmsg(sock_fd, hdr, n, flags | MSG_DONTWAIT, NULL);
  if (status >= 0)
    n = status;
 #else
  n = 1;
-  status = recvmsg(sock_fd, &hdr[0].msg_hdr, 0);
+  status = recvmsg(sock_fd, &hdr[0].msg_hdr, flags);
  if (status >= 0)
    hdr[0].msg_len = status;
 #endif
@ -686,7 +717,7 @@ NIO_SendPacket(NTP_Packet *packet, NTP_Remote_Address *remote_addr,
  union sockaddr_in46 remote;
  struct msghdr msg;
  struct iovec iov;
-  struct cmsghdr cmsgbuf[CMSGBUF_SIZE / sizeof (struct cmsghdr)];
+  struct cmsghdr *cmsg, cmsgbuf[CMSGBUF_SIZE / sizeof (struct cmsghdr)];
  int cmsglen;
  socklen_t addrlen = 0;

@ -725,7 +756,6 @@ NIO_SendPacket(NTP_Packet *packet, NTP_Remote_Address *remote_addr,

 #ifdef HAVE_IN_PKTINFO
  if (local_addr->ip_addr.family == IPADDR_INET4) {
-    struct cmsghdr *cmsg;
    struct in_pktinfo *ipi;

    cmsg = CMSG_FIRSTHDR(&msg);
@ -743,7 +773,6 @@ NIO_SendPacket(NTP_Packet *packet, NTP_Remote_Address *remote_addr,

 #ifdef HAVE_IN6_PKTINFO
  if (local_addr->ip_addr.family == IPADDR_INET6) {
-    struct cmsghdr *cmsg;
    struct in6_pktinfo *ipi;

    cmsg = CMSG_FIRSTHDR(&msg);
@ -760,6 +789,11 @@ NIO_SendPacket(NTP_Packet *packet, NTP_Remote_Address *remote_addr,
  }
 #endif

+#ifdef HAVE_LINUX_TIMESTAMPING
+  if (process_tx)
+   cmsglen = NIO_Linux_RequestTxTimestamp(&msg, cmsglen, local_addr->sock_fd);
+#endif
+
  msg.msg_controllen = cmsglen;
  /* This is apparently required on some systems */
  if (!cmsglen)
--- a/ntp_io_linux.c
+++ b/ntp_io_linux.c
@ -0,0 +1,266 @@
+/*
+  chronyd/chronyc - Programs for keeping computer clocks accurate.
+
+ **********************************************************************
+ * Copyright (C) Miroslav Lichvar  2016
+ * 
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ * 
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * 
+ **********************************************************************
+
+  =======================================================================
+
+  Functions for NTP I/O specific to Linux
+  */
+
+#include "config.h"
+
+#include "sysincl.h"
+
+#include <linux/errqueue.h>
+#include <linux/ethtool.h>
+#include <linux/net_tstamp.h>
+#include <linux/sockios.h>
+
+#include "array.h"
+#include "conf.h"
+#include "local.h"
+#include "logging.h"
+#include "ntp_core.h"
+#include "ntp_io.h"
+#include "ntp_io_linux.h"
+#include "ntp_sources.h"
+#include "sched.h"
+#include "sys_linux.h"
+#include "util.h"
+
+union sockaddr_in46 {
+  struct sockaddr_in in4;
+#ifdef FEAT_IPV6
+  struct sockaddr_in6 in6;
+#endif
+  struct sockaddr u;
+};
+
+/* RX/TX and TX-specific timestamping socket options */
+static int ts_flags;
+static int ts_tx_flags;
+
+/* Flag indicating the socket options can't be changed in control messages */
+static int permanent_ts_options;
+
+/* ================================================== */
+
+void
+NIO_Linux_Initialise(void)
+{
+  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;
+
+  /* Kernels before 4.7 ignore timestamping flags set in control messages */
+  permanent_ts_options = !SYS_Linux_CheckKernelVersion(4, 7);
+}
+
+/* ================================================== */
+
+void
+NIO_Linux_Finalise(void)
+{
+}
+
+/* ================================================== */
+
+int
+NIO_Linux_SetTimestampSocketOptions(int sock_fd, int client_only, int *events)
+{
+  int val, flags;
+
+  if (!ts_flags)
+    return 0;
+
+  /* Enable SCM_TIMESTAMPING control messages and the socket's error queue in
+     order to receive our transmitted packets with more accurate timestamps */
+
+  val = 1;
+  flags = ts_flags;
+
+  if (client_only || permanent_ts_options)
+    flags |= ts_tx_flags;
+
+  if (setsockopt(sock_fd, SOL_SOCKET, SO_SELECT_ERR_QUEUE, &val, sizeof (val)) < 0) {
+    LOG(LOGS_ERR, LOGF_NtpIOLinux, "Could not set %s socket option", "SO_SELECT_ERR_QUEUE");
+    ts_flags = 0;
+    return 0;
+  }
+
+  if (setsockopt(sock_fd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof (flags)) < 0) {
+    LOG(LOGS_ERR, LOGF_NtpIOLinux, "Could not set %s socket option", "SO_TIMESTAMPING");
+    ts_flags = 0;
+    return 0;
+  }
+
+  *events |= SCH_FILE_EXCEPTION;
+  return 1;
+}
+
+/* ================================================== */
+/* Extract UDP data from a layer 2 message.  Supported is Ethernet
+   with optional VLAN tags. */
+
+static int
+extract_udp_data(unsigned char *msg, NTP_Remote_Address *remote_addr, int len)
+{
+  unsigned char *msg_start = msg;
+  union sockaddr_in46 addr;
+
+  remote_addr->ip_addr.family = IPADDR_UNSPEC;
+  remote_addr->port = 0;
+
+  /* Skip MACs */
+  if (len < 12)
+    return 0;
+  len -= 12, msg += 12;
+
+  /* Skip VLAN tag(s) if present */
+  while (len >= 4 && msg[0] == 0x81 && msg[1] == 0x00)
+    len -= 4, msg += 4;
+
+  /* Skip IPv4 or IPv6 ethertype */
+  if (len < 2 || !((msg[0] == 0x08 && msg[1] == 0x00) ||
+                   (msg[0] == 0x86 && msg[1] == 0xdd)))
+    return 0;
+  len -= 2, msg += 2;
+
+  /* Parse destination address and port from IPv4/IPv6 and UDP headers */
+  if (len >= 20 && msg[0] >> 4 == 4) {
+    int ihl = (msg[0] & 0xf) * 4;
+
+    if (len < ihl + 8 || msg[9] != 17)
+      return 0;
+
+    memcpy(&addr.in4.sin_addr.s_addr, msg + 16, sizeof (uint32_t));
+    addr.in4.sin_port = *(uint16_t *)(msg + ihl + 2);
+    addr.in4.sin_family = AF_INET;
+    len -= ihl + 8, msg += ihl + 8;
+#ifdef FEAT_IPV6
+  } else if (len >= 48 && msg[0] >> 4 == 6) {
+    /* IPv6 extension headers are not supported */
+    if (msg[6] != 17)
+      return 0;
+
+    memcpy(&addr.in6.sin6_addr.s6_addr, msg + 24, 16);
+    addr.in6.sin6_port = *(uint16_t *)(msg + 40 + 2);
+    addr.in6.sin6_family = AF_INET6;
+    len -= 48, msg += 48;
+#endif
+  } else {
+    return 0;
+  }
+
+  UTI_SockaddrToIPAndPort(&addr.u, &remote_addr->ip_addr, &remote_addr->port);
+
+  /* Move the message to fix alignment of its fields */
+  if (len > 0)
+    memmove(msg_start, msg, len);
+
+  return len;
+}
+
+/* ================================================== */
+
+int
+NIO_Linux_ProcessMessage(NTP_Remote_Address *remote_addr, NTP_Local_Address *local_addr,
+                         NTP_Local_Timestamp *local_ts, struct msghdr *hdr,
+                         int length, int sock_fd)
+{
+  struct cmsghdr *cmsg;
+
+  for (cmsg = CMSG_FIRSTHDR(hdr); cmsg; cmsg = CMSG_NXTHDR(hdr, cmsg)) {
+    if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) {
+      struct scm_timestamping ts3;
+
+      memcpy(&ts3, CMSG_DATA(cmsg), sizeof (ts3));
+
+      if (!UTI_IsZeroTimespec(&ts3.ts[0])) {
+        LCL_CookTime(&ts3.ts[0], &local_ts->ts, &local_ts->err);
+        local_ts->source = NTP_TS_KERNEL;
+      }
+    }
+
+    if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) ||
+        (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) {
+      struct sock_extended_err err;
+
+      memcpy(&err, CMSG_DATA(cmsg), sizeof (err));
+
+      if (err.ee_errno != ENOMSG || err.ee_info != SCM_TSTAMP_SND ||
+          err.ee_origin != SO_EE_ORIGIN_TIMESTAMPING) {
+        DEBUG_LOG(LOGF_NtpIOLinux, "Unknown extended error");
+        /* Drop the message */
+        return 1;
+      }
+    }
+  }
+
+  /* Return the message if it's not received from the error queue */
+  if (!(hdr->msg_flags & MSG_ERRQUEUE))
+    return 0;
+
+  /* The data from the error queue includes all layers up to UDP.  We have to
+     extract the UDP data and also the destination address with port as there
+     currently doesn't seem to be a better way to get them both. */
+  length = extract_udp_data(hdr->msg_iov[0].iov_base, remote_addr, length);
+
+  DEBUG_LOG(LOGF_NtpIOLinux, "Received %d bytes from error queue for %s:%d fd=%d tss=%d",
+            length, UTI_IPToString(&remote_addr->ip_addr), remote_addr->port,
+            sock_fd, local_ts->source);
+
+  if (length < NTP_NORMAL_PACKET_LENGTH)
+    return 1;
+
+  NSR_ProcessTx(remote_addr, local_addr, local_ts,
+                (NTP_Packet *)hdr->msg_iov[0].iov_base, length);
+
+  return 1;
+}
+
+/* ================================================== */
+
+int
+NIO_Linux_RequestTxTimestamp(struct msghdr *msg, int cmsglen, int sock_fd)
+{
+  struct cmsghdr *cmsg;
+
+  /* Check if TX timestamping is disabled on this socket */
+  if (permanent_ts_options || !NIO_IsServerSocket(sock_fd))
+    return cmsglen;
+
+  /* Add control message that will enable TX timestamping for this message.
+     Don't use CMSG_NXTHDR as the one in glibc is buggy for creating new
+     control messages. */
+  cmsg = (struct cmsghdr *)((char *)CMSG_FIRSTHDR(msg) + cmsglen);
+  memset(cmsg, 0, CMSG_SPACE(sizeof (ts_tx_flags)));
+  cmsglen += CMSG_SPACE(sizeof (ts_tx_flags));
+
+  cmsg->cmsg_level = SOL_SOCKET;
+  cmsg->cmsg_type = SO_TIMESTAMPING;
+  cmsg->cmsg_len = CMSG_LEN(sizeof (ts_tx_flags));
+
+  memcpy(CMSG_DATA(cmsg), &ts_tx_flags, sizeof (ts_tx_flags));
+
+  return cmsglen;
+}
--- a/ntp_io_linux.h
+++ b/ntp_io_linux.h
@ -0,0 +1,37 @@
+/*
+  chronyd/chronyc - Programs for keeping computer clocks accurate.
+
+ **********************************************************************
+ * Copyright (C) Miroslav Lichvar  2016
+ * 
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ * 
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ * 
+ **********************************************************************
+
+  =======================================================================
+
+  This is the header file for the Linux-specific NTP socket I/O bits.
+  */
+
+extern void NIO_Linux_Initialise(void);
+
+extern void NIO_Linux_Finalise(void);
+
+extern int NIO_Linux_SetTimestampSocketOptions(int sock_fd, int client_only, int *events);
+
+extern int NIO_Linux_ProcessMessage(NTP_Remote_Address *remote_addr, NTP_Local_Address *local_addr,
+                                    NTP_Local_Timestamp *local_ts, struct msghdr *hdr, int length,
+                                    int sock_fd);
+
+extern int NIO_Linux_RequestTxTimestamp(struct msghdr *msg, int cmsglen, int sock_fd);
--- a/sys_linux.c
+++ b/sys_linux.c
@ -501,6 +501,9 @@ SYS_Linux_EnableSystemCallFilter(int level)
 #endif
    { SOL_SOCKET, SO_BROADCAST }, { SOL_SOCKET, SO_REUSEADDR },
    { SOL_SOCKET, SO_TIMESTAMP }, { SOL_SOCKET, SO_TIMESTAMPNS },
+#ifdef HAVE_LINUX_TIMESTAMPING
+    { SOL_SOCKET, SO_SELECT_ERR_QUEUE }, { SOL_SOCKET, SO_TIMESTAMPING },
+#endif
  };

  const static int fcntls[] = { F_GETFD, F_SETFD };
--- a/test/compilation/001-features
+++ b/test/compilation/001-features
@ -17,6 +17,8 @@ for opts in \
 	"--disable-cmdmon" \
 	"--disable-ntp" \
 	"--disable-refclock" \
+	"--disable-timestamping" \
+	"--disable-timestamping --disable-ntp" \
 	"--disable-cmdmon --disable-ntp" \
 	"--disable-cmdmon --disable-refclock" \
 	"--disable-cmdmon --disable-ntp --disable-refclock"