From 14a1059e43d04cb3865fb96ba17b326f2c7bdb88 Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Mon, 24 Oct 2016 12:44:59 +0200 Subject: [PATCH] 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 | 29 +++++++ conf.h | 3 + configure | 12 +-- ntp_io.c | 3 + ntp_io_linux.c | 224 ++++++++++++++++++++++++++++++++++++++++++++++++- 5 files changed, 263 insertions(+), 8 deletions(-) diff --git a/conf.c b/conf.c index a88df6f..c255c24 100644 --- a/conf.c +++ b/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; +} diff --git a/conf.h b/conf.h index ae5a890..13e58b5 100644 --- a/conf.h +++ b/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 */ diff --git a/configure b/configure index e2e1138..67b1944 100755 --- a/configure +++ b/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="" diff --git a/ntp_io.c b/ntp_io.c index 80b062c..7660eb6 100644 --- a/ntp_io.c +++ b/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)); diff --git a/ntp_io_linux.c b/ntp_io_linux.c index 0eaa162..6025de5 100644 --- a/ntp_io_linux.c +++ b/ntp_io_linux.c @@ -31,10 +31,13 @@ #include #include #include +#include #include +#include #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;