From f399182474b1301139e99b0fe392552990baf0a0 Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sun, 13 Sep 2026 12:10:37 -0300 Subject: [PATCH 1/9] netutils/ptpd: Fix Delay_Resp consumption by sendmsg. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ptp_sendmsg() called a blocking recvmsg(state->tx_socket, ...) right after sending a Delay_Req whenever hardware_ts was set, assuming a Linux-style MSG_ERRQUEUE/loopback semantics NuttX does not have. Since tx_socket and event_socket share the same underlying connection, this call instead blocked on and consumed whatever PTP packet arrived next on the wire — almost always the Delay_Resp, which typically arrives within milliseconds of the request. Its payload was read into a local buffer that went out of scope on return, so the packet never reached ptp_process_rx_packet() and path_delay_ns stayed at 0 in -H mode. t3 is now captured locally via ptp_gettime(), the same way -S mode already did, until hardware TX timestamping is supported. Also replaces the path delay heuristic in ptp_process_delay_resp() (which derived an approximation of (t2-t1) from path_delay_ns and last_delta_ns, only valid once the clock had already converged) with the canonical IEEE 1588-2008 §11.3 formula: store (t2-t1) directly from Sync/Follow_Up as sync_diff_ns, then average it with (t4-t3) from the Delay_Req/Delay_Resp exchange. Relaxes the path delay ceiling to 10ms unconditionally, since Delay_Req's t3 is software- timestamped in both modes until hardware TX timestamping is supported. Assisted-by: Claude:claude-sonnet-5 Assisted-by: Gemini:gemini-3.8-flash-medium Signed-off-by: Daniel P. Carvalho --- netutils/ptpd/ptpd.c | 99 ++++++++++++++++++++++---------------------- 1 file changed, 49 insertions(+), 50 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index e528da8a977..8b1ac10f9bf 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -139,6 +139,8 @@ struct ptp_state_s int path_delay_avgcount; long path_delay_ns; long delayreq_interval; + int64_t sync_diff_ns; + bool sync_diff_valid; /* Latest received packet and its timestamp (CLOCK_REALTIME) */ @@ -788,26 +790,13 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, { return ERROR; } - - if (state->config->hardware_ts && sendts != NULL) - { - 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) - { - ptp_getrxtime(state, &msg, sendts); - } - } } else { ret = sendto(state->tx_socket, buf, buflen, 0, addr, addrlen); } - if (!state->config->hardware_ts && sendts != NULL) + if (sendts != NULL) { ptp_gettime(state, sendts); } @@ -1206,6 +1195,31 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, return ret; } +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); +} + /* Process received PTP sync packet */ static int ptp_process_sync(FAR struct ptp_state_s *state, @@ -1240,34 +1254,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 +1293,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); @@ -1356,7 +1354,6 @@ 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; @@ -1371,10 +1368,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,22 +1388,21 @@ 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; - if (!state->config->hardware_ts && - max_path_delay < 10 * (int64_t)NSEC_PER_MSEC) + if (max_path_delay < 10 * (int64_t)NSEC_PER_MSEC) { - /* Software timestamping includes network stack and OS latency, - * allow up to 10 ms. + /* Software TX latency on Delay_Req transmission can add up to + * several milliseconds. Allow up to 10 ms until hardware TX + * timestamping is available. */ max_path_delay = 10 * (int64_t)NSEC_PER_MSEC; From 4ef70d5ccb6b38de87d233ef61d0fd1cf7adaeb5 Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sun, 13 Sep 2026 13:06:36 -0300 Subject: [PATCH 2/9] netutils/ptpd: Implement IEEE 1588 peer-to-peer (P2P) delay mechanism. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implements the Peer-to-Peer (P2P) transparent clock delay measurement mechanism (IEEE 1588-2008 §11.4 / IEEE 802.1AS / IEC/IEEE 61850-9-3) in apps/netutils/ptpd: - Add PTP_MSGTYPE_PDELAY_REQ, PTP_MSGTYPE_PDELAY_RESP, and PTP_MSGTYPE_PDELAY_RESP_FOLLOW_UP definitions and structs in ptpv2.h. - Define IEEE 1588-2008 Annex F peer delay multicast MAC address 01:80:c2:00:00:0e and Annex D peer delay IP address 224.0.0.107. - Replace bool delay_e2e with enum ptp_delay_mechanism_e (PTP_DELAY_NONE, PTP_DELAY_E2E, PTP_DELAY_P2P) in include/netutils/ptpd.h. - Add -P CLI option in system/ptpd/ptpd_main.c with mutual exclusion check against -E, and display last_transmitted_pdelayreq in status. - Implement responder logic in ptp_process_pdelay_req() sending Pdelay_Resp (t2) and Pdelay_Resp_Follow_Up (t3) regardless of master or slave state. - Implement requester logic in ptp_send_pdelay_req() gated on the physical link without requiring prior BMCA master selection. - Implement ptp_process_pdelay_resp() and ptp_process_pdelay_resp_followup() using canonical mean path delay formula ((t4 - t1) - (t3 - t2)) / 2. - Refactor path delay bounds checking and moving average filter into ptp_record_path_delay() shared across E2E and P2P mechanisms. - Set PTP version 2.0 and controlField 0x05 in Pdelay_Req, Pdelay_Resp and Pdelay_Resp_Follow_Up, and in the own-identity header, so that peers such as linuxptp accept the messages. - Clear pdelay_waiting_followup when a new Pdelay_Req is sent, so an orphaned Pdelay_Resp_Follow_Up from an abandoned cycle is not paired with stale timestamps. - Warn at startup when P2P is selected without CONFIG_SCHED_TICKLESS, since a tick-driven clock cannot resolve the peer delay. - Skip IP multicast join/leave handling for AF_PACKET. Assisted-by: Claude:claude-sonnet-5 Assisted-by: Gemini:gemini-3.8-flash-medium Signed-off-by: Daniel P. Carvalho --- include/netutils/ptpd.h | 10 +- netutils/ptpd/ptpd.c | 480 +++++++++++++++++++++++++++++++++++----- netutils/ptpd/ptpv2.h | 56 ++++- system/ptpd/ptpd_main.c | 45 +++- 4 files changed, 525 insertions(+), 66 deletions(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 1d455c6be70..ae397d57cda 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -37,13 +37,20 @@ * 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; }; @@ -106,6 +113,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/ptpd.c b/netutils/ptpd/ptpd.c index 8b1ac10f9bf..162ba98f7ef 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -26,6 +26,7 @@ #include +#include #include #include @@ -100,6 +101,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 */ @@ -131,6 +133,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) */ @@ -142,18 +145,28 @@ struct ptp_state_s 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))]; @@ -481,9 +494,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) { @@ -656,6 +679,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. @@ -669,8 +705,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]; @@ -713,6 +750,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, @@ -729,9 +772,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 @@ -749,22 +806,36 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, 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); @@ -937,6 +1008,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) @@ -969,18 +1092,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) { @@ -988,6 +1112,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; } @@ -1007,9 +1150,12 @@ 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; + } } } @@ -1350,6 +1496,45 @@ 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 >= 0 && path_delay < max_path_delay) + { + 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) { @@ -1357,7 +1542,6 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, struct timespec remote_rxtime; uint16_t sequence; int interval; - int64_t max_path_delay; bool source_match; bool request_match; @@ -1396,52 +1580,222 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, path_delay = timespec_delta_ns(&remote_rxtime, &state->delayreq_time); 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 (max_path_delay < 10 * (int64_t)NSEC_PER_MSEC) + /* Calculate interval until next packet */ + + if (msg->header.logmessageinterval <= 12) { - /* Software TX latency on Delay_Req transmission can add up to - * several milliseconds. Allow up to 10 ms until hardware TX - * timestamping is available. - */ + 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) */ + +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; - max_path_delay = 10 * (int64_t)NSEC_PER_MSEC; + 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 */ + + addr.sin_port = HTONS(PTP_UDP_PORT_INFO); - ptpinfo("Path delay: %ld ns (avg: %ld ns)\n", - (long)path_delay, (long)state->path_delay_ns); + 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; } @@ -1523,6 +1877,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", @@ -1623,6 +1993,8 @@ static void ptp_dump_status_file(FAR struct ptp_state_s *state) status.last_transmitted_delayresp = state->last_transmitted_delayresp; status.last_transmitted_delayreq = state->last_transmitted_delayreq; + status.last_transmitted_pdelayreq = state->last_transmitted_pdelayreq; + snprintf(tmppath, sizeof(tmppath), "%s.tmp", CONFIG_NETUTILS_PTPD_STATUSFILE); 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..516b75b5f77 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,6 +161,7 @@ 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" " -t [pid] look the status of ptp daemon\n" @@ -184,7 +187,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 @@ -193,7 +196,7 @@ int main(int argc, FAR char *argv[]) config.bmca = false; config.af = AF_INET; - while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHS")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHSP")) != ERROR) { switch (option) { @@ -217,7 +220,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': @@ -242,5 +260,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); } From 9daebfcaa4a0d5f8bfd2f2a096200193025fe58b Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Wed, 16 Sep 2026 15:19:41 -0300 Subject: [PATCH 3/9] netutils/ptpd: discard implausible drift-rate samples before averaging A single drift-rate sample computed between two consecutive sync updates was clamped against CLOCK_ADJTIME_SLEWLIMIT_PPM - the hardware's slew-rate safety limit, not a bound on how large a real crystal-oscillator drift measurement can plausibly be. 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) could therefore produce a wildly implausible sample that still passed the check and corrupted the long-term drift_ppb average. Add CONFIG_NETUTILS_PTPD_MAX_DRIFT_PPB (default 500000, well above any real crystal's few-hundred-ppm drift) as a dedicated plausibility bound, intentionally much tighter than CLOCK_ADJTIME_SLEWLIMIT_PPM. A sample outside this bound is discarded and the previous averaged drift_ppb is kept unchanged instead of being corrupted. Assisted-by: Claude:claude-sonnet-5 Signed-off-by: Daniel P. Carvalho --- netutils/ptpd/Kconfig | 20 ++++++++++++++++++++ netutils/ptpd/ptpd.c | 15 ++++++++++++--- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index b27143ba2ce..0bd2389fd90 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 diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 162ba98f7ef..23b2180d5ba 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -1232,8 +1232,6 @@ 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 (!state->has_last_delta) { @@ -1269,8 +1267,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; From acee716af612684d36c3d55eb0820aee3e8bb36d Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Thu, 17 Sep 2026 17:50:44 -0300 Subject: [PATCH 4/9] netutils/ptpd: phase-lock hardware clock via POSIX clock_adjtime. When state->clockid is configured to a hardware PTP clock device (e.g., /dev/ptp0) instead of CLOCK_REALTIME, ptp_adjtime() previously passed only the measured frequency drift (-ppb) to clock_adjtime(), ignoring the residual phase offset (delta_ns / adjustment_ns). As a result, while the hardware counter tracked frequency, its phase was never pulled into alignment with the master clock. Convert delta_ns (which combines frequency drift and current phase error clamped to max_adjust_ns) to ppb over CONFIG_CLOCK_ADJTIME_PERIOD_MS, acting as a proportional-integral (PI) phase servo. This drives the hardware clock to phase lock with the master via POSIX clock_adjtime() using ADJ_FREQUENCY without requiring proprietary ioctl calls. Assisted-by: Claude:claude-sonnet-5 Assisted-by: Gemini:gemini-3.8-flash-medium Signed-off-by: Daniel P. Carvalho --- netutils/ptpd/ptpd.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 23b2180d5ba..e8d08c5d3b8 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -30,9 +30,10 @@ #include #include -#include #include #include +#include +#include #include #include @@ -441,9 +442,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); From ea537f71ce70cdaefca9adea6f605c689a4f98e0 Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sat, 19 Sep 2026 14:40:40 -0300 Subject: [PATCH 5/9] netutils/ptpd: Add ingress latency compensation for RX timestamps. The MAC latches a hardware receive timestamp later than the frame reaches the wire, because of the PHY and the clock domain crossing. This fixed delay is the ingressLatency port parameter of IEEE 1588 and shows up as a constant phase error between the local and the master clock. Subtract the configured latency from every hardware receive timestamp in ptp_getrxtime(), the single place where they enter the daemon, so Sync, Delay_Resp and the peer delay messages are all corrected. - Add CONFIG_NETUTILS_PTPD_INGRESS_LATENCY_NS (default 0, which applies no compensation). - Add the -I option to override it at run time. - Add ingress_latency_ns to struct ptpd_config_s. Software timestamps are not affected. Assisted-by: Claude:claude-sonnet-5 Signed-off-by: Daniel P. Carvalho --- include/netutils/ptpd.h | 1 + netutils/ptpd/Kconfig | 20 ++++++++++++++++++++ netutils/ptpd/ptpd.c | 21 +++++++++++++++++++++ system/ptpd/ptpd_main.c | 7 ++++++- 4 files changed, 48 insertions(+), 1 deletion(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index ae397d57cda..f418e689287 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -53,6 +53,7 @@ struct ptpd_config_s enum ptp_delay_mechanism_e delay_mechanism; bool bmca; sa_family_t af; + int32_t ingress_latency_ns; /* Hardware RX timestamp latency (ns) */ }; /* PTPD status information structure */ diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 0bd2389fd90..fc26e821bd9 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -219,4 +219,24 @@ 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. + endif # NETUTILS_PTPD diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index e8d08c5d3b8..ae56660b663 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -314,6 +314,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. */ @@ -500,6 +516,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; } } diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 516b75b5f77..370f915e12b 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -164,6 +164,7 @@ static void usage(FAR const char *progname) " -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" " -t [pid] look the status of ptp daemon\n" " -d [pid] stop ptp daemon\n", progname); @@ -195,8 +196,9 @@ 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; - while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHSP")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:I:rs246BEHSP")) != ERROR) { switch (option) { @@ -251,6 +253,9 @@ int main(int argc, FAR char *argv[]) case 'p': config.clock = optarg; break; + case 'I': + config.ingress_latency_ns = atoi(optarg); + break; case 'r': config.clock = "realtime"; break; From 37ecf708c6acd7726d1c6f8a337eec8fe7338245 Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Fri, 18 Sep 2026 10:10:53 -0300 Subject: [PATCH 6/9] netutils/ptpd: retain in-memory IPC for CONFIG_BUILD_FLAT in ptpd_status() PR #3789 replaced the in-memory sigqueue + shared memory IPC in ptpd_status() with file-based IPC to support Protected and Kernel modes across address spaces. However, on microcontrollers running CONFIG_BUILD_FLAT, a filesystem or /tmp (TMPFS) is rarely mounted or available, causing ptpd_status() to fail with -ETIMEDOUT (errno 110) because the status file cannot be created. Retain the file-based IPC for !CONFIG_BUILD_FLAT (Protected and Kernel modes) while restoring the zero-overhead in-memory sigqueue + semaphore IPC for CONFIG_BUILD_FLAT. Both modes share the status serialization logic via ptp_populate_status() and support all fields including P2P. Assisted-by: Gemini:gemini-3.8-flash-medium Signed-off-by: Daniel P. Carvalho --- netutils/ptpd/ptpd.c | 179 ++++++++++++++++++++++++++++++++----------- 1 file changed, 135 insertions(+), 44 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index ae56660b663..1ddc8346ce6 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -42,6 +42,9 @@ #include #include #include +#ifdef CONFIG_BUILD_FLAT +#include +#endif #include #include #include @@ -67,6 +70,16 @@ * Private Types ****************************************************************************/ +#ifdef CONFIG_BUILD_FLAT +/* Carrier structure for querying PTPD status in flat build mode */ + +struct ptpd_statusreq_s +{ + FAR sem_t *done; + FAR struct ptpd_status_s *dest; +}; +#endif + /* Main PTPD state storage */ struct ptp_state_s @@ -75,6 +88,9 @@ struct ptp_state_s bool stop; bool dump; /* Set by SIGUSR1, checked in main loop */ +#ifdef CONFIG_BUILD_FLAT + struct ptpd_statusreq_s status_req; +#endif /* Address of network interface we are operating on */ @@ -1966,6 +1982,14 @@ static void ptp_signal_handler(int signo, FAR siginfo_t *siginfo, { state->stop = true; } +#ifdef CONFIG_BUILD_FLAT + else if (signo == SIGUSR1 && siginfo != NULL && + siginfo->si_value.sival_ptr != NULL) + { + state->status_req = + *(FAR struct ptpd_statusreq_s *)siginfo->si_value.sival_ptr; + } +#endif else if (signo == SIGUSR1) { state->dump = true; @@ -1985,68 +2009,99 @@ static void ptp_setup_sighandlers(FAR struct ptp_state_s *state) sigaction(SIGUSR1, &act, NULL); } -/* Dump status to file when requested via signal. - * Write atomically: temp file + rename. - */ +/* Populate status information structure from current state */ -static void ptp_dump_status_file(FAR struct ptp_state_s *state) +static void ptp_populate_status(FAR struct ptp_state_s *state, + FAR struct ptpd_status_s *status) { - struct ptpd_status_s status; - char tmppath[64]; - int fd; - int ret; + memset(status, 0, sizeof(*status)); + status->clock_source_valid = state->selected_source_valid; - if (!state->dump) - { - return; - } - - state->dump = false; - - memset(&status, 0, sizeof(status)); - status.clock_source_valid = state->selected_source_valid; - - if (status.clock_source_valid) + if (status->clock_source_valid) { FAR struct ptp_announce_s *s = &state->selected_source; - memcpy(status.clock_source_info.id, + memcpy(status->clock_source_info.id, s->header.sourceidentity, - sizeof(status.clock_source_info.id)); + sizeof(status->clock_source_info.id)); - status.clock_source_info.utcoffset = + 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 = + 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, + memcpy(status->clock_source_info.gm_id, s->gm_identity, - sizeof(status.clock_source_info.gm_id)); + sizeof(status->clock_source_info.gm_id)); - status.clock_source_info.stepsremoved = + status->clock_source_info.stepsremoved = ((uint16_t)s->stepsremoved[0] << 8) | s->stepsremoved[1]; - status.clock_source_info.timesource = s->timesource; + 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) +{ + if (!state->status_req.dest) + { + return; /* No active request */ } - 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; + ptp_populate_status(state, state->status_req.dest); - 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; + /* Post semaphore to inform that we are done */ - status.last_transmitted_pdelayreq = state->last_transmitted_pdelayreq; + if (state->status_req.done) + { + sem_post(state->status_req.done); + } + + state->status_req.done = NULL; + state->status_req.dest = NULL; +} +#endif + +/* Dump status to file when requested via signal. + * Write atomically: temp file + rename. + */ + +static void ptp_dump_status_file(FAR struct ptp_state_s *state) +{ + struct ptpd_status_s status; + char tmppath[64]; + int fd; + int ret; + + if (!state->dump) + { + return; + } + + state->dump = false; + + ptp_populate_status(state, &status); snprintf(tmppath, sizeof(tmppath), "%s.tmp", CONFIG_NETUTILS_PTPD_STATUSFILE); @@ -2191,6 +2246,9 @@ 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); +#endif ptp_dump_status_file(state); } @@ -2224,6 +2282,38 @@ 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; + sem_t donesem; + struct ptpd_statusreq_s req; + union sigval val; + struct timespec timeout; + + /* Fill in the status request */ + + memset(status, 0, sizeof(struct ptpd_status_s)); + sem_init(&donesem, 0, 0); + req.done = &donesem; + req.dest = status; + val.sival_ptr = &req; + + if (sigqueue(pid, SIGUSR1, val) != OK) + { + return -errno; + } + + /* Wait for status request to be handled */ + + clock_gettime(CLOCK_MONOTONIC, &timeout); + timeout.tv_sec += 1; + if (sem_clockwait(&donesem, CLOCK_MONOTONIC, &timeout) != 0) + { + ret = -errno; + } + + sem_destroy(&donesem); + return ret; +#else int fd; int ret; int elapsed; @@ -2270,6 +2360,7 @@ int ptpd_status(int pid, FAR struct ptpd_status_s *status) } return OK; +#endif } /**************************************************************************** From 09abb1ff960839f098c7f2d3dda77f686a4cf35a Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sat, 19 Sep 2026 17:35:04 -0300 Subject: [PATCH 7/9] netutils/ptpd: Discard outlier Sync phase error samples. A single Sync sample whose receive timestamp was taken late, for example because the task was scheduled late with software timestamping, was fed straight into the phase correction and the drift estimate, and could pull the clock away from the master. - Add CONFIG_NETUTILS_PTPD_OUTLIER_THRESHOLD_NS (default 0, which disables the check). A phase error that differs by more than this many nanoseconds from the median of the last five accepted samples is discarded, with a warning. - Accept the sample after eight consecutive rejections and restart the history from it, so that a real step of the master is still followed while a short burst of disturbed samples is ridden out. - Restart the history whenever the clock is stepped, since the old samples no longer describe the new time base. - With the default of 0 the behaviour is unchanged. Signed-off-by: Daniel P. Carvalho Assisted-by: Claude:claude-sonnet-5 --- netutils/ptpd/Kconfig | 20 +++++++++ netutils/ptpd/ptpd.c | 99 +++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 119 insertions(+) diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index fc26e821bd9..371497609c7 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -209,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" diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 1ddc8346ce6..8091a238a74 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -66,6 +66,22 @@ #include "netutils/netlib.h" #include "ptpv2.h" +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#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 ****************************************************************************/ @@ -128,6 +144,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. @@ -1222,6 +1244,69 @@ static int ptp_process_announce(FAR struct ptp_state_s *state, 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. */ @@ -1267,6 +1352,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) { @@ -1293,6 +1383,15 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, (int64_t)CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * CONFIG_CLOCK_ADJTIME_PERIOD_MS; +#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) { /* First measurement after jump or startup: no previous From 2361cc211560adb9f423fc6278602cca1aea9c3c Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sat, 19 Sep 2026 15:45:39 -0300 Subject: [PATCH 8/9] netutils/ptpd: Retrieve hardware TX timestamps via MSG_ERRQUEUE. Timestamp transmitted event messages with the hardware clock instead of reading the clock after sendmsg() returns. When hardware timestamping is selected on an AF_PACKET socket, request SOF_TIMESTAMPING_TX_HARDWARE for each event message, wait for the looped-back packet on the error queue with MSG_ERRQUEUE, and take the timestamp from its SO_TIMESTAMPING control message, as on Linux. Sync, Delay_Req and Pdelay_Req get their real departure time. - Use a transmit socket of its own, separate from the event socket, so the error queue is not shared with received packets. - Handle POLLERR separately from POLLIN in the main loop and drain all pending packets on each wakeup. - If the timestamp does not arrive, fall back to a software timestamp taken before the frame is sent. After three consecutive failures the driver is assumed not to provide hardware transmit timestamps, a warning is printed once and only software timestamps are used, so a driver without support does not stall the daemon. - Take the software timestamp before sending in every mode. It used to be taken after sendmsg() returned, so a fast peer's reply could appear to arrive before the request had left and give a negative delay. - Accept a measured path delay down to -100 microseconds and clamp it to zero, since hardware timestamps on both ends can make a short link measure slightly negative. Assisted-by: Claude:claude-sonnet-5 Signed-off-by: Daniel P. Carvalho --- netutils/ptpd/ptpd.c | 240 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 229 insertions(+), 11 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 8091a238a74..761ccd2fcdb 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -70,6 +70,12 @@ * 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 @@ -116,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; @@ -639,9 +652,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); @@ -652,6 +662,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) { @@ -880,11 +899,103 @@ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) return OK; } +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) +/**************************************************************************** + * 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; +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) + 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) { @@ -938,9 +1049,48 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, msg.msg_control = NULL; msg.msg_controllen = 0; +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) + if (do_hwts) + { + 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 + ret = sendmsg(state->tx_socket, &msg, 0); if (ret < 0) { +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) + if (do_hwts) + { + int val = 0; + + setsockopt(state->tx_socket, SOL_SOCKET, SO_TIMESTAMPING, + &val, sizeof(val)); + } + +#endif return ERROR; } } @@ -951,7 +1101,38 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, if (sendts != NULL) { - ptp_gettime(state, sendts); +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) + if (do_hwts) + { + int val = 0; + + if (ptp_get_tx_timestamp(state, sendts) == OK) + { + state->hwts_tx_failures = 0; + } + 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; @@ -1683,8 +1864,13 @@ static void ptp_record_path_delay(FAR struct ptp_state_s *state, max_path_delay = 10 * NSEC_PER_MSEC; } - if (path_delay >= 0 && path_delay < max_path_delay) + 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) { @@ -2311,15 +2497,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. +#if defined(CONFIG_NET_TIMESTAMP) && defined(SO_TIMESTAMPING) + 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); + } } } From ef70201ab8beefafe649b23c2378c74ab9ffb826 Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sat, 19 Sep 2026 15:45:39 -0300 Subject: [PATCH 9/9] netutils/ptpd: Add egress latency compensation for TX timestamps. The frame leaves the MAC later than the moment its hardware transmit timestamp is latched, because of the clock domain crossing and the PHY. This fixed delay is the egressLatency port parameter of IEEE 1588. Add the configured latency to every hardware transmit timestamp obtained through MSG_ERRQUEUE, the counterpart of the ingress compensation. - Add CONFIG_NETUTILS_PTPD_EGRESS_LATENCY_NS (default 0, which applies no compensation). - Add the -O option to override it at run time. - Add egress_latency_ns to struct ptpd_config_s. Software timestamps are not affected. Assisted-by: Claude:claude-sonnet-5 Signed-off-by: Daniel P. Carvalho --- include/netutils/ptpd.h | 1 + netutils/ptpd/Kconfig | 21 +++++++++++++++++++++ netutils/ptpd/ptpd.c | 6 ++++++ system/ptpd/ptpd_main.c | 11 ++++++++++- 4 files changed, 38 insertions(+), 1 deletion(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index f418e689287..c86f3d647ef 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -54,6 +54,7 @@ struct ptpd_config_s 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 */ diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 371497609c7..eb560d7b0a8 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -259,4 +259,25 @@ config NETUTILS_PTPD_INGRESS_LATENCY_NS 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 761ccd2fcdb..696584f2c33 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -1109,6 +1109,12 @@ static int ptp_sendmsg(FAR struct ptp_state_s *state, FAR const void *buf, 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 { diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 370f915e12b..3347f6c2d4b 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -165,6 +165,7 @@ static void usage(FAR const char *progname) " -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); @@ -197,8 +198,13 @@ int main(int argc, FAR char *argv[]) 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:I:rs246BEHSP")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:I:O:rs246BEHSP")) != ERROR) { switch (option) { @@ -256,6 +262,9 @@ int main(int argc, FAR char *argv[]) case 'I': config.ingress_latency_ns = atoi(optarg); break; + case 'O': + config.egress_latency_ns = atoi(optarg); + break; case 'r': config.clock = "realtime"; break;