diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 1d455c6be70..c86f3d647ef 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -37,15 +37,24 @@ * Public Types ****************************************************************************/ +enum ptp_delay_mechanism_e +{ + PTP_DELAY_NONE = 0, + PTP_DELAY_E2E, + PTP_DELAY_P2P +}; + struct ptpd_config_s { FAR const char *interface; FAR const char *clock; bool client_only; bool hardware_ts; - bool delay_e2e; + enum ptp_delay_mechanism_e delay_mechanism; bool bmca; sa_family_t af; + int32_t ingress_latency_ns; /* Hardware RX timestamp latency (ns) */ + int32_t egress_latency_ns; /* Hardware TX timestamp latency (ns) */ }; /* PTPD status information structure */ @@ -106,6 +115,7 @@ struct ptpd_status_s struct timespec last_transmitted_announce; struct timespec last_transmitted_delayresp; struct timespec last_transmitted_delayreq; + struct timespec last_transmitted_pdelayreq; }; /**************************************************************************** diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index b27143ba2ce..eb560d7b0a8 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -175,6 +175,26 @@ config NETUTILS_PTPD_DRIFT_AVERAGE_S gives more stable estimate but reacts slower to crystal oscillator speed changes (such as caused by temperature changes). +config NETUTILS_PTPD_MAX_DRIFT_PPB + int "PTP client maximum plausible clock drift rate (ppb)" + default 500000 + range 1000 20000000 + ---help--- + A single drift-rate sample computed between two consecutive sync + updates is discarded (the previous averaged drift_ppb is kept + unchanged) if its magnitude exceeds this bound. Real crystal + oscillators drift by at most a few hundred ppm (hundreds of + thousands of ppb), so this catches bogus samples caused by an + abnormally short or long measurement interval - e.g. right after + a clock source outage/reconnect, or a burst of closely spaced + sync packets following packet loss - before they corrupt the + long-term drift_ppb average and get applied to the hardware. + + This is intentionally much tighter than + CLOCK_ADJTIME_SLEWLIMIT_PPM, which bounds how fast a correction + may be applied rather than how large a real drift measurement + can plausibly be. + config NETUTILS_PTPD_MAX_PATH_DELAY_NS int "PTP client maximum path delay (ns)" default 100000 @@ -189,6 +209,26 @@ config NETUTILS_PTPD_DELAYREQ_AVGCOUNT ---help--- Measured path delay is averaged over this many samples. +config NETUTILS_PTPD_OUTLIER_THRESHOLD_NS + int "PTP outlier rejection threshold (ns)" + default 0 + range 0 1000000000 + ---help--- + A phase error measurement that differs from the median of the + latest accepted ones by more than this many nanoseconds is + discarded instead of being used to correct the clock. It protects + the frequency estimate and the phase correction from a single + disturbed sample, for example a receive timestamp taken late by + the scheduler with software timestamping. + + A change that lasts is accepted after several discarded samples in + a row, so the daemon still follows a real step of the master, and + a short burst of disturbed samples is still ridden out. + + Choose a value well above the normal spread of the measurement: + a few microseconds are typical with hardware timestamping and + hundreds with software timestamping. 0 disables the rejection. + config NETUTILS_PTPD_STATUSFILE string "PTP daemon status file path" default "/tmp/ptpd.status" @@ -199,4 +239,45 @@ config NETUTILS_PTPD_STATUSFILE memory, making it work across all build modes (Flat, Protected, Kernel). Written atomically via temp + rename. +config NETUTILS_PTPD_INGRESS_LATENCY_NS + int "PTP hardware receive timestamp latency (ns)" + default 0 + range -1000000 1000000 + ---help--- + Fixed delay, in nanoseconds, between a frame reaching the wire + reference plane and the moment the MAC latches its hardware + receive timestamp. This is the ingressLatency port parameter of + IEEE 1588: the PHY and the MAC clock-domain crossing make the + timestamp point lag the true arrival of the frame. + + The latency is subtracted from every hardware receive timestamp, + so a positive value moves the timestamps earlier. It has no + effect with software timestamping. It can be overridden at + run time with the -I option. + + The value depends on the PHY and board and has to be measured, + for example by comparing a physical PPS output against a + reference. The default of 0 applies no compensation. + +config NETUTILS_PTPD_EGRESS_LATENCY_NS + int "PTP hardware transmit timestamp latency (ns)" + default 0 + range -1000000 1000000 + depends on NET_TIMESTAMP + ---help--- + Fixed delay, in nanoseconds, between the MAC latching a hardware + transmit timestamp and the frame reaching the wire reference + plane. This is the egressLatency port parameter of IEEE 1588: the + clock domain crossing and the PHY make the frame leave later than + the timestamp point. + + The latency is added to every hardware transmit timestamp, so a + positive value moves the timestamps later. It has no effect with + software timestamping or when the driver does not provide + hardware transmit timestamps. It can be overridden at run time + with the -O option. + + The value depends on the PHY and board and has to be measured. + The default of 0 applies no compensation. + endif # NETUTILS_PTPD diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index e528da8a977..f10606bf5f0 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -26,12 +26,14 @@ #include +#include #include #include -#include #include #include +#include +#include #include #include @@ -40,6 +42,8 @@ #include #include #include +#include +#include #include #include #include @@ -61,10 +65,42 @@ #include "netutils/netlib.h" #include "ptpv2.h" +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Number of consecutive missing hardware TX timestamps after which the + * driver is assumed not to provide them and software timestamps are used. + */ + +#define PTP_HWTS_TX_MAX_FAILURES 3 + +#if CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS > 0 +/* Outlier rejection of the measured phase error: number of recent samples + * the median is taken over, the least number of samples needed before + * anything is rejected, and how many samples in a row can be rejected + * before they are taken as a real change of the phase. + */ + +# define PTP_OUTLIER_HISTORY 5 +# define PTP_OUTLIER_MIN_HISTORY 3 +# define PTP_OUTLIER_MAX_CONSECUTIVE 8 +#endif + /**************************************************************************** * Private Types ****************************************************************************/ +#ifdef CONFIG_BUILD_FLAT +/* Carrier structure for querying PTPD status in flat build mode */ + +struct ptpd_statusreq_s +{ + sem_t done; + struct ptpd_status_s dest; +}; +#endif + /* Main PTPD state storage */ struct ptp_state_s @@ -72,7 +108,11 @@ struct ptp_state_s /* Request for PTPD task to stop or dump status */ bool stop; +#ifdef CONFIG_BUILD_FLAT + FAR struct ptpd_statusreq_s *status_req; /* Set by SIGUSR1 */ +#else bool dump; /* Set by SIGUSR1, checked in main loop */ +#endif /* Address of network interface we are operating on */ @@ -82,6 +122,13 @@ struct ptp_state_s int tx_socket; + /* Hardware TX timestamp retrieval: consecutive failures, and whether it + * was given up on because the driver does not provide the timestamps. + */ + + unsigned int hwts_tx_failures; + bool hwts_tx_disabled; + /* Sockets for PTP event and information ports */ int event_socket; @@ -100,6 +147,7 @@ struct ptp_state_s uint16_t announce_seq; uint16_t sync_seq; uint16_t delay_req_seq; + uint16_t pdelay_req_seq; /* Previous measurement and estimated clock drift rate */ @@ -109,6 +157,12 @@ struct ptp_state_s long drift_avg_total_ms; long drift_ppb; bool has_last_delta; +#if CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS > 0 + int64_t delta_hist[PTP_OUTLIER_HISTORY]; + unsigned int delta_hist_count; + unsigned int delta_hist_next; + unsigned int outlier_count; +#endif /* Identity of currently selected clock source, * from the latest announcement message. @@ -131,6 +185,7 @@ struct ptp_state_s struct timespec last_transmitted_announce; struct timespec last_transmitted_delayresp; struct timespec last_transmitted_delayreq; + struct timespec last_transmitted_pdelayreq; /* Timestamps related to path delay calculation (CLOCK_REALTIME) */ @@ -139,19 +194,31 @@ struct ptp_state_s int path_delay_avgcount; long path_delay_ns; long delayreq_interval; + int64_t sync_diff_ns; + bool sync_diff_valid; + + /* Timestamps related to P2P peer delay calculation (CLOCK_REALTIME) */ + + struct timespec pdelayreq_tx_time; /* t1 */ + struct timespec pdelayreq_rx_time; /* t2 */ + struct timespec pdelayresp_rx_time; /* t4 */ + bool pdelay_waiting_followup; /* Latest received packet and its timestamp (CLOCK_REALTIME) */ struct timespec rxtime; union { - struct ptp_header_s header; - struct ptp_announce_s announce; - struct ptp_sync_s sync; - struct ptp_follow_up_s follow_up; - struct ptp_delay_req_s delay_req; - struct ptp_delay_resp_s delay_resp; - uint8_t raw[128]; + struct ptp_header_s header; + struct ptp_announce_s announce; + struct ptp_sync_s sync; + struct ptp_follow_up_s follow_up; + struct ptp_delay_req_s delay_req; + struct ptp_delay_resp_s delay_resp; + struct ptp_pdelay_req_s pdelay_req; + struct ptp_pdelay_resp_s pdelay_resp; + struct ptp_pdelay_resp_follow_up_s pdelay_resp_fup; + uint8_t raw[128]; } rxbuf; uint8_t rxcmsg[CMSG_LEN(sizeof(struct timespec))]; @@ -165,6 +232,25 @@ struct ptp_state_s FAR const struct ptpd_config_s *config; }; +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef CONFIG_BUILD_FLAT +/* The status request of ptpd_status(). The daemon keeps its address until it + * answers, which can be after ptpd_status() gave up waiting and returned, so + * it lives in static memory and never on the stack of the caller. The lock + * lets only one caller use it at a time. + */ + +static struct ptpd_statusreq_s g_statusreq = +{ + SEM_INITIALIZER(0) +}; + +static pthread_mutex_t g_statusreq_lock = PTHREAD_MUTEX_INITIALIZER; +#endif + /**************************************************************************** * Private Functions ****************************************************************************/ @@ -298,6 +384,22 @@ static int64_t timespec_to_ms(FAR const struct timespec *ts) return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC); } +/* Add a positive or negative number of nanoseconds to a timespec value. */ + +static void timespec_add_ns(FAR struct timespec *ts, int64_t ns) +{ + int64_t total = ts->tv_sec * NSEC_PER_SEC + ts->tv_nsec + ns; + + ts->tv_sec = total / NSEC_PER_SEC; + ts->tv_nsec = total % NSEC_PER_SEC; + + if (ts->tv_nsec < 0) + { + ts->tv_sec--; + ts->tv_nsec += NSEC_PER_SEC; + } +} + /* Get positive or negative delta between two timespec values. * If value would exceed int64 limit (292 years), return INT64_MAX/MIN. */ @@ -426,9 +528,31 @@ static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns, else { struct timex buf; + int64_t hw_ppb; + const int64_t slew_limit_ppb = + CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000; + + /* delta_ns passed here is adjustment_ns, which already + * combines frequency drift and current phase error clamped + * to max_adjust_ns. Converting it to ppb over + * CONFIG_CLOCK_ADJTIME_PERIOD_MS produces the rate needed to + * pull the hardware counter into phase lock. + */ + + hw_ppb = delta_ns * MSEC_PER_SEC / + CONFIG_CLOCK_ADJTIME_PERIOD_MS; + + if (hw_ppb > slew_limit_ppb) + { + hw_ppb = slew_limit_ppb; + } + else if (hw_ppb < -slew_limit_ppb) + { + hw_ppb = -slew_limit_ppb; + } memset(&buf, 0, sizeof(buf)); - buf.freq = (long)(-ppb * 65536 / 1000); + buf.freq = hw_ppb * 65536 / 1000; buf.modes = ADJ_FREQUENCY; return clock_adjtime(state->clockid, &buf); @@ -462,6 +586,11 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, if (ts->tv_sec > 0 || ts->tv_nsec > 0) { + /* The MAC latches the timestamp later than the frame + * reaches the wire: compensate the ingress latency. + */ + + timespec_add_ns(ts, -state->config->ingress_latency_ns); return OK; } } @@ -479,9 +608,19 @@ static int ptp_destroy_state(FAR struct ptp_state_s *state) ptp_close(state->clockid); - mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); - ipmsfilter(&state->interface_addr.sin_addr, - &mcast_addr, MCAST_EXCLUDE); + if (state->config->af == AF_INET) + { + mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); + ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, MCAST_EXCLUDE); + + if (state->config->delay_mechanism == PTP_DELAY_P2P) + { + mcast_addr.s_addr = HTONL(PTP_PDELAY_MULTICAST_ADDR); + ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, MCAST_EXCLUDE); + } + } if (state->tx_socket > 0) { @@ -532,9 +671,6 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state) goto errout; } - state->event_socket = dup(state->tx_socket); - state->info_socket = -1; - addr.sll_family = AF_PACKET; addr.sll_ifindex = if_nametoindex(state->config->interface); addr.sll_protocol = htons(ETHERTYPE_PTP); @@ -545,6 +681,15 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state) ptperr("ERROR: binding socket failed: %d\n", errno); goto errout; } + + state->event_socket = dup(state->tx_socket); + if (state->event_socket < 0) + { + ptperr("Failed to dup event socket: %d\n", errno); + goto errout; + } + + state->info_socket = -1; } else if (state->config->af == AF_INET) { @@ -654,6 +799,19 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state) ptperr("Failed to join multicast group: %d\n", errno); goto errout; } + + if (state->config->delay_mechanism == PTP_DELAY_P2P) + { + mcast_addr.s_addr = HTONL(PTP_PDELAY_MULTICAST_ADDR); + ret = ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, MCAST_INCLUDE); + if (ret < 0) + { + ptperr("Failed to join peer delay multicast group: %d\n", + errno); + goto errout; + } + } } /* Get hardware address to initialize the identity field in header. @@ -667,8 +825,9 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state) goto errout; } - state->own_identity.header.version = PTP_VERSION_2_1; + state->own_identity.header.version = PTP_VERSION_2_0; state->own_identity.header.domain = CONFIG_NETUTILS_PTPD_DOMAIN; + state->own_identity.header.controlfield = 0x05; state->own_identity.header.sourceidentity[0] = req.ifr_hwaddr.sa_data[0]; state->own_identity.header.sourceidentity[1] = req.ifr_hwaddr.sa_data[1]; state->own_identity.header.sourceidentity[2] = req.ifr_hwaddr.sa_data[2]; @@ -711,6 +870,12 @@ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) struct in_addr mcast_addr; struct timespec time_now; struct timespec delta; + int ret; + + if (state->config->af != AF_INET) + { + return OK; + } clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_received_multicast, @@ -727,9 +892,23 @@ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) &mcast_addr, MCAST_EXCLUDE); - return ipmsfilter(&state->interface_addr.sin_addr, - &mcast_addr, - MCAST_INCLUDE); + ret = ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, + MCAST_INCLUDE); + + if (state->config->delay_mechanism == PTP_DELAY_P2P) + { + mcast_addr.s_addr = HTONL(PTP_PDELAY_MULTICAST_ADDR); + ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, + MCAST_EXCLUDE); + + ret = ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, + MCAST_INCLUDE); + } + + return ret; } #else @@ -739,30 +918,136 @@ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) return OK; } +#ifdef CONFIG_NET_TIMESTAMP +/**************************************************************************** + * Name: ptp_get_tx_timestamp + * + * Description: + * Retrieve the hardware TX timestamp delivered via MSG_ERRQUEUE on the + * socket after transmission. + * + * Input Parameters: + * state - Pointer to PTP daemon state + * tx_ts - Location to return the hardware timestamp + * + * Returned Value: + * OK on success; ERROR on failure or timeout. + * + ****************************************************************************/ + +static int ptp_get_tx_timestamp(FAR struct ptp_state_s *state, + FAR struct timespec *tx_ts) +{ + struct pollfd pfd; + int ret; + + pfd.fd = state->tx_socket; + pfd.events = POLLPRI; + pfd.revents = 0; + + ret = poll(&pfd, 1, 500); + if (ret > 0 && (pfd.revents & (POLLPRI | POLLERR)) != 0) + { + char errbuf[128]; + char cmsgbuf[128]; + struct msghdr msg; + struct iovec iov; + FAR struct cmsghdr *cmsg; + ssize_t n; + + memset(&msg, 0, sizeof(msg)); + iov.iov_base = errbuf; + iov.iov_len = sizeof(errbuf); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = cmsgbuf; + msg.msg_controllen = sizeof(cmsgbuf); + + n = recvmsg(state->tx_socket, &msg, MSG_ERRQUEUE); + if (n >= 0) + { + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; + cmsg = CMSG_NXTHDR(&msg, cmsg)) + { + if (cmsg->cmsg_level == SOL_SOCKET && + cmsg->cmsg_type == SO_TIMESTAMPING) + { + FAR struct timespec *ts = + (FAR struct timespec *)CMSG_DATA(cmsg); + + *tx_ts = ts[2]; + return OK; + } + } + + ptpwarn("PTP TX HWTS: recvmsg %zd B without SO_TIMESTAMPING\n", + n); + } + else + { + ptpwarn("PTP TX HWTS: recvmsg MSG_ERRQUEUE failed errno=%d\n", + errno); + } + } + else + { + ptpwarn("PTP TX HWTS: poll ret=%d revents=0x%04x errno=%d\n", + ret, pfd.revents, errno); + } + + return ERROR; +} +#endif + static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, size_t buflen, FAR const void *addr, socklen_t addrlen, FAR struct timespec *sendts) { int ret; + struct timespec sw_ts; +#ifdef CONFIG_NET_TIMESTAMP + bool do_hwts = (sendts != NULL && state->config->hardware_ts && + !state->hwts_tx_disabled && + state->config->af == AF_PACKET); +#endif + + if (sendts != NULL) + { + ptp_gettime(state, &sw_ts); + } if (state->config->af == AF_PACKET) { - /* IEEE 1588-2008 Annex F primary multicast MAC address */ + /* IEEE 1588-2008 Annex F multicast MAC addresses */ const uint8_t ptp_multicast_mac[ETHER_ADDR_LEN] = - { - 0x01, 0x1b, 0x19, 0x00, 0x00, 0x00 - }; - + PTP_MULTICAST_MAC; + const uint8_t ptp_pdelay_multicast_mac[ETHER_ADDR_LEN] = + PTP_PDELAY_MULTICAST_MAC; + FAR const struct ptp_header_s *hdr = buf; + FAR const uint8_t *dst_mac; char raw[sizeof(struct ether_header) + sizeof(struct ptp_announce_s)]; FAR struct ether_header *header; struct msghdr msg; struct iovec iov; + uint8_t msgtype; DEBUGASSERT(sizeof(struct ptp_announce_s) >= buflen); + msgtype = hdr->messagetype & PTP_MSGTYPE_MASK; + if (msgtype == PTP_MSGTYPE_PDELAY_REQ || + msgtype == PTP_MSGTYPE_PDELAY_RESP || + msgtype == PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP) + { + dst_mac = ptp_pdelay_multicast_mac; + } + else + { + dst_mac = ptp_multicast_mac; + } + header = (FAR struct ether_header *)&raw; - memcpy(header->ether_dhost, ptp_multicast_mac, ETHER_ADDR_LEN); + memcpy(header->ether_dhost, dst_mac, ETHER_ADDR_LEN); netlib_getmacaddr(state->config->interface, header->ether_shost); header->ether_type = htons(ETHERTYPE_PTP); memcpy(&raw[sizeof(*header)], buf, buflen); @@ -783,23 +1068,49 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, msg.msg_control = NULL; msg.msg_controllen = 0; - ret = sendmsg(state->tx_socket, &msg, 0); - if (ret < 0) +#ifdef CONFIG_NET_TIMESTAMP + if (do_hwts) { - return ERROR; + char drainbuf[128]; + char draincmsg[128]; + struct msghdr drainmsg; + struct iovec drainiov; + int val; + + memset(&drainmsg, 0, sizeof(drainmsg)); + drainiov.iov_base = drainbuf; + drainiov.iov_len = sizeof(drainbuf); + drainmsg.msg_iov = &drainiov; + drainmsg.msg_iovlen = 1; + drainmsg.msg_control = draincmsg; + drainmsg.msg_controllen = sizeof(draincmsg); + + while (recvmsg(state->tx_socket, &drainmsg, + MSG_ERRQUEUE | MSG_DONTWAIT) > 0) + { + } + + val = SOF_TIMESTAMPING_TX_HARDWARE | + SOF_TIMESTAMPING_RAW_HARDWARE; + setsockopt(state->tx_socket, SOL_SOCKET, SO_TIMESTAMPING, + &val, sizeof(val)); } +#endif - if (state->config->hardware_ts && sendts != NULL) + ret = sendmsg(state->tx_socket, &msg, 0); + if (ret < 0) { - uint8_t rxcmsg[CMSG_LEN(sizeof(struct timespec))]; - - msg.msg_control = &rxcmsg; - msg.msg_controllen = CMSG_LEN(sizeof(struct timespec)); - ret = recvmsg(state->tx_socket, &msg, 0); - if (ret >= 0) +#ifdef CONFIG_NET_TIMESTAMP + if (do_hwts) { - ptp_getrxtime(state, &msg, sendts); + int val = 0; + + setsockopt(state->tx_socket, SOL_SOCKET, SO_TIMESTAMPING, + &val, sizeof(val)); } + +#endif + return ERROR; } } else @@ -807,9 +1118,46 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, ret = sendto(state->tx_socket, buf, buflen, 0, addr, addrlen); } - if (!state->config->hardware_ts && sendts != NULL) + if (sendts != NULL) { - ptp_gettime(state, sendts); +#ifdef CONFIG_NET_TIMESTAMP + if (do_hwts) + { + int val = 0; + + if (ptp_get_tx_timestamp(state, sendts) == OK) + { + state->hwts_tx_failures = 0; + + /* The frame reaches the wire later than the MAC latches the + * timestamp: compensate the egress latency. + */ + + timespec_add_ns(sendts, state->config->egress_latency_ns); + } + else + { + ptpwarn("PTP TX HWTS timeout, fallback to SW ts: " + "%jd.%09ld s\n", + (intmax_t)sw_ts.tv_sec, sw_ts.tv_nsec); + *sendts = sw_ts; + + if (++state->hwts_tx_failures >= PTP_HWTS_TX_MAX_FAILURES) + { + state->hwts_tx_disabled = true; + ptpwarn("Hardware TX timestamps unavailable, " + "using software timestamps\n"); + } + } + + setsockopt(state->tx_socket, SOL_SOCKET, SO_TIMESTAMPING, + &val, sizeof(val)); + } + else +#endif + { + *sendts = sw_ts; + } } return ret; @@ -948,6 +1296,58 @@ static int ptp_send_delay_req(FAR struct ptp_state_s *state) return ret; } +/* Send peer delay request packet (P2P) */ + +static int ptp_send_pdelay_req(FAR struct ptp_state_s *state) +{ + struct ptp_pdelay_req_s req; + struct sockaddr_in addr; + int ret; + + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = HTONL(PTP_PDELAY_MULTICAST_ADDR); + addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); + + memset(&req, 0, sizeof(req)); + req.header = state->own_identity.header; + req.header.messagetype = PTP_MSGTYPE_PDELAY_REQ; + req.header.version = PTP_VERSION_2_0; + req.header.messagelength[1] = sizeof(req); + req.header.controlfield = 0x05; + req.header.logmessageinterval = PTP_LOG_INTERVAL_DELAY_REQ; + ptp_increment_sequence(&state->pdelay_req_seq, &req.header); + + /* Starting a new request cycle invalidates any Pdelay_Resp we might + * still be waiting a Follow_Up for from the previous one (e.g. its + * Resp was lost and only its Follow_Up shows up later, after this + * new cycle has already updated pdelay_req_seq). Without this, that + * orphaned Follow_Up would still pass the sequence check below (it + * now matches the new cycle) and get paired with pdelayreq_rx_time + * (t2) captured for the OLD cycle - producing a path delay that is + * off by roughly one full request interval. + */ + + state->pdelay_waiting_followup = false; + + ptp_gettime(state, &state->pdelayreq_tx_time); + timespec_to_ptp_format(&state->pdelayreq_tx_time, req.origintimestamp); + + ret = ptp_sendmsg(state, &req, sizeof(req), + &addr, sizeof(addr), &state->pdelayreq_tx_time); + if (ret < 0) + { + ptperr("ptp sendmsg failed: %d\n", errno); + } + else + { + clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_pdelayreq); + ptpinfo("Sent Pdelay_Req, seq %d\n", + ptp_get_sequence(&req.header)); + } + + return ret; +} + /* Check if we need to send packets */ static int ptp_periodic_send(FAR struct ptp_state_s *state) @@ -980,18 +1380,19 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) } } - if (state->config->delay_e2e && state->selected_source_valid && - state->can_send_delayreq) + if (state->config->delay_mechanism == PTP_DELAY_E2E && + state->selected_source_valid && state->can_send_delayreq) { struct timespec time_now; struct timespec delta; + long interval_s; clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_transmitted_delayreq, &delta); - long interval_s = (state->delayreq_interval > 0) ? - state->delayreq_interval : 1; + interval_s = (state->delayreq_interval > 0) ? + state->delayreq_interval : 1; if (timespec_to_ms(&delta) >= interval_s * MSEC_PER_SEC) { @@ -999,6 +1400,25 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) } } + if (state->config->delay_mechanism == PTP_DELAY_P2P) + { + struct timespec time_now; + struct timespec delta; + long interval_s; + + clock_gettime(CLOCK_MONOTONIC, &time_now); + clock_timespec_subtract(&time_now, + &state->last_transmitted_pdelayreq, &delta); + + interval_s = (state->delayreq_interval > 0) ? + state->delayreq_interval : 1; + + if (timespec_to_ms(&delta) >= interval_s * MSEC_PER_SEC) + { + ptp_send_pdelay_req(state); + } + } + return OK; } @@ -1018,15 +1438,81 @@ static int ptp_process_announce(FAR struct ptp_state_s *state, state->selected_source = *msg; state->last_received_sync = state->last_received_announce; - state->path_delay_avgcount = 0; - state->path_delay_ns = 0; - state->delayreq_time.tv_sec = 0; + if (state->config->delay_mechanism == PTP_DELAY_E2E) + { + state->path_delay_avgcount = 0; + state->path_delay_ns = 0; + state->delayreq_time.tv_sec = 0; + } } } return OK; } +#if CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS > 0 +/* Tell whether a phase error measurement is an outlier, i.e. it differs from + * the median of the latest accepted ones by more than the threshold. A + * measurement that is disturbed on its own (a late receive timestamp, for + * example) would otherwise move the frequency and phase corrections. + * + * A change that lasts is not an outlier: after a few rejections in a row + * the measurement is accepted and the history starts over. + */ + +static bool ptp_is_outlier(FAR struct ptp_state_s *state, int64_t delta_ns) +{ + int64_t sorted[PTP_OUTLIER_HISTORY]; + int64_t deviation; + unsigned int count = state->delta_hist_count; + unsigned int i; + unsigned int j; + + if (count >= PTP_OUTLIER_MIN_HISTORY) + { + for (i = 0; i < count; i++) + { + int64_t value = state->delta_hist[i]; + + for (j = i; j > 0 && sorted[j - 1] > value; j--) + { + sorted[j] = sorted[j - 1]; + } + + sorted[j] = value; + } + + deviation = delta_ns - sorted[count / 2]; + if (deviation < 0) + { + deviation = -deviation; + } + + if (deviation > CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS) + { + if (++state->outlier_count < PTP_OUTLIER_MAX_CONSECUTIVE) + { + return true; + } + + state->delta_hist_count = 0; + state->delta_hist_next = 0; + } + } + + state->outlier_count = 0; + state->delta_hist[state->delta_hist_next] = delta_ns; + state->delta_hist_next = (state->delta_hist_next + 1) % + PTP_OUTLIER_HISTORY; + if (state->delta_hist_count < PTP_OUTLIER_HISTORY) + { + state->delta_hist_count++; + } + + return false; +} +#endif + /* Update local clock either by smooth adjustment or by jumping. * Remote time was remote_timestamp at local_timestamp. */ @@ -1072,6 +1558,11 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, state->drift_avg_total_ms = 0; state->drift_ppb = 0; state->has_last_delta = false; +#if CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS > 0 + state->delta_hist_count = 0; + state->delta_hist_next = 0; + state->outlier_count = 0; +#endif if (ret == OK) { @@ -1097,8 +1588,15 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, const int64_t max_adjust_ns = (int64_t)CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * CONFIG_CLOCK_ADJTIME_PERIOD_MS; - const int64_t slew_limit_ppb = - (int64_t)CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000; + +#if CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS > 0 + if (ptp_is_outlier(state, delta_ns)) + { + ptpwarn("Discarding outlier sample: delta %" PRId64 " ns\n", + delta_ns); + return OK; + } +#endif if (!state->has_last_delta) { @@ -1134,8 +1632,19 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, interval_ms = 1; } - if (drift_ppb > slew_limit_ppb || drift_ppb < -slew_limit_ppb) + if (drift_ppb > CONFIG_NETUTILS_PTPD_MAX_DRIFT_PPB || + drift_ppb < -CONFIG_NETUTILS_PTPD_MAX_DRIFT_PPB) { + /* Physically implausible for a real crystal oscillator - + * almost always the result of an abnormally short interval + * between samples (e.g. a burst of packets right after a + * clock source outage/reconnect) rather than actual drift. + * Discard it instead of letting it corrupt the long-term + * average; CLOCK_ADJTIME_SLEWLIMIT_PPM is a much looser + * hardware safety bound and would let this through + * unchanged. + */ + ptpwarn("Drift estimate out of range: %lld\n", (long long)drift_ppb); drift_ppb = state->drift_ppb; @@ -1206,14 +1715,39 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, return ret; } -/* Process received PTP sync packet */ - -static int ptp_process_sync(FAR struct ptp_state_s *state, - FAR struct ptp_sync_s *msg) +static void ptp_add_correction_time(FAR const uint8_t *correction, + FAR struct timespec *ts) { - struct timespec remote_time; - - if (state->config->bmca && + uint64_t correction_time = (((uint64_t)correction[0]) << 40) + | (((uint64_t)correction[1]) << 32) + | (((uint64_t)correction[2]) << 24) + | (((uint64_t)correction[3]) << 16) + | (((uint64_t)correction[4]) << 8) + | (((uint64_t)correction[5]) << 0); + + ptpinfo("correction before: %jd.%09ld\n", (intmax_t)ts->tv_sec, + ts->tv_nsec); + + ts->tv_sec += correction_time / NSEC_PER_SEC; + ts->tv_nsec += correction_time % NSEC_PER_SEC; + if (ts->tv_nsec >= NSEC_PER_SEC) + { + ts->tv_nsec -= NSEC_PER_SEC; + ts->tv_sec += 1; + } + + ptpinfo("correction after: %jd.%09ld\n", (intmax_t)ts->tv_sec, + ts->tv_nsec); +} + +/* Process received PTP sync packet */ + +static int ptp_process_sync(FAR struct ptp_state_s *state, + FAR struct ptp_sync_s *msg) +{ + struct timespec remote_time; + + if (state->config->bmca && memcmp(msg->header.sourceidentity, state->selected_source.header.sourceidentity, sizeof(msg->header.sourceidentity)) != 0) @@ -1240,34 +1774,12 @@ static int ptp_process_sync(FAR struct ptp_state_s *state, /* Update local clock */ ptp_format_to_timespec(msg->origintimestamp, &remote_time); + ptp_add_correction_time(msg->header.correction, &remote_time); + state->sync_diff_ns = timespec_delta_ns(&state->rxtime, &remote_time); + state->sync_diff_valid = true; return ptp_update_local_clock(state, &remote_time, &state->rxtime); } -static void ptp_add_correction_time(FAR const uint8_t *correction, - FAR struct timespec *ts) -{ - uint64_t correction_time = (((uint64_t)correction[0]) << 40) - | (((uint64_t)correction[1]) << 32) - | (((uint64_t)correction[2]) << 24) - | (((uint64_t)correction[3]) << 16) - | (((uint64_t)correction[4]) << 8) - | (((uint64_t)correction[5]) << 0); - - ptpinfo("correction before: %jd.%09ld\n", (intmax_t)ts->tv_sec, - ts->tv_nsec); - - ts->tv_sec += correction_time / NSEC_PER_SEC; - ts->tv_nsec += correction_time % NSEC_PER_SEC; - if (ts->tv_nsec >= NSEC_PER_SEC) - { - ts->tv_nsec -= NSEC_PER_SEC; - ts->tv_sec += 1; - } - - ptpinfo("correction after: %jd.%09ld\n", (intmax_t)ts->tv_sec, - ts->tv_nsec); -} - static int ptp_process_followup(FAR struct ptp_state_s *state, FAR struct ptp_follow_up_s *msg) { @@ -1301,6 +1813,12 @@ static int ptp_process_followup(FAR struct ptp_state_s *state, ptp_add_correction_time(msg->header.correction, &remote_time); + /* Store (t2 - t1) for canonical IEEE 1588-2008 §11.3 path delay */ + + state->sync_diff_ns = timespec_delta_ns(&state->twostep_rxtime, + &remote_time); + state->sync_diff_valid = true; + /* done */ return ptp_update_local_clock(state, &remote_time, &state->twostep_rxtime); @@ -1352,15 +1870,57 @@ static int ptp_process_delay_req(FAR struct ptp_state_s *state, return ret; } +/* Record and filter measured path delay (used by both E2E and P2P) */ + +static void ptp_record_path_delay(FAR struct ptp_state_s *state, + int64_t path_delay) +{ + int64_t max_path_delay; + + max_path_delay = CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS; + + if (max_path_delay < 10 * NSEC_PER_MSEC) + { + /* Software TX latency on delay measurement transmission can add up + * to several milliseconds. Allow up to 10 ms until hardware TX + * timestamping is available. + */ + + max_path_delay = 10 * NSEC_PER_MSEC; + } + + if (path_delay >= -100000 && path_delay < max_path_delay) + { + if (path_delay < 0) + { + path_delay = 0; + } + + if (state->path_delay_avgcount < + CONFIG_NETUTILS_PTPD_DELAYREQ_AVGCOUNT) + { + state->path_delay_avgcount++; + } + + state->path_delay_ns += (path_delay - state->path_delay_ns) + / state->path_delay_avgcount; + + ptpinfo("Path delay: %" PRId64 " ns (avg: %ld ns)\n", + path_delay, state->path_delay_ns); + } + else + { + ptpwarn("Path delay out of range: %" PRId64 " ns\n", path_delay); + } +} + static int ptp_process_delay_resp(FAR struct ptp_state_s *state, FAR struct ptp_delay_resp_s *msg) { int64_t path_delay; - int64_t sync_delay; struct timespec remote_rxtime; uint16_t sequence; int interval; - int64_t max_path_delay; bool source_match; bool request_match; @@ -1371,10 +1931,13 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, state->own_identity.header.sourceidentity, sizeof(msg->reqidentity)) == 0; - if (!state->selected_source_valid || !source_match || !request_match) + if (!state->selected_source_valid || !state->sync_diff_valid || + !source_match || !request_match) { - ptpwarn("Delay_Resp ignored: valid=%d, src_match=%d, req_match=%d\n", - state->selected_source_valid, source_match, request_match); + ptpwarn("Delay_Resp ignored: valid=%d, sync_valid=%d, src_match=%d, " + "req_match=%d\n", + state->selected_source_valid, state->sync_diff_valid, + source_match, request_match); return OK; /* This packet wasn't for us */ } @@ -1388,61 +1951,230 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, } /* Path delay is calculated as the average between delta for sync - * message and delta for delay req message. + * message (t2 - t1) and delta for delay req message (t4 - t3). * (IEEE-1588 section 11.3: Delay request-response mechanism) */ ptp_format_to_timespec(msg->receivetimestamp, &remote_rxtime); path_delay = timespec_delta_ns(&remote_rxtime, &state->delayreq_time); - sync_delay = state->path_delay_ns - state->last_delta_ns; - path_delay = (path_delay + sync_delay) / 2; + path_delay = (state->sync_diff_ns + path_delay) / 2; - max_path_delay = CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS; + ptp_record_path_delay(state, path_delay); - if (!state->config->hardware_ts && - max_path_delay < 10 * (int64_t)NSEC_PER_MSEC) + /* Calculate interval until next packet */ + + if (msg->header.logmessageinterval <= 12) { - /* Software timestamping includes network stack and OS latency, - * allow up to 10 ms. - */ + interval = (1 << msg->header.logmessageinterval); + } + else + { + interval = 4096; /* Refuse to obey excessively long intervals */ + } + + /* Randomize up to 2x nominal delay) */ + + state->delayreq_interval = interval + (random() % interval); + + return OK; +} + +/* Process received peer delay request (responder role) */ - max_path_delay = 10 * (int64_t)NSEC_PER_MSEC; +static int ptp_process_pdelay_req(FAR struct ptp_state_s *state, + FAR struct ptp_pdelay_req_s *msg) +{ + struct ptp_pdelay_resp_s resp; + struct ptp_pdelay_resp_follow_up_s fup; + struct sockaddr_in addr; + struct timespec t3; + int ret; + + if (state->config->delay_mechanism != PTP_DELAY_P2P) + { + return OK; } - if (path_delay >= 0 && path_delay < max_path_delay) + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = HTONL(PTP_PDELAY_MULTICAST_ADDR); + addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); + + memset(&resp, 0, sizeof(resp)); + resp.header = state->own_identity.header; + resp.header.messagetype = PTP_MSGTYPE_PDELAY_RESP; + resp.header.version = PTP_VERSION_2_0; + resp.header.messagelength[1] = sizeof(resp); + resp.header.flags[0] = PTP_FLAGS0_TWOSTEP; + resp.header.controlfield = 0x05; + memcpy(resp.header.sequenceid, msg->header.sequenceid, + sizeof(resp.header.sequenceid)); + resp.header.logmessageinterval = 0x7f; + + timespec_to_ptp_format(&state->rxtime, resp.requestreceipttimestamp); + memcpy(resp.reqidentity, msg->header.sourceidentity, + sizeof(resp.reqidentity)); + memcpy(resp.reqportindex, msg->header.sourceportindex, + sizeof(resp.reqportindex)); + + ret = ptp_sendmsg(state, &resp, sizeof(resp), &addr, sizeof(addr), &t3); + if (ret < 0) { - if (state->path_delay_avgcount < - CONFIG_NETUTILS_PTPD_DELAYREQ_AVGCOUNT) - { - state->path_delay_avgcount++; - } + ptperr("ptp sendmsg failed for Pdelay_Resp: %d\n", errno); + return ret; + } - state->path_delay_ns += (path_delay - state->path_delay_ns) - / state->path_delay_avgcount; + clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayresp); + ptpinfo("Sent Pdelay_Resp, seq %d\n", + ptp_get_sequence(&resp.header)); + + /* Send Pdelay_Resp_Follow_Up with transmit timestamp t3 */ - ptpinfo("Path delay: %ld ns (avg: %ld ns)\n", - (long)path_delay, (long)state->path_delay_ns); + addr.sin_port = HTONS(PTP_UDP_PORT_INFO); + + memset(&fup, 0, sizeof(fup)); + fup.header = state->own_identity.header; + fup.header.messagetype = PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP; + fup.header.version = PTP_VERSION_2_0; + fup.header.messagelength[1] = sizeof(fup); + fup.header.controlfield = 0x05; + memcpy(fup.header.sequenceid, msg->header.sequenceid, + sizeof(fup.header.sequenceid)); + fup.header.logmessageinterval = 0x7f; + + timespec_to_ptp_format(&t3, fup.responseorigintimestamp); + memcpy(fup.reqidentity, msg->header.sourceidentity, + sizeof(fup.reqidentity)); + memcpy(fup.reqportindex, msg->header.sourceportindex, + sizeof(fup.reqportindex)); + + ret = ptp_sendmsg(state, &fup, sizeof(fup), &addr, sizeof(addr), NULL); + if (ret < 0) + { + ptperr("ptp sendmsg failed for Pdelay_Resp_Follow_Up: %d\n", errno); + return ret; } - else + + ptpinfo("Sent Pdelay_Resp_Follow_Up, seq %d\n", + ptp_get_sequence(&fup.header)); + + return OK; +} + +/* Process received peer delay response (requester role) */ + +static int ptp_process_pdelay_resp(FAR struct ptp_state_s *state, + FAR struct ptp_pdelay_resp_s *msg) +{ + uint16_t sequence; + + if (state->config->delay_mechanism != PTP_DELAY_P2P) { - ptpwarn("Path delay out of range: %lld ns\n", - (long long)path_delay); + return OK; } - /* Calculate interval until next packet */ + if (memcmp(msg->reqidentity, state->own_identity.header.sourceidentity, + sizeof(msg->reqidentity)) != 0) + { + return OK; /* Not for us */ + } - if (msg->header.logmessageinterval <= 12) + sequence = ptp_get_sequence(&msg->header); + if (sequence != state->pdelay_req_seq) { - interval = (1 << msg->header.logmessageinterval); + ptpwarn("Ignoring out-of-sequence Pdelay_Resp (%d vs. expected %d)\n", + sequence, state->pdelay_req_seq); + return OK; + } + + /* Store t4 (local receive timestamp) and t2 (receipt timestamp + * from peer). + */ + + state->pdelayresp_rx_time = state->rxtime; + ptp_format_to_timespec(msg->requestreceipttimestamp, + &state->pdelayreq_rx_time); + ptp_add_correction_time(msg->header.correction, + &state->pdelayreq_rx_time); + + if (msg->header.flags[0] & PTP_FLAGS0_TWOSTEP) + { + state->pdelay_waiting_followup = true; + ptpinfo("Waiting for Pdelay_Resp_Follow_Up, seq %d\n", + sequence); } else { - interval = 4096; /* Refuse to obey excessively long intervals */ + /* One-step: turnaround time (t3 - t2) is carried in correctionField */ + + int64_t t4_t1_ns; + int64_t t3_t2_ns; + int64_t path_delay; + uint64_t correction_time; + + correction_time = (((uint64_t)msg->header.correction[0]) << 40) + | (((uint64_t)msg->header.correction[1]) << 32) + | (((uint64_t)msg->header.correction[2]) << 24) + | (((uint64_t)msg->header.correction[3]) << 16) + | (((uint64_t)msg->header.correction[4]) << 8) + | msg->header.correction[5]; + + t4_t1_ns = timespec_delta_ns(&state->pdelayresp_rx_time, + &state->pdelayreq_tx_time); + t3_t2_ns = correction_time; + path_delay = (t4_t1_ns - t3_t2_ns) / 2; + + ptp_record_path_delay(state, path_delay); } - /* Randomize up to 2x nominal delay) */ + return OK; +} - state->delayreq_interval = interval + (random() % interval); +/* Process received peer delay response follow-up (requester role) */ + +static int ptp_process_pdelay_resp_followup( + FAR struct ptp_state_s *state, + FAR struct ptp_pdelay_resp_follow_up_s *msg) +{ + struct timespec t3; + int64_t t4_t1_ns; + int64_t t3_t2_ns; + int64_t path_delay; + uint16_t sequence; + + if (state->config->delay_mechanism != PTP_DELAY_P2P || + !state->pdelay_waiting_followup) + { + return OK; + } + + if (memcmp(msg->reqidentity, state->own_identity.header.sourceidentity, + sizeof(msg->reqidentity)) != 0) + { + return OK; + } + + sequence = ptp_get_sequence(&msg->header); + if (sequence != state->pdelay_req_seq) + { + ptpwarn("Ignoring out-of-sequence Pdelay_Resp_Follow_Up " + "(%d vs. expected %d)\n", + sequence, state->pdelay_req_seq); + return OK; + } + + state->pdelay_waiting_followup = false; + + ptp_format_to_timespec(msg->responseorigintimestamp, &t3); + ptp_add_correction_time(msg->header.correction, &t3); + + /* IEEE 1588-2008 §11.4.3: meanPathDelay = ((t4 - t1) - (t3 - t2)) / 2 */ + + t4_t1_ns = timespec_delta_ns(&state->pdelayresp_rx_time, + &state->pdelayreq_tx_time); + t3_t2_ns = timespec_delta_ns(&t3, &state->pdelayreq_rx_time); + path_delay = (t4_t1_ns - t3_t2_ns) / 2; + + ptp_record_path_delay(state, path_delay); return OK; } @@ -1524,6 +2256,22 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, ptp_get_sequence(&state->rxbuf.header)); return ptp_process_delay_req(state, &state->rxbuf.delay_req); + case PTP_MSGTYPE_PDELAY_REQ: + ptpinfo("Got pdelay req, seq %d\n", + ptp_get_sequence(&state->rxbuf.header)); + return ptp_process_pdelay_req(state, &state->rxbuf.pdelay_req); + + case PTP_MSGTYPE_PDELAY_RESP: + ptpinfo("Got pdelay resp, seq %d\n", + ptp_get_sequence(&state->rxbuf.header)); + return ptp_process_pdelay_resp(state, &state->rxbuf.pdelay_resp); + + case PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP: + ptpinfo("Got pdelay resp follow-up, seq %d\n", + ptp_get_sequence(&state->rxbuf.header)); + return ptp_process_pdelay_resp_followup( + state, &state->rxbuf.pdelay_resp_fup); + default: ptpwarn("Ignoring unknown PTP packet type: 0x%02x " "(masked: 0x%02x)\n", @@ -1546,7 +2294,11 @@ static void ptp_signal_handler(int signo, FAR siginfo_t *siginfo, } else if (signo == SIGUSR1) { +#ifdef CONFIG_BUILD_FLAT + state->status_req = siginfo->si_value.sival_ptr; +#else state->dump = true; +#endif } } @@ -1563,6 +2315,79 @@ static void ptp_setup_sighandlers(FAR struct ptp_state_s *state) sigaction(SIGUSR1, &act, NULL); } +/* Populate status information structure from current state */ + +static void ptp_populate_status(FAR struct ptp_state_s *state, + FAR struct ptpd_status_s *status) +{ + memset(status, 0, sizeof(*status)); + status->clock_source_valid = state->selected_source_valid; + + if (status->clock_source_valid) + { + FAR struct ptp_announce_s *s = &state->selected_source; + + memcpy(status->clock_source_info.id, + s->header.sourceidentity, + sizeof(status->clock_source_info.id)); + + status->clock_source_info.utcoffset = + (int16_t)(((uint16_t)s->utcoffset[0] << 8) | s->utcoffset[1]); + status->clock_source_info.priority1 = s->gm_priority1; + status->clock_source_info.clockclass = s->gm_quality[0]; + status->clock_source_info.accuracy = s->gm_quality[1]; + status->clock_source_info.priority2 = s->gm_priority2; + status->clock_source_info.variance = + ((uint16_t)s->gm_quality[2] << 8) | s->gm_quality[3]; + + memcpy(status->clock_source_info.gm_id, + s->gm_identity, + sizeof(status->clock_source_info.gm_id)); + + status->clock_source_info.stepsremoved = + ((uint16_t)s->stepsremoved[0] << 8) | s->stepsremoved[1]; + status->clock_source_info.timesource = s->timesource; + } + + status->last_clock_update = state->last_delta_timestamp; + status->last_delta_ns = state->last_delta_ns; + status->last_adjtime_ns = state->last_adjtime_ns; + status->drift_ppb = state->drift_ppb; + status->path_delay_ns = state->path_delay_ns; + + status->last_received_multicast = state->last_received_multicast; + status->last_received_announce = state->last_received_announce; + status->last_received_sync = state->last_received_sync; + status->last_transmitted_sync = state->last_transmitted_sync; + status->last_transmitted_announce = state->last_transmitted_announce; + status->last_transmitted_delayresp = state->last_transmitted_delayresp; + status->last_transmitted_delayreq = state->last_transmitted_delayreq; + status->last_transmitted_pdelayreq = state->last_transmitted_pdelayreq; +} + +#ifdef CONFIG_BUILD_FLAT +/* Process status information request in flat build mode */ + +static void ptp_process_statusreq(FAR struct ptp_state_s *state) +{ + FAR struct ptpd_statusreq_s *req = state->status_req; + + if (req == NULL) + { + return; /* No active request */ + } + + state->status_req = NULL; + ptp_populate_status(state, &req->dest); + + /* Post semaphore to inform that we are done. The request belongs to the + * caller of ptpd_status() and must not be touched after this. + */ + + sem_post(&req->done); +} +#else + /* Dump status to file when requested via signal. * Write atomically: temp file + rename. */ @@ -1581,48 +2406,7 @@ static void ptp_dump_status_file(FAR struct ptp_state_s *state) state->dump = false; - memset(&status, 0, sizeof(status)); - status.clock_source_valid = state->selected_source_valid; - - if (status.clock_source_valid) - { - FAR struct ptp_announce_s *s = &state->selected_source; - - memcpy(status.clock_source_info.id, - s->header.sourceidentity, - sizeof(status.clock_source_info.id)); - - status.clock_source_info.utcoffset = - (int16_t)(((uint16_t)s->utcoffset[0] << 8) | s->utcoffset[1]); - status.clock_source_info.priority1 = s->gm_priority1; - status.clock_source_info.clockclass = s->gm_quality[0]; - status.clock_source_info.accuracy = s->gm_quality[1]; - status.clock_source_info.priority2 = s->gm_priority2; - status.clock_source_info.variance = - ((uint16_t)s->gm_quality[2] << 8) | s->gm_quality[3]; - - memcpy(status.clock_source_info.gm_id, - s->gm_identity, - sizeof(status.clock_source_info.gm_id)); - - status.clock_source_info.stepsremoved = - ((uint16_t)s->stepsremoved[0] << 8) | s->stepsremoved[1]; - status.clock_source_info.timesource = s->timesource; - } - - status.last_clock_update = state->last_delta_timestamp; - status.last_delta_ns = state->last_delta_ns; - status.last_adjtime_ns = state->last_adjtime_ns; - status.drift_ppb = state->drift_ppb; - status.path_delay_ns = state->path_delay_ns; - - status.last_received_multicast = state->last_received_multicast; - status.last_received_announce = state->last_received_announce; - status.last_received_sync = state->last_received_sync; - status.last_transmitted_sync = state->last_transmitted_sync; - status.last_transmitted_announce = state->last_transmitted_announce; - status.last_transmitted_delayresp = state->last_transmitted_delayresp; - status.last_transmitted_delayreq = state->last_transmitted_delayreq; + ptp_populate_status(state, &status); snprintf(tmppath, sizeof(tmppath), "%s.tmp", CONFIG_NETUTILS_PTPD_STATUSFILE); @@ -1645,6 +2429,7 @@ static void ptp_dump_status_file(FAR struct ptp_state_s *state) unlink(tmppath); } } +#endif /**************************************************************************** * Public Functions @@ -1733,15 +2518,47 @@ int ptpd_start(FAR const struct ptpd_config_s *config) if (pollfds[0].revents) { - /* Receive time-critical packet, potentially with cmsg - * indicating the timestamp. +#ifdef CONFIG_NET_TIMESTAMP + if ((pollfds[0].revents & POLLERR) != 0) + { + char errbuf[128]; + char cmsgbuf[128]; + struct msghdr errhdr; + struct iovec erriov; + + memset(&errhdr, 0, sizeof(errhdr)); + erriov.iov_base = errbuf; + erriov.iov_len = sizeof(errbuf); + errhdr.msg_iov = &erriov; + errhdr.msg_iovlen = 1; + errhdr.msg_control = cmsgbuf; + errhdr.msg_controllen = sizeof(cmsgbuf); + + while (recvmsg(state->event_socket, &errhdr, + MSG_ERRQUEUE | MSG_DONTWAIT) > 0) + { + } + } +#endif + + /* Receive time-critical packet if POLLIN or POLLRDNORM + * is signaled. */ - ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT); - if (ret > 0) + if ((pollfds[0].revents & (POLLIN | POLLRDNORM)) != 0) { - ptp_getrxtime(state, &rxhdr, &state->rxtime); - ptp_process_rx_packet(state, ret); + while ((ret = recvmsg(state->event_socket, &rxhdr, + MSG_DONTWAIT)) > 0) + { + ptp_getrxtime(state, &rxhdr, &state->rxtime); + ptp_process_rx_packet(state, ret); + + rxhdr.msg_namelen = 0; + rxhdr.msg_iovlen = 1; + rxhdr.msg_controllen = sizeof(state->rxcmsg); + rxhdr.msg_flags = 0; + rxiov.iov_len = sizeof(state->rxbuf); + } } } @@ -1767,7 +2584,11 @@ int ptpd_start(FAR const struct ptpd_config_s *config) ptp_periodic_send(state); state->selected_source_valid = is_selected_source_valid(state); +#ifdef CONFIG_BUILD_FLAT + ptp_process_statusreq(state); +#else ptp_dump_status_file(state); +#endif } errout: @@ -1800,6 +2621,48 @@ int ptpd_start(FAR const struct ptpd_config_s *config) int ptpd_status(int pid, FAR struct ptpd_status_s *status) { +#ifdef CONFIG_BUILD_FLAT + int ret = OK; + union sigval val; + struct timespec timeout; + + memset(status, 0, sizeof(struct ptpd_status_s)); + + pthread_mutex_lock(&g_statusreq_lock); + + /* Drop the late answer to a request that timed out earlier */ + + while (sem_trywait(&g_statusreq.done) == 0) + { + } + + /* Send the status request */ + + val.sival_ptr = &g_statusreq; + + if (sigqueue(pid, SIGUSR1, val) != OK) + { + ret = -errno; + goto errout; + } + + /* Wait for status request to be handled */ + + clock_gettime(CLOCK_MONOTONIC, &timeout); + timeout.tv_sec += 1; + if (sem_clockwait(&g_statusreq.done, CLOCK_MONOTONIC, &timeout) != 0) + { + ret = -errno; + } + else + { + memcpy(status, &g_statusreq.dest, sizeof(struct ptpd_status_s)); + } + +errout: + pthread_mutex_unlock(&g_statusreq_lock); + return ret; +#else int fd; int ret; int elapsed; @@ -1846,6 +2709,7 @@ int ptpd_status(int pid, FAR struct ptpd_status_s *status) } return OK; +#endif } /**************************************************************************** diff --git a/netutils/ptpd/ptpv2.h b/netutils/ptpd/ptpv2.h index ebedfa397c1..2540ef0d4cd 100644 --- a/netutils/ptpd/ptpv2.h +++ b/netutils/ptpd/ptpv2.h @@ -42,9 +42,17 @@ #define PTP_UDP_PORT_EVENT 319 #define PTP_UDP_PORT_INFO 320 -/* Multicast address to send to: 224.0.1.129 */ +/* Multicast addresses to send to: 224.0.1.129 (primary) and + * 224.0.0.107 (peer delay). + */ + +#define PTP_MULTICAST_ADDR ((in_addr_t)0xE0000181) +#define PTP_PDELAY_MULTICAST_ADDR ((in_addr_t)0xE000006B) + +/* IEEE 1588-2008 Annex F Multicast MAC Addresses */ -#define PTP_MULTICAST_ADDR ((in_addr_t)0xE0000181) +#define PTP_MULTICAST_MAC { 0x01, 0x1b, 0x19, 0x00, 0x00, 0x00 } +#define PTP_PDELAY_MULTICAST_MAC { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e } /* PTP over Ethernet (IEEE 802.3 / Layer 2) EtherType */ @@ -54,12 +62,15 @@ /* Message types */ -#define PTP_MSGTYPE_MASK 0x0F -#define PTP_MSGTYPE_SYNC 0 -#define PTP_MSGTYPE_DELAY_REQ 1 -#define PTP_MSGTYPE_FOLLOW_UP 8 -#define PTP_MSGTYPE_DELAY_RESP 9 -#define PTP_MSGTYPE_ANNOUNCE 11 +#define PTP_MSGTYPE_MASK 0x0F +#define PTP_MSGTYPE_SYNC 0 +#define PTP_MSGTYPE_DELAY_REQ 1 +#define PTP_MSGTYPE_PDELAY_REQ 2 +#define PTP_MSGTYPE_PDELAY_RESP 3 +#define PTP_MSGTYPE_FOLLOW_UP 8 +#define PTP_MSGTYPE_DELAY_RESP 9 +#define PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP 0x0A +#define PTP_MSGTYPE_ANNOUNCE 11 /* Message flags */ @@ -151,4 +162,33 @@ begin_packed_struct struct ptp_delay_resp_s uint8_t reqportindex[2]; } end_packed_struct; +/* PdelayReq: request peer delay measurement */ + +begin_packed_struct struct ptp_pdelay_req_s +{ + struct ptp_header_s header; + uint8_t origintimestamp[10]; + uint8_t reserved[10]; +} end_packed_struct; + +/* PdelayResp: response to PdelayReq */ + +begin_packed_struct struct ptp_pdelay_resp_s +{ + struct ptp_header_s header; + uint8_t requestreceipttimestamp[10]; + uint8_t reqidentity[8]; + uint8_t reqportindex[2]; +} end_packed_struct; + +/* PdelayRespFollowUp: actual transmit timestamp of PdelayResp */ + +begin_packed_struct struct ptp_pdelay_resp_follow_up_s +{ + struct ptp_header_s header; + uint8_t responseorigintimestamp[10]; + uint8_t reqidentity[8]; + uint8_t reqportindex[2]; +} end_packed_struct; + #endif /* __APPS_NETUTILS_PTPD_PTPV2_H */ diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 8496a5365c6..3347f6c2d4b 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -123,6 +123,8 @@ static int do_ptpd_status(int pid) (intmax_t)(time_now.tv_sec - status.last_transmitted_delayresp.tv_sec)); printf("- last_transmitted_delayreq: %jd s ago\n", (intmax_t)(time_now.tv_sec - status.last_transmitted_delayreq.tv_sec)); + printf("- last_transmitted_pdelayreq: %jd s ago\n", + (intmax_t)(time_now.tv_sec - status.last_transmitted_pdelayreq.tv_sec)); return EXIT_SUCCESS; } @@ -159,8 +161,11 @@ static void usage(FAR const char *progname) " -B The best master clock algorithm is used\n" " -r synchronize system (realtime) clock\n" " -E E2E, support client delay request-response\n" + " -P P2P, support peer delay request-response\n" " -i [dev] interface device to use, for example 'eth0'\n" " -p [dev] clock device to use\n" + " -I [ns] hardware RX timestamp latency to compensate\n" + " -O [ns] hardware TX timestamp latency to compensate\n" " -t [pid] look the status of ptp daemon\n" " -d [pid] stop ptp daemon\n", progname); @@ -184,7 +189,7 @@ int main(int argc, FAR char *argv[]) config.interface = "eth0"; config.clock = "realtime"; config.client_only = false; - config.delay_e2e = false; + config.delay_mechanism = PTP_DELAY_NONE; #ifdef CONFIG_NET_TIMESTAMP config.hardware_ts = true; #else @@ -192,8 +197,14 @@ int main(int argc, FAR char *argv[]) #endif config.bmca = false; config.af = AF_INET; + config.ingress_latency_ns = CONFIG_NETUTILS_PTPD_INGRESS_LATENCY_NS; +#ifdef CONFIG_NET_TIMESTAMP + config.egress_latency_ns = CONFIG_NETUTILS_PTPD_EGRESS_LATENCY_NS; +#else + config.egress_latency_ns = 0; +#endif - while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHS")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:I:O:rs246BEHSP")) != ERROR) { switch (option) { @@ -217,7 +228,22 @@ int main(int argc, FAR char *argv[]) config.bmca = true; break; case 'E': - config.delay_e2e = true; + if (config.delay_mechanism != PTP_DELAY_NONE) + { + usage(argv[0]); + return EXIT_FAILURE; + } + + config.delay_mechanism = PTP_DELAY_E2E; + break; + case 'P': + if (config.delay_mechanism != PTP_DELAY_NONE) + { + usage(argv[0]); + return EXIT_FAILURE; + } + + config.delay_mechanism = PTP_DELAY_P2P; break; #ifdef CONFIG_NET_TIMESTAMP case 'H': @@ -233,6 +259,12 @@ int main(int argc, FAR char *argv[]) case 'p': config.clock = optarg; break; + case 'I': + config.ingress_latency_ns = atoi(optarg); + break; + case 'O': + config.egress_latency_ns = atoi(optarg); + break; case 'r': config.clock = "realtime"; break; @@ -242,5 +274,26 @@ int main(int argc, FAR char *argv[]) } } +#ifndef CONFIG_SCHED_TICKLESS + if (config.delay_mechanism == PTP_DELAY_P2P) + { + /* Without a tickless (hardware timer-backed) clock, clock_gettime() + * only advances once per CONFIG_USEC_PER_TICK scheduler tick, with + * no interpolation. The P2P peer delay formula subtracts two local + * timestamps (t1, t4) captured microseconds apart on a link this + * fast, which almost always fall inside the same tick: (t4 - t1) + * comes out exactly 0, or a full tick jump on the rare occasions a + * tick boundary falls in between. Either way path_delay_ns will be + * rejected as out of range and never converge. + */ + + fprintf(stderr, + "WARNING: P2P (-P) selected without CONFIG_SCHED_TICKLESS. " + "path_delay_ns measurements require a tickless " + "(hardware timer-backed) clock and will likely never " + "converge on this build.\n"); + } +#endif + return do_ptpd_start(&config); }