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;