From 997406fe47d8eaa65f29bae25709023c0db66281 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Fri, 7 Oct 2016 17:03:09 +0200 Subject: [PATCH] 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. --- configure | 21 +++ logging.h | 1 + ntp_io.c | 54 +++++-- ntp_io_linux.c | 266 ++++++++++++++++++++++++++++++++++ ntp_io_linux.h | 37 +++++ sys_linux.c | 3 + test/compilation/001-features | 2 + 7 files changed, 374 insertions(+), 10 deletions(-) create mode 100644 ntp_io_linux.c create mode 100644 ntp_io_linux.h diff --git a/configure b/configure index f2486b2..e2e1138 100755 --- a/configure +++ b/configure @@ -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' '' '' ''; then diff --git a/logging.h b/logging.h index f3ebb47..41d640e 100644 --- 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, diff --git a/ntp_io.c b/ntp_io.c index 07f6a00..3253448 100644 --- 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) diff --git a/ntp_io_linux.c b/ntp_io_linux.c new file mode 100644 index 0000000..187bbc0 --- /dev/null +++ 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 +#include +#include +#include + +#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; +} diff --git a/ntp_io_linux.h b/ntp_io_linux.h new file mode 100644 index 0000000..a70ac1b --- /dev/null +++ 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); diff --git a/sys_linux.c b/sys_linux.c index 8020f77..c89726d 100644 --- 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 }; diff --git a/test/compilation/001-features b/test/compilation/001-features index 783e601..d61c07f 100755 --- 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"