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/7] 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/7] 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/7] 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/7] 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/7] 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 6ec43413b476f2aaa7271819d8e30d35ab4e670f Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Fri, 18 Sep 2026 10:10:53 -0300 Subject: [PATCH 6/7] 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 | 205 +++++++++++++++++++++++++++++++++---------- 1 file changed, 161 insertions(+), 44 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index ae56660b663..8b8adf9a241 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include #include #include @@ -67,6 +69,16 @@ * 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 @@ -74,7 +86,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 */ @@ -181,6 +197,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 ****************************************************************************/ @@ -1968,7 +2003,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 } } @@ -1985,68 +2024,98 @@ 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; - - if (!state->dump) - { - return; - } - - state->dump = false; - - memset(&status, 0, sizeof(status)); - status.clock_source_valid = state->selected_source_valid; + 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) +{ + FAR struct ptpd_statusreq_s *req = state->status_req; + + if (req == NULL) + { + 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; + state->status_req = NULL; + ptp_populate_status(state, &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. The request belongs to the + * caller of ptpd_status() and must not be touched after this. + */ - status.last_transmitted_pdelayreq = state->last_transmitted_pdelayreq; + sem_post(&req->done); +} +#else + +/* 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); @@ -2069,6 +2138,7 @@ static void ptp_dump_status_file(FAR struct ptp_state_s *state) unlink(tmppath); } } +#endif /**************************************************************************** * Public Functions @@ -2191,7 +2261,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: @@ -2224,6 +2298,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; @@ -2270,6 +2386,7 @@ int ptpd_status(int pid, FAR struct ptpd_status_s *status) } return OK; +#endif } /**************************************************************************** From 5a3584dfc23eead6e1eadaa16ecd46393960fdaa Mon Sep 17 00:00:00 2001 From: "Daniel P. Carvalho" Date: Sat, 19 Sep 2026 17:35:04 -0300 Subject: [PATCH 7/7] 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 8b8adf9a241..969cbc3524f 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -65,6 +65,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. @@ -1241,6 +1263,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. */ @@ -1286,6 +1371,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) { @@ -1312,6 +1402,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