diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 1d455c6be..ae397d57c 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 8b1ac10f9..162ba98f7 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,37 +1580,7 @@ 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; - - if (max_path_delay < 10 * (int64_t)NSEC_PER_MSEC) - { - /* 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; - } - - 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: %ld ns (avg: %ld ns)\n", - (long)path_delay, (long)state->path_delay_ns); - } - else - { - ptpwarn("Path delay out of range: %lld ns\n", - (long long)path_delay); - } + ptp_record_path_delay(state, path_delay); /* Calculate interval until next packet */ @@ -1446,6 +1600,206 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, 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; + + if (state->config->delay_mechanism != PTP_DELAY_P2P) + { + return OK; + } + + 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) + { + ptperr("ptp sendmsg failed for Pdelay_Resp: %d\n", errno); + return ret; + } + + 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); + + 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; + } + + 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) + { + return OK; + } + + if (memcmp(msg->reqidentity, state->own_identity.header.sourceidentity, + sizeof(msg->reqidentity)) != 0) + { + return OK; /* Not for us */ + } + + sequence = ptp_get_sequence(&msg->header); + if (sequence != state->pdelay_req_seq) + { + 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 + { + /* 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); + } + + return OK; +} + +/* 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; +} + /* Determine received packet type and process it */ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, @@ -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 ebedfa397..2540ef0d4 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_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_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 8496a5365..516b75b5f 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); }