From dc94b2418a9323cd06379da26e254b69e8468167 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Sat, 15 Mar 2025 13:53:43 +0800 Subject: [PATCH 01/16] netutils/ptpd: add CMakelist for ptpd support cmake build Signed-off-by: dongjiuzhu1 --- netutils/ptpd/CMakeLists.txt | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 netutils/ptpd/CMakeLists.txt diff --git a/netutils/ptpd/CMakeLists.txt b/netutils/ptpd/CMakeLists.txt new file mode 100644 index 00000000000..35c36b5b145 --- /dev/null +++ b/netutils/ptpd/CMakeLists.txt @@ -0,0 +1,23 @@ +# ############################################################################## +# apps/netutils/ptpd/CMakeLists.txt +# +# Licensed to the Apache Software Foundation (ASF) under one or more contributor +# license agreements. See the NOTICE file distributed with this work for +# additional information regarding copyright ownership. The ASF licenses this +# file to you under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. +# +# ############################################################################## + +if(CONFIG_NETUTILS_PTPD) + target_sources(apps PRIVATE ptpd.c) +endif() From 32ef6dc61225c8ede32cf2a14ba805f1ff95f4a3 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Sat, 15 Mar 2025 14:01:35 +0800 Subject: [PATCH 02/16] netutils/ptpd: byte align for ptp structure pass structure between remote and local core Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpv2.h | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/netutils/ptpd/ptpv2.h b/netutils/ptpd/ptpv2.h index 8b031567b74..3f452542848 100644 --- a/netutils/ptpd/ptpv2.h +++ b/netutils/ptpd/ptpv2.h @@ -27,6 +27,8 @@ * Included Files ****************************************************************************/ +#include + #include /**************************************************************************** @@ -67,7 +69,7 @@ /* Common header for all message types */ -struct ptp_header_s +begin_packed_struct struct ptp_header_s { uint8_t messagetype; uint8_t version; @@ -82,11 +84,11 @@ struct ptp_header_s uint8_t sequenceid[2]; uint8_t controlfield; uint8_t logmessageinterval; -}; +} end_packed_struct; /* Announce a master clock */ -struct ptp_announce_s +begin_packed_struct struct ptp_announce_s { struct ptp_header_s header; uint8_t origintimestamp[10]; @@ -98,40 +100,40 @@ struct ptp_announce_s uint8_t gm_identity[8]; uint8_t stepsremoved[2]; uint8_t timesource; -}; +} end_packed_struct; /* Sync: transmit timestamp from master clock */ -struct ptp_sync_s +begin_packed_struct struct ptp_sync_s { struct ptp_header_s header; uint8_t origintimestamp[10]; -}; +} end_packed_struct; /* FollowUp: actual timestamp of when sync message was sent */ -struct ptp_follow_up_s +begin_packed_struct struct ptp_follow_up_s { struct ptp_header_s header; uint8_t origintimestamp[10]; -}; +} end_packed_struct; /* DelayReq: request delay measurement */ -struct ptp_delay_req_s +begin_packed_struct struct ptp_delay_req_s { struct ptp_header_s header; uint8_t origintimestamp[10]; -}; +} end_packed_struct; /* DelayResp: response to DelayReq */ -struct ptp_delay_resp_s +begin_packed_struct struct ptp_delay_resp_s { struct ptp_header_s header; uint8_t receivetimestamp[10]; uint8_t reqidentity[8]; uint8_t reqportindex[2]; -}; +} end_packed_struct; #endif /* __APPS_NETUTILS_PTPD_PTPV2_H */ From 5aecb30da3cc850773736f10a2dc83c4a55f57c3 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Sun, 23 Mar 2025 23:05:52 +0800 Subject: [PATCH 03/16] netutils/ptpd: using DEBUG_PTP_ERR/WARN/INFO to replace NETUTILS_PTPD_DEBUG using new ptp clock device debug function Signed-off-by: dongjiuzhu1 --- netutils/ptpd/Kconfig | 7 ------- netutils/ptpd/ptpd.c | 10 ---------- 2 files changed, 17 deletions(-) diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 93cd288ca56..110223bd7c2 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -17,13 +17,6 @@ config NETUTILS_PTPD if NETUTILS_PTPD -config NETUTILS_PTPD_DEBUG - bool "Enable PTP debug messages" - default n - depends on DEBUG_INFO - ---help--- - Enable PTP debug messages even if CONFIG_DEBUG_NET_INFO is not enabled. - config NETUTILS_PTPD_CLIENT bool "Enable client support" default y diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 309f78a07f1..bdfae08a0dc 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -172,16 +172,6 @@ struct ptp_state_s * debugging without having excessive amount of logging from net. */ -#ifdef CONFIG_NETUTILS_PTPD_DEBUG -# define ptpinfo _info -# define ptpwarn _warn -# define ptperr _err -#else -# define ptpinfo ninfo -# define ptpwarn nwarn -# define ptperr nerr -#endif - /**************************************************************************** * Private Functions ****************************************************************************/ From e2737dd6f101f23538de05199eba7a9e0da3b42d Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Mon, 24 Mar 2025 16:10:35 +0800 Subject: [PATCH 04/16] system/ptpd: using main task to replace new task new starting command: ptpd start interface & we should run it in background ways Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 73 +++++++++++------------------------------ system/ptpd/ptpd_main.c | 17 ++++------ 2 files changed, 26 insertions(+), 64 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index bdfae08a0dc..411c9f4eb78 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -1376,11 +1376,27 @@ static void ptp_process_statusreq(FAR struct ptp_state_s *state) state->status_req.dest = NULL; } -/* Main PTPD task */ +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: ptpd_start + * + * Description: + * Start the PTP daemon and bind it to specified interface. + * + * Input Parameters: + * interface - Name of the network interface to bind to, e.g. "eth0" + * + * Returned Value: + * On success, the non-negative task ID of the PTP daemon is returned; + * On failure, a negated errno value is returned. + * + ****************************************************************************/ -static int ptp_daemon(int argc, FAR char** argv) +int ptpd_start(FAR const char *interface) { - FAR const char *interface = "eth0"; FAR struct ptp_state_s *state; struct pollfd pollfds[2]; struct msghdr rxhdr; @@ -1392,11 +1408,6 @@ static int ptp_daemon(int argc, FAR char** argv) state = calloc(1, sizeof(struct ptp_state_s)); - if (argc > 1) - { - interface = argv[1]; - } - if (ptp_initialize_state(state, interface) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); @@ -1477,52 +1488,6 @@ static int ptp_daemon(int argc, FAR char** argv) return 0; } -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: ptpd_start - * - * Description: - * Start the PTP daemon and bind it to specified interface. - * - * Input Parameters: - * interface - Name of the network interface to bind to, e.g. "eth0" - * - * Returned Value: - * On success, the non-negative task ID of the PTP daemon is returned; - * On failure, a negated errno value is returned. - * - ****************************************************************************/ - -int ptpd_start(FAR const char *interface) -{ - int pid; - FAR char *task_argv[] = - { - (FAR char *)interface, - NULL - }; - - pid = task_create("PTPD", CONFIG_NETUTILS_PTPD_SERVERPRIO, - CONFIG_NETUTILS_PTPD_STACKSIZE, ptp_daemon, task_argv); - - /* Use kill with signal 0 to check if the process is still alive - * after initialization. - */ - - usleep(USEC_PER_TICK); - if (kill(pid, 0) != OK) - { - return ERROR; - } - else - { - return pid; - } -} - /**************************************************************************** * Name: ptpd_status * diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 503300bcef5..36b8b67c6a2 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -37,17 +37,14 @@ static int do_ptpd_start(FAR const char *interface) { - int pid; + int ret; - pid = ptpd_start(interface); - if (pid < 0) - { - fprintf(stderr, "ERROR: ptpd_start() failed\n"); - return EXIT_FAILURE; - } + ret = ptpd_start(interface); - printf("Started the PTP daemon as PID=%d\n", pid); - return EXIT_SUCCESS; + /* Should never happen */ + + fprintf(stderr, "ERROR: ptpd_start() failed:%d\n", ret); + return EXIT_FAILURE; } static int do_ptpd_status(int pid) @@ -168,7 +165,7 @@ int main(int argc, FAR char *argv[]) else { fprintf(stderr, "Usage: \n" - "ptpd start \n" + "ptpd start &\n" "ptpd status \n" "ptpd stop \n"); return EXIT_FAILURE; From 09420b3f466e86813f8f11aeda8e3bcfe408f445 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Sat, 15 Mar 2025 13:46:07 +0800 Subject: [PATCH 05/16] system/ptpd: provide more parameter by struct ptpd_config_s when starting ptpd origin command change to new command: ptpd start interface & -> ptpd -i eth0 & ptpd stop pid -> ptpd -d pid ptpd status pid -> ptpd -t status Signed-off-by: dongjiuzhu1 --- include/netutils/ptpd.h | 16 +++++-- netutils/ptpd/ptpd.c | 8 +++- system/ptpd/ptpd_main.c | 95 ++++++++++++++++++++++++++++++++--------- 3 files changed, 94 insertions(+), 25 deletions(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 8e5a3c50850..73a615be0d3 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -27,6 +27,8 @@ * Included Files ****************************************************************************/ +#include + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -35,6 +37,14 @@ * Public Types ****************************************************************************/ +struct ptpd_config_s +{ + FAR const char *interface; + FAR const char *clock; + bool hardware_ts; + sa_family_t af; +}; + /* PTPD status information structure */ struct ptpd_status_s @@ -115,10 +125,10 @@ extern "C" * Name: ptpd_start * * Description: - * Start the PTP daemon and bind it to specified interface. + * Start the PTP daemon and bind it to specified config. * * Input Parameters: - * interface - Name of the network interface to bind to, e.g. "eth0" + * config - The configs of PTP daemon, includes interface, af and clock... * * Returned Value: * On success, the non-negative task ID of the PTP daemon is returned; @@ -126,7 +136,7 @@ extern "C" * ****************************************************************************/ -int ptpd_start(FAR const char *interface); +int ptpd_start(FAR const struct ptpd_config_s *config); /**************************************************************************** * Name: ptpd_status diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 411c9f4eb78..492f762b36c 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -1395,7 +1395,7 @@ static void ptp_process_statusreq(FAR struct ptp_state_s *state) * ****************************************************************************/ -int ptpd_start(FAR const char *interface) +int ptpd_start(FAR const struct ptpd_config_s *config) { FAR struct ptp_state_s *state; struct pollfd pollfds[2]; @@ -1407,8 +1407,12 @@ int ptpd_start(FAR const char *interface) memset(&rxiov, 0, sizeof(rxiov)); state = calloc(1, sizeof(struct ptp_state_s)); + if (state == NULL) + { + return -ENOMEM; + } - if (ptp_initialize_state(state, interface) != OK) + if (ptp_initialize_state(state, config->interface) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 36b8b67c6a2..75cca2900fe 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -28,6 +28,8 @@ #include #include +#include +#include #include "netutils/ptpd.h" @@ -35,11 +37,11 @@ * Private Functions ****************************************************************************/ -static int do_ptpd_start(FAR const char *interface) +static int do_ptpd_start(FAR const struct ptpd_config_s *config) { int ret; - ret = ptpd_start(interface); + ret = ptpd_start(config); /* Should never happen */ @@ -140,6 +142,24 @@ int do_ptpd_stop(int pid) } } +static void usage(FAR const char *progname) +{ + fprintf(stderr, "Usage: %s [options]: run this daemon in background.\n" + " Network Transport:\n" + " -2 IEEE 802.3\n" + " -4 UDP IPV4 (default)\n" + " -6 UDP IPV6\n" + " Time Stamping:\n" + " -H HARDWARE (default) depends on NET_TIMESTAMP\n" + " -S SOFTWARE\n" + " -r synchronize system (realtime) clock\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" + " -s [pid] stop ptp daemon\n", + progname); +} + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -150,24 +170,59 @@ int do_ptpd_stop(int pid) int main(int argc, FAR char *argv[]) { - if (argc == 3 && strcmp(argv[1], "start") == 0) - { - return do_ptpd_start(argv[2]); - } - else if (argc == 3 && strcmp(argv[1], "status") == 0) - { - return do_ptpd_status(atoi(argv[2])); - } - else if (argc == 3 && strcmp(argv[1], "stop") == 0) - { - return do_ptpd_stop(atoi(argv[2])); - } - else + struct ptpd_config_s config; + int option; + + /* Default config for ptp daemon */ + + config.interface = "eth0"; + config.clock = "realtime"; +#ifdef CONFIG_NET_TIMESTAMP + config.hardware_ts = true; +#else + config.hardware_ts = false; +#endif + config.af = AF_INET; + + while ((option = getopt(argc, argv, "p:i:t:s:r246HS")) != ERROR) { - fprintf(stderr, "Usage: \n" - "ptpd start &\n" - "ptpd status \n" - "ptpd stop \n"); - return EXIT_FAILURE; + switch (option) + { + case 't': + return do_ptpd_status(atoi(optarg)); + case 's': + return do_ptpd_stop(atoi(optarg)); + case '2': + config.af = AF_PACKET; + break; + case '4': + config.af = AF_INET; + break; + case '6': + config.af = AF_INET6; + break; +#ifdef CONFIG_NET_TIMESTAMP + case 'H': + config.hardware_ts = true; + break; +#endif + case 'S': + config.hardware_ts = false; + break; + case 'i': + config.interface = optarg; + break; + case 'p': + config.clock = optarg; + break; + case 'r': + config.clock = "realtime"; + break; + default: + usage(argv[0]); + return 0; + } } + + return do_ptpd_start(&config); } From 0985006e26a72dbc6faa8d5a82e28a4ef0acf667 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Mon, 17 Mar 2025 16:45:32 +0800 Subject: [PATCH 06/16] netutils/ptpd: using hardware_ts to replace CONFIG_NET_TIMESTAMP runtime to config the ways of timestamp Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 38 ++++++++++++++++++-------------------- 1 file changed, 18 insertions(+), 20 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 492f762b36c..b04d7be7214 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -159,6 +159,7 @@ struct ptp_state_s struct ptp_sync_s twostep_packet; struct timespec twostep_rxtime; + FAR const struct ptpd_config_s *config; }; #ifdef CONFIG_NETUTILS_PTPD_SERVER @@ -362,10 +363,14 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, FAR struct msghdr *rxhdr, FAR struct timespec *ts) { + FAR struct cmsghdr *cmsg; + /* Get hardware or kernel timestamp if available */ -#ifdef CONFIG_NET_TIMESTAMP - struct cmsghdr *cmsg; + if (!state->config->hardware_ts) + { + return ptp_gettime(state, ts); + } for_each_cmsghdr(cmsg, rxhdr) { @@ -385,11 +390,7 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, } ptpwarn("CONFIG_NET_TIMESTAMP enabled but did not get packet timestamp\n"); -#endif - - /* Fall back to current timestamp */ - - return ptp_gettime(state, ts); + return ERROR; } /* Initialize PTP client/server state and create sockets */ @@ -398,13 +399,10 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, FAR const char *interface) { int ret; + int arg = 1; struct ifreq req; struct sockaddr_in bind_addr; -#ifdef CONFIG_NET_TIMESTAMP - int arg; -#endif - /* Create sockets */ state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0); @@ -504,18 +502,17 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, return ERROR; } -#ifdef CONFIG_NET_TIMESTAMP - arg = 1; - ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMP, - &arg, sizeof(arg)); - - if (ret < 0) + if (state->config->hardware_ts) { - ptperr("Failed to enable SO_TIMESTAMP: %s\n", strerror(errno)); + ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMP, + &arg, sizeof(arg)); - /* PTPD can operate without, but with worse accuracy */ + if (ret < 0) + { + ptperr("Failed to enable SO_TIMESTAMP: %s\n", strerror(errno)); + return ERROR; + } } -#endif /* Bind socket for announcements */ @@ -1412,6 +1409,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) return -ENOMEM; } + state->config = config; if (ptp_initialize_state(state, config->interface) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); From d5713d249455369bfd880322223cbf4c3625ad27 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Sun, 23 Mar 2025 22:30:24 +0800 Subject: [PATCH 07/16] netutils/ptpd: get nanoseconds by SO_TIMESTAMPNS to satisfy higher precision using nanoseconds as timestamp Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index b04d7be7214..4ba8d2c863e 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -375,7 +375,7 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, for_each_cmsghdr(cmsg, rxhdr) { if (cmsg->cmsg_level == SOL_SOCKET && - cmsg->cmsg_type == SO_TIMESTAMP && + cmsg->cmsg_type == SO_TIMESTAMPNS && cmsg->cmsg_len == CMSG_LEN(sizeof(struct timeval))) { TIMEVAL_TO_TIMESPEC((FAR struct timeval *)CMSG_DATA(cmsg), ts); @@ -504,12 +504,12 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, if (state->config->hardware_ts) { - ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMP, + ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMPNS, &arg, sizeof(arg)); if (ret < 0) { - ptperr("Failed to enable SO_TIMESTAMP: %s\n", strerror(errno)); + ptperr("Failed to enable SO_TIMESTAMPNS: %s\n", strerror(errno)); return ERROR; } } From 7bc094d8a8ec59de9a45204184fceab387601927 Mon Sep 17 00:00:00 2001 From: Xiang Xiao Date: Mon, 24 Feb 2025 21:57:52 +0800 Subject: [PATCH 08/16] netutils/ptpd: Add the missing comparison in is_better_clock The original implementation only checked if each field was less than, but didn't check for greater than before proceeding to the next field. This caused incorrect clock selection behavior in the PTP Best Master Clock Algorithm (BMCA). The fix expands each comparison to explicitly check both < and > conditions, returning the appropriate result immediately. This ensures proper precedence evaluation according to IEEE 1588 specification: - gm_priority1 - gm_quality (class, accuracy, variance) - gm_priority2 - gm_identity Signed-off-by: Xiang Xiao Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 75 +++++++++++++++++++++++++++++++++++++++----- 1 file changed, 67 insertions(+), 8 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 4ba8d2c863e..2003e5a0848 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -231,20 +231,79 @@ static void ptp_format_to_timespec(FAR const uint8_t *timestamp, static bool is_better_clock(FAR const struct ptp_announce_s *a, FAR const struct ptp_announce_s *b) { - if (a->gm_priority1 < b->gm_priority1 /* Main priority field */ - || a->gm_quality[0] < b->gm_quality[0] /* Clock class */ - || a->gm_quality[1] < b->gm_quality[1] /* Clock accuracy */ - || a->gm_quality[2] < b->gm_quality[2] /* Clock variance high byte */ - || a->gm_quality[3] < b->gm_quality[3] /* Clock variance low byte */ - || a->gm_priority2 < b->gm_priority2 /* Sub priority field */ - || memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0) + /* Main priority field */ + + if (a->gm_priority1 < b->gm_priority1) + { + return true; + } + + if (a->gm_priority1 > b->gm_priority1) + { + return false; + } + + /* Clock class */ + + if (a->gm_quality[0] < b->gm_quality[0]) + { + return true; + } + + if (a->gm_quality[0] > b->gm_quality[0]) + { + return false; + } + + /* Clock accuracy */ + + if (a->gm_quality[1] < b->gm_quality[1]) + { + return true; + } + + if (a->gm_quality[1] > b->gm_quality[1]) + { + return false; + } + + /* Clock variance high byte */ + + if (a->gm_quality[2] < b->gm_quality[2]) + { + return true; + } + + if (a->gm_quality[2] > b->gm_quality[2]) + { + return false; + } + + /* Clock variance low byte */ + + if (a->gm_quality[3] < b->gm_quality[3]) + { + return true; + } + + if (a->gm_quality[3] > b->gm_quality[3]) + { + return false; + } + + /* Sub priority field */ + + if (a->gm_priority2 < b->gm_priority2) { return true; } - else + + if (a->gm_priority2 > b->gm_priority2) { return false; } + + return memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0; } static int64_t timespec_to_ms(FAR const struct timespec *ts) From 336b9066cc65079808bedbecccd0c83c7eef8b41 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Mon, 24 Feb 2025 21:57:13 +0800 Subject: [PATCH 09/16] netutils/ptp: destroy donesem at the end of ptpd_status fix minor issue Signed-off-by: Xiang Xiao Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 2003e5a0848..17f232406bf 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -56,7 +56,7 @@ #include "ptpv2.h" /**************************************************************************** - * Private Data + * Private Types ****************************************************************************/ /* Carrier structure for querying PTPD status */ @@ -179,7 +179,7 @@ struct ptp_state_s /* Convert from timespec to PTP format */ -static void timespec_to_ptp_format(FAR struct timespec *ts, +static void timespec_to_ptp_format(FAR const struct timespec *ts, FAR uint8_t *timestamp) { /* IEEE 1588 uses 48 bits for seconds and 32 bits for nanoseconds, @@ -490,7 +490,7 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, */ memset(&req, 0, sizeof(req)); - strncpy(req.ifr_name, interface, sizeof(req.ifr_name)); + strlcpy(req.ifr_name, interface, sizeof(req.ifr_name)); if (ioctl(state->event_socket, SIOCGIFADDR, (unsigned long)&req) < 0) { @@ -1293,7 +1293,7 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast); switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK) - { + { #ifdef CONFIG_NETUTILS_PTPD_CLIENT case PTP_MSGTYPE_ANNOUNCE: ptpinfo("Got announce packet, seq %ld\n", @@ -1327,7 +1327,7 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n", state->rxbuf.header.messagetype); return OK; - } + } } /* Signal handler for status / stop requests */ @@ -1610,6 +1610,7 @@ int ptpd_status(int pid, FAR struct ptpd_status_s *status) ret = -errno; } + sem_destroy(&donesem); return ret; #endif /* CONFIG_BUILD_FLAT */ From fee380577ad5913e642c963f471c4146ca1c47a0 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Thu, 27 Mar 2025 21:57:06 +0800 Subject: [PATCH 10/16] netutils/ptpd: using dynamic config to config clien-only mode and delaye2e fix compile warning when only enable PTPD_CLIENT or PTPD_SERVER ptpd.c:493:38: error: 'CONFIG_NETUTILS_PTPD_PRIORITY1' undeclared (first use in this function); did you mean 'CONFIG_NETUTILS_PTPD_CLIENT'? ptpd.c:494:39: error: 'CONFIG_NETUTILS_PTPD_CLASS' undeclared (first use in this function); did you mean 'CONFIG_NETUTILS_PTPD_CLIENT'? ptpd.c:495:39: error: 'CONFIG_NETUTILS_PTPD_ACCURACY' undeclared (first use in this function); did you mean 'CONFIG_NETUTILS_PTPD_DEBUG'? ptpd.c:498:38: error: 'CONFIG_NETUTILS_PTPD_PRIORITY2' undeclared (first use in this function); did you mean 'CONFIG_NETUTILS_PTPD_CLIENT'? ptpd.c:502:36: error: 'CONFIG_NETUTILS_PTPD_CLOCKSOURCE' undeclared (first use in this function); did you mean 'CONFIG_NETUTILS_PTPD_STACKSIZE'? Signed-off-by: dongjiuzhu1 --- include/netutils/ptpd.h | 2 ++ netutils/ptpd/Kconfig | 37 ------------------------------------- netutils/ptpd/ptpd.c | 36 ++++++++++++++---------------------- system/ptpd/ptpd_main.c | 16 +++++++++++++--- 4 files changed, 29 insertions(+), 62 deletions(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 73a615be0d3..ea4b9fb7baf 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -41,7 +41,9 @@ struct ptpd_config_s { FAR const char *interface; FAR const char *clock; + bool client_only; bool hardware_ts; + bool delay_e2e; sa_family_t af; }; diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 110223bd7c2..6c2b17bd886 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -17,24 +17,6 @@ config NETUTILS_PTPD if NETUTILS_PTPD -config NETUTILS_PTPD_CLIENT - bool "Enable client support" - default y - ---help--- - Act as a PTP client, synchronizing the NuttX clock to a remote master - clock. - -config NETUTILS_PTPD_SERVER - bool "Enable server support" - default n - ---help--- - Act as a PTP server, providing NuttX clock time to other systems. - - Both server and client can be simultaneously enabled. NuttX will then - synchronize to a higher priority master clock, or act as a master - clock itself if it has the highest priority. - Refer to Best Master Clock algorithm in IEEE-1588 for details. - config NETUTILS_PTPD_STACKSIZE int "PTP daemon stack stack size" default DEFAULT_TASK_STACKSIZE @@ -51,8 +33,6 @@ config NETUTILS_PTPD_DOMAIN Set PTP domain to participate in. Default domain is 0, other domains can be used to isolate reference clocks from each other. -if NETUTILS_PTPD_SERVER - config NETUTILS_PTPD_PRIORITY1 int "PTP server priority1" default 128 @@ -154,10 +134,6 @@ config NETUTILS_PTPD_DELAYRESP_INTERVAL Default value 4 results in 16 second interval. -endif # NETUTILS_PTPD_SERVER - -if NETUTILS_PTPD_CLIENT - config NETUTILS_PTPD_TIMEOUT_MS int "PTP client timeout for changing clock source (ms)" default 60000 @@ -191,15 +167,6 @@ 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_SEND_DELAYREQ - bool "PTP client enable delay requests" - default n - ---help--- - If enabled, sends delay request messages to measure the network delay - to server. If disabled, assumes zero delay. - -if NETUTILS_PTPD_SEND_DELAYREQ - config NETUTILS_PTPD_MAX_PATH_DELAY_NS int "PTP client maximum path delay (ns)" default 100000 @@ -214,8 +181,4 @@ config NETUTILS_PTPD_DELAYREQ_AVGCOUNT ---help--- Measured path delay is averaged over this many samples. -endif # NETUTILS_PTPD_SEND_DELAYREQ - -endif # NETUTILS_PTPD_CLIENT - endif # NETUTILS_PTPD diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 17f232406bf..9c251c334bc 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -162,17 +162,6 @@ struct ptp_state_s FAR const struct ptpd_config_s *config; }; -#ifdef CONFIG_NETUTILS_PTPD_SERVER -# define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC -#else -# define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_TIMEOUT_MS -#endif - -/* PTP debug messages are enabled by either CONFIG_DEBUG_NET_INFO - * or separately by CONFIG_NETUTILS_PTPD_DEBUG. This simplifies - * debugging without having excessive amount of logging from net. - */ - /**************************************************************************** * Private Functions ****************************************************************************/ @@ -834,12 +823,11 @@ static int ptp_send_delay_req(FAR struct ptp_state_s *state) static int ptp_periodic_send(FAR struct ptp_state_s *state) { -#ifdef CONFIG_NETUTILS_PTPD_SERVER /* If there is no better master clock on the network, * act as the reference source and send server packets. */ - if (!state->selected_source_valid) + if (!state->config->client_only && !state->selected_source_valid) { struct timespec time_now; struct timespec delta; @@ -862,10 +850,9 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) ptp_send_sync(state); } } -#endif /* CONFIG_NETUTILS_PTPD_SERVER */ -#ifdef CONFIG_NETUTILS_PTPD_SEND_DELAYREQ - if (state->selected_source_valid && state->can_send_delayreq) + if (state->config->delay_e2e && state->selected_source_valid && + state->can_send_delayreq) { struct timespec time_now; struct timespec delta; @@ -879,7 +866,6 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) ptp_send_delay_req(state); } } -#endif return OK; } @@ -1294,7 +1280,6 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK) { -#ifdef CONFIG_NETUTILS_PTPD_CLIENT case PTP_MSGTYPE_ANNOUNCE: ptpinfo("Got announce packet, seq %ld\n", (long)ptp_get_sequence(&state->rxbuf.header)); @@ -1314,14 +1299,11 @@ static int ptp_process_rx_packet(FAR struct ptp_state_s *state, ptpinfo("Got delay-resp, seq %ld\n", (long)ptp_get_sequence(&state->rxbuf.header)); return ptp_process_delay_resp(state, &state->rxbuf.delay_resp); -#endif -#ifdef CONFIG_NETUTILS_PTPD_SERVER case PTP_MSGTYPE_DELAY_REQ: ptpinfo("Got delay req, seq %ld\n", (long)ptp_get_sequence(&state->rxbuf.header)); return ptp_process_delay_req(state, &state->rxbuf.delay_req); -#endif default: ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n", @@ -1457,6 +1439,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) struct pollfd pollfds[2]; struct msghdr rxhdr; struct iovec rxiov; + int timeout; int ret; memset(&rxhdr, 0, sizeof(rxhdr)); @@ -1479,6 +1462,15 @@ int ptpd_start(FAR const struct ptpd_config_s *config) return ERROR; } + if (config->client_only) + { + timeout = CONFIG_NETUTILS_PTPD_TIMEOUT_MS; + } + else + { + timeout = CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC; + } + ptp_setup_sighandlers(state); pollfds[0].events = POLLIN; @@ -1502,7 +1494,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) pollfds[0].revents = 0; pollfds[1].revents = 0; - ret = poll(pollfds, 2, PTPD_POLL_INTERVAL); + ret = poll(pollfds, 2, timeout); if (pollfds[0].revents) { diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 75cca2900fe..e659059874d 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -145,6 +145,7 @@ int do_ptpd_stop(int pid) static void usage(FAR const char *progname) { fprintf(stderr, "Usage: %s [options]: run this daemon in background.\n" + " -s client only synchronization mode\n" " Network Transport:\n" " -2 IEEE 802.3\n" " -4 UDP IPV4 (default)\n" @@ -153,10 +154,11 @@ static void usage(FAR const char *progname) " -H HARDWARE (default) depends on NET_TIMESTAMP\n" " -S SOFTWARE\n" " -r synchronize system (realtime) clock\n" + " -E E2E, support client 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" - " -s [pid] stop ptp daemon\n", + " -d [pid] stop ptp daemon\n", progname); } @@ -177,6 +179,8 @@ int main(int argc, FAR char *argv[]) config.interface = "eth0"; config.clock = "realtime"; + config.client_only = false; + config.delay_e2e = false; #ifdef CONFIG_NET_TIMESTAMP config.hardware_ts = true; #else @@ -184,13 +188,16 @@ int main(int argc, FAR char *argv[]) #endif config.af = AF_INET; - while ((option = getopt(argc, argv, "p:i:t:s:r246HS")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:rs246EHS")) != ERROR) { switch (option) { + case 's': + config.client_only = true; + break; case 't': return do_ptpd_status(atoi(optarg)); - case 's': + case 'd': return do_ptpd_stop(atoi(optarg)); case '2': config.af = AF_PACKET; @@ -201,6 +208,9 @@ int main(int argc, FAR char *argv[]) case '6': config.af = AF_INET6; break; + case 'E': + config.delay_e2e = true; + break; #ifdef CONFIG_NET_TIMESTAMP case 'H': config.hardware_ts = true; From 413a4ccbea364eceaad332e16b14790c29c7ea07 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Mon, 17 Mar 2025 09:53:02 +0800 Subject: [PATCH 11/16] netutils/ptpd: support adjtime for ptp device support ptp clock deivce to adjust time Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 81 ++++++++++++++++++++++++++++++++++---------- 1 file changed, 63 insertions(+), 18 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 9c251c334bc..a1b0d92d171 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -50,6 +51,7 @@ #include #include #include +#include #include #include @@ -89,6 +91,10 @@ struct ptp_state_s int event_socket; int info_socket; + /* The ptp device file descriptor */ + + clockid_t clockid; + /* Our own identity as a clock source */ struct ptp_announce_s own_identity; @@ -372,16 +378,37 @@ static uint16_t ptp_get_sequence(FAR const struct ptp_header_s *hdr) return ((uint16_t)hdr->sequenceid[0] << 8) | hdr->sequenceid[1]; } -/* Get current system timestamp as a timespec - * TODO: Possibly add support for selecting different clock or using - * architecture-specific interface for clock access. - */ +static clockid_t ptp_open(FAR const char *clock) +{ + int fd; + + if (!strcmp(clock, "realtime")) + { + return CLOCK_REALTIME; + } + + fd = open(clock, O_RDWR | O_CLOEXEC); + if (fd < 0) + { + ptperr("Failed to open PTP clock device:%s, %d\n", clock, errno); + return fd; + } + + return (fd << CLOCK_SHIFT) | CLOCK_FD; +} + +static void ptp_close(clockid_t clockid) +{ + if (clockid > 0 && clockid != CLOCK_REALTIME) + { + close(clockid >> CLOCK_SHIFT); + } +} static int ptp_gettime(FAR struct ptp_state_s *state, FAR struct timespec *ts) { - UNUSED(state); - return clock_gettime(CLOCK_REALTIME, ts); + return clock_gettime(state->clockid, ts); } /* Change current system timestamp by jumping */ @@ -389,20 +416,32 @@ static int ptp_gettime(FAR struct ptp_state_s *state, static int ptp_settime(FAR struct ptp_state_s *state, FAR struct timespec *ts) { - UNUSED(state); - return clock_settime(CLOCK_REALTIME, ts); + return clock_settime(state->clockid, ts); } /* Smoothly adjust timestamp. */ static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns) { - struct timeval delta; + if (state->clockid == CLOCK_REALTIME) + { + struct timeval delta; - delta.tv_sec = delta_ns / NSEC_PER_SEC; - delta_ns -= (int64_t)delta.tv_sec * NSEC_PER_SEC; - delta.tv_usec = delta_ns / NSEC_PER_USEC; - return adjtime(&delta, NULL); + delta.tv_sec = delta_ns / NSEC_PER_SEC; + delta_ns -= (int64_t)delta.tv_sec * NSEC_PER_SEC; + delta.tv_usec = delta_ns / NSEC_PER_USEC; + return adjtime(&delta, NULL); + } + else + { + struct timex buf; + + memset(&buf, 0, sizeof(buf)); + buf.freq = (long)(-state->drift_ppb * 65536 / 1000); + buf.modes = ADJ_FREQUENCY; + + return clock_adjtime(state->clockid, &buf); + } } /* Get timestamp of latest received packet */ @@ -451,6 +490,13 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, struct ifreq req; struct sockaddr_in bind_addr; + state->clockid = ptp_open(state->config->clock); + if (state->clockid < 0) + { + ptperr("Invalid clockid %d for ptp daemon\n", state->clockid); + return ERROR; + } + /* Create sockets */ state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0); @@ -593,6 +639,8 @@ static int ptp_destroy_state(FAR struct ptp_state_s *state) { struct in_addr mcast_addr; + ptp_close(state->clockid); + mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); ipmsfilter(&state->interface_addr.sin_addr, &mcast_addr, MCAST_EXCLUDE); @@ -1455,11 +1503,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) if (ptp_initialize_state(state, config->interface) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); - - ptp_destroy_state(state); - free(state); - - return ERROR; + goto errout; } if (config->client_only) @@ -1535,6 +1579,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) ptp_process_statusreq(state); } +errout: ptp_destroy_state(state); free(state); From c9ce387c57626f846d14d200b2e7d5ab44b105b4 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Wed, 2 Apr 2025 22:58:55 +0800 Subject: [PATCH 12/16] netutils/ptpd: add NETUTILS_PTPD_ADJTIME_THRESHOLD_NS to accelerate adjustment optimize the speed of adjustment Signed-off-by: dongjiuzhu1 --- netutils/ptpd/Kconfig | 8 ++++++++ netutils/ptpd/ptpd.c | 14 +++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 6c2b17bd886..f29065f80d6 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -149,6 +149,14 @@ config NETUTILS_PTPD_SETTIME_THRESHOLD_MS time is reset with settimeofday() instead of changing the rate with adjtime(). +config NETUTILS_PTPD_ADJTIME_THRESHOLD_NS + int "PTP client threshold for using accumulated ppb to adjust system time (ns)" + default 500 + ---help--- + If difference between local and remote clock exceeds this threshold, + the PTP client can utilize current ppb instead of accumulated ppb to + accelerate system time adjustment. + config NETUTILS_PTPD_MULTICAST_TIMEOUT_MS int "PTP client timeout to rejoin multicast group (ms)" default 30000 diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index a1b0d92d171..c581e624e0f 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -421,7 +421,8 @@ static int ptp_settime(FAR struct ptp_state_s *state, /* Smoothly adjust timestamp. */ -static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns) +static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns, + int64_t ppb) { if (state->clockid == CLOCK_REALTIME) { @@ -437,7 +438,7 @@ static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns) struct timex buf; memset(&buf, 0, sizeof(buf)); - buf.freq = (long)(-state->drift_ppb * 65536 / 1000); + buf.freq = (long)(-ppb * 65536 / 1000); buf.modes = ADJ_FREQUENCY; return clock_adjtime(state->clockid, &buf); @@ -1096,7 +1097,14 @@ static int ptp_update_local_clock(FAR struct ptp_state_s *state, (long long)state->last_adjtime_ns, (long long)state->drift_ppb); - ret = ptp_adjtime(state, adjustment_ns); + if (absdelta_ns > CONFIG_NETUTILS_PTPD_ADJTIME_THRESHOLD_NS) + { + ret = ptp_adjtime(state, delta_ns, drift_ppb); + } + else + { + ret = ptp_adjtime(state, adjustment_ns, state->drift_ppb); + } if (ret != OK) { From 8aacee42d5fbbb8a17ae433d41dab77059f94741 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Thu, 21 Aug 2025 23:37:42 +0800 Subject: [PATCH 13/16] netutils/ptpd: add NET_PKT to enable PTP support ptp clock device Signed-off-by: dongjiuzhu1 --- netutils/ptpd/Kconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index f29065f80d6..63d756ad9c0 100644 --- a/netutils/ptpd/Kconfig +++ b/netutils/ptpd/Kconfig @@ -8,7 +8,7 @@ config NETUTILS_PTPD default n depends on NET_IPv4 depends on NET_IGMP - depends on NET_UDP + depends on NET_UDP || NET_PKT ---help--- Build a minimal implementation of IEEE-1588 precision time protocol. Uses system gettimeofday() and adjtime() calls to synchronize clock From 25b135214ac98ad96d11b8b434e28411c4ad7f38 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Tue, 18 Mar 2025 09:58:07 +0800 Subject: [PATCH 14/16] netutils/ptpd: support PTP ethernet transport support ptp by rawsocket Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 415 ++++++++++++++++++++++++++----------------- 1 file changed, 255 insertions(+), 160 deletions(-) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index c581e624e0f..e08b7fab2da 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -45,6 +45,8 @@ #include #include +#include +#include #include #include @@ -55,6 +57,7 @@ #include #include +#include "netutils/netlib.h" #include "ptpv2.h" /**************************************************************************** @@ -157,7 +160,7 @@ struct ptp_state_s uint8_t raw[128]; } rxbuf; - uint8_t rxcmsg[CMSG_LEN(sizeof(struct timeval))]; + uint8_t rxcmsg[CMSG_LEN(sizeof(struct timespec))]; /* Buffered sync packet for two-step clock setting where server sends * the accurate timestamp in a separate follow-up message. @@ -464,9 +467,9 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, { if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPNS && - cmsg->cmsg_len == CMSG_LEN(sizeof(struct timeval))) + cmsg->cmsg_len == CMSG_LEN(sizeof(struct timespec))) { - TIMEVAL_TO_TIMESPEC((FAR struct timeval *)CMSG_DATA(cmsg), ts); + memcpy(ts, CMSG_DATA(cmsg), sizeof(*ts)); /* Sanity-check the value */ @@ -481,15 +484,46 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, return ERROR; } +/* Unsubscribe multicast and destroy sockets */ + +static int ptp_destroy_state(FAR struct ptp_state_s *state) +{ + struct in_addr mcast_addr; + + ptp_close(state->clockid); + + mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); + ipmsfilter(&state->interface_addr.sin_addr, + &mcast_addr, MCAST_EXCLUDE); + + if (state->tx_socket > 0) + { + close(state->tx_socket); + state->tx_socket = -1; + } + + if (state->event_socket > 0) + { + close(state->event_socket); + state->event_socket = -1; + } + + if (state->info_socket > 0) + { + close(state->info_socket); + state->info_socket = -1; + } + + return OK; +} + /* Initialize PTP client/server state and create sockets */ -static int ptp_initialize_state(FAR struct ptp_state_s *state, - FAR const char *interface) +static int ptp_initialize_state(FAR struct ptp_state_s *state) { int ret; int arg = 1; struct ifreq req; - struct sockaddr_in bind_addr; state->clockid = ptp_open(state->config->clock); if (state->clockid < 0) @@ -500,25 +534,115 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, /* Create sockets */ - state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0); - if (state->tx_socket < 0) + if (state->config->af == AF_PACKET) { - ptperr("Failed to create tx socket: %d\n", errno); - return ERROR; - } + struct sockaddr_ll addr; + + state->tx_socket = socket(AF_PACKET, SOCK_RAW, 0); + if (state->tx_socket < 0) + { + ptperr("Failed to create tx socket: %d\n", errno); + goto errout; + } - state->event_socket = socket(AF_INET, SOCK_DGRAM, 0); - if (state->event_socket < 0) + 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); + ret = bind(state->tx_socket, (FAR struct sockaddr *)&addr, + sizeof(addr)); + if (ret < 0) + { + ptperr("ERROR: binding socket failed: %d\n", errno); + goto errout; + } + } + else if (state->config->af == AF_INET) { - ptperr("Failed to create event socket: %d\n", errno); - return ERROR; + struct sockaddr_in bind_addr; + + state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->tx_socket < 0) + { + ptperr("Failed to create tx socket: %d\n", errno); + goto errout; + } + + state->event_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->event_socket < 0) + { + ptperr("Failed to create event socket: %d\n", errno); + goto errout; + } + + state->info_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->info_socket < 0) + { + ptperr("Failed to create info socket: %d\n", errno); + goto errout; + } + + /* Subscribe to PTP multicast address */ + + bind_addr.sin_family = AF_INET; + bind_addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); + + ret = ipmsfilter(&state->interface_addr.sin_addr, + &bind_addr.sin_addr, MCAST_INCLUDE); + if (ret < 0) + { + ptperr("Failed to bind multicast address: %d\n", errno); + goto errout; + } + + /* Bind socket for events */ + + bind_addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); + ret = bind(state->event_socket, (FAR struct sockaddr *)&bind_addr, + sizeof(bind_addr)); + if (ret < 0) + { + ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port); + goto errout; + } + + /* Bind socket for announcements */ + + bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO); + ret = bind(state->info_socket, (FAR struct sockaddr *)&bind_addr, + sizeof(bind_addr)); + if (ret < 0) + { + ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port); + goto errout; + } + + /* Bind TX socket to interface address (local addr cannot be + * multicast) + */ + + bind_addr.sin_addr = state->interface_addr.sin_addr; + ret = bind(state->tx_socket, (FAR struct sockaddr *)&bind_addr, + sizeof(bind_addr)); + if (ret < 0) + { + ptperr("Failed to bind tx to port %d\n", bind_addr.sin_port); + goto errout; + } } - state->info_socket = socket(AF_INET, SOCK_DGRAM, 0); - if (state->info_socket < 0) + if (state->config->hardware_ts) { - ptperr("Failed to create info socket: %d\n", errno); - return ERROR; + ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMPNS, + &arg, sizeof(arg)); + + if (ret < 0) + { + ptperr("Failed to enable SO_TIMESTAMPNS: %s\n", strerror(errno)); + goto errout; + } } /* Get address information of the specified interface for binding socket @@ -526,13 +650,13 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, */ memset(&req, 0, sizeof(req)); - strlcpy(req.ifr_name, interface, sizeof(req.ifr_name)); + strlcpy(req.ifr_name, state->config->interface, sizeof(req.ifr_name)); if (ioctl(state->event_socket, SIOCGIFADDR, (unsigned long)&req) < 0) { ptperr("Failed to get IP address information for interface %s\n", - interface); - return ERROR; + state->config->interface); + goto errout; } state->interface_addr = *(FAR struct sockaddr_in *)&req.ifr_ifru.ifru_addr; @@ -544,8 +668,8 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, if (ioctl(state->event_socket, SIOCGIFHWADDR, (unsigned long)&req) < 0) { ptperr("Failed to get HW address information for interface %s\n", - interface); - return ERROR; + state->config->interface); + goto errout; } state->own_identity.header.version = 2; @@ -571,100 +695,13 @@ static int ptp_initialize_state(FAR struct ptp_state_s *state, sizeof(state->own_identity.gm_identity)); state->own_identity.timesource = CONFIG_NETUTILS_PTPD_CLOCKSOURCE; - /* Subscribe to PTP multicast address */ - - bind_addr.sin_family = AF_INET; - bind_addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); - clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast); - ret = ipmsfilter(&state->interface_addr.sin_addr, - &bind_addr.sin_addr, MCAST_INCLUDE); - if (ret < 0) - { - ptperr("Failed to bind multicast address: %d\n", errno); - return ERROR; - } - - /* Bind socket for events */ - - bind_addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); - ret = bind(state->event_socket, (FAR struct sockaddr *)&bind_addr, - sizeof(bind_addr)); - if (ret < 0) - { - ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port); - return ERROR; - } - - if (state->config->hardware_ts) - { - ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMPNS, - &arg, sizeof(arg)); - - if (ret < 0) - { - ptperr("Failed to enable SO_TIMESTAMPNS: %s\n", strerror(errno)); - return ERROR; - } - } - - /* Bind socket for announcements */ - - bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO); - ret = bind(state->info_socket, (FAR struct sockaddr *)&bind_addr, - sizeof(bind_addr)); - if (ret < 0) - { - ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port); - return ERROR; - } - - /* Bind TX socket to interface address (local addr cannot be multicast) */ - - bind_addr.sin_addr = state->interface_addr.sin_addr; - ret = bind(state->tx_socket, (FAR struct sockaddr *)&bind_addr, - sizeof(bind_addr)); - if (ret < 0) - { - ptperr("Failed to bind tx to port %d\n", bind_addr.sin_port); - return ERROR; - } - return OK; -} - -/* Unsubscribe multicast and destroy sockets */ - -static int ptp_destroy_state(FAR struct ptp_state_s *state) -{ - struct in_addr mcast_addr; - - ptp_close(state->clockid); - mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); - ipmsfilter(&state->interface_addr.sin_addr, - &mcast_addr, MCAST_EXCLUDE); - - if (state->tx_socket > 0) - { - close(state->tx_socket); - state->tx_socket = -1; - } - - if (state->event_socket > 0) - { - close(state->event_socket); - state->event_socket = -1; - } - - if (state->info_socket > 0) - { - close(state->info_socket); - state->info_socket = -1; - } - - return OK; +errout: + ptp_destroy_state(state); + return ERROR; } /* Re-subscribe multicast address. @@ -706,6 +743,76 @@ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) return OK; } +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; + + if (state->config->af == AF_PACKET) + { + /* IEE802.1AS Multicast address for gptp */ + + const uint8_t ptp_multicast_mac[ETHER_ADDR_LEN] = + { + 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e + }; + + char raw[sizeof(struct ether_header) + buflen]; + FAR struct ether_header *header; + struct msghdr msg; + struct iovec iov; + + header = (FAR struct ether_header *)&raw; + memcpy(header->ether_dhost, ptp_multicast_mac, ETHER_ADDR_LEN); + netlib_getmacaddr(state->config->interface, header->ether_shost); + header->ether_type = ETHERTYPE_PTP; + memcpy(&raw[sizeof(*header)], buf, buflen); + buflen += sizeof(*header); + + iov.iov_base = raw; + iov.iov_len = buflen; + + msg.msg_name = (FAR void *)addr; + msg.msg_namelen = addrlen; + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_flags = 0; + msg.msg_control = NULL; + msg.msg_controllen = 0; + + ret = sendmsg(state->tx_socket, &msg, 0); + if (ret < 0) + { + 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) + { + ptp_gettime(state, sendts); + } + + return ret; +} + /* Send PTP server announcement packet */ static int ptp_send_announce(FAR struct ptp_state_s *state) @@ -728,12 +835,10 @@ static int ptp_send_announce(FAR struct ptp_state_s *state) ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); - ret = sendto(state->tx_socket, &msg, sizeof(msg), 0, - (FAR struct sockaddr *)&addr, sizeof(addr)); - + ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL); if (ret < 0) { - ptperr("sendto failed: %d", errno); + ptperr("ptp sendmsg failed: %d", errno); } else { @@ -748,17 +853,11 @@ static int ptp_send_announce(FAR struct ptp_state_s *state) static int ptp_send_sync(FAR struct ptp_state_s *state) { - struct msghdr txhdr; - struct iovec txiov; struct ptp_sync_s msg; struct sockaddr_in addr; struct timespec ts; - uint8_t controlbuf[64]; int ret; - memset(&txhdr, 0, sizeof(txhdr)); - memset(&txiov, 0, sizeof(txiov)); - addr.sin_family = AF_INET; addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); @@ -772,22 +871,13 @@ static int ptp_send_sync(FAR struct ptp_state_s *state) msg.header.flags[0] = PTP_FLAGS0_TWOSTEP; #endif - txhdr.msg_name = &addr; - txhdr.msg_namelen = sizeof(addr); - txhdr.msg_iov = &txiov; - txhdr.msg_iovlen = 1; - txhdr.msg_control = controlbuf; - txhdr.msg_controllen = sizeof(controlbuf); - txiov.iov_base = &msg; - txiov.iov_len = sizeof(msg); - /* Timestamp and send the sync message */ ptp_increment_sequence(&state->sync_seq, &msg.header); ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); - ret = sendmsg(state->tx_socket, &txhdr, 0); + ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), &ts); if (ret < 0) { ptperr("sendmsg for sync message failed: %d\n", errno); @@ -795,22 +885,16 @@ static int ptp_send_sync(FAR struct ptp_state_s *state) } #ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC - /* Get timestamp after send completes and send follow-up message - * - * TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here. - */ - ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); msg.header.messagetype = PTP_MSGTYPE_FOLLOW_UP; msg.header.flags[0] = 0; addr.sin_port = HTONS(PTP_UDP_PORT_INFO); - ret = sendto(state->tx_socket, &msg, sizeof(msg), 0, - (FAR struct sockaddr *)&addr, sizeof(addr)); + ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL); if (ret < 0) { - ptperr("sendto for follow-up message failed: %d\n", errno); + ptperr("ptp sendmsg for follow-up message failed: %d\n", errno); return ret; } @@ -845,18 +929,11 @@ static int ptp_send_delay_req(FAR struct ptp_state_s *state) ptp_gettime(state, &state->delayreq_time); timespec_to_ptp_format(&state->delayreq_time, req.origintimestamp); - ret = sendto(state->tx_socket, &req, sizeof(req), 0, - (FAR struct sockaddr *)&addr, sizeof(addr)); - - /* Get timestamp after send completes. - * TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here. - */ - - ptp_gettime(state, &state->delayreq_time); - + ret = ptp_sendmsg(state, &req, sizeof(req), + &addr, sizeof(addr), &state->delayreq_time); if (ret < 0) { - ptperr("sendto failed: %d", errno); + ptperr("ptp sendmsg failed: %d", errno); } else { @@ -1219,12 +1296,10 @@ static int ptp_process_delay_req(FAR struct ptp_state_s *state, sizeof(resp.header.sequenceid)); resp.header.logmessageinterval = CONFIG_NETUTILS_PTPD_DELAYRESP_INTERVAL; - ret = sendto(state->tx_socket, &resp, sizeof(resp), 0, - (FAR struct sockaddr *)&addr, sizeof(addr)); - + ret = ptp_sendmsg(state, &resp, sizeof(resp), &addr, sizeof(addr), NULL); if (ret < 0) { - ptperr("sendto failed: %d", errno); + ptperr("ptp sendmsg failed: %d", errno); } else { @@ -1318,6 +1393,21 @@ static int ptp_process_delay_resp(FAR struct ptp_state_s *state, static int ptp_process_rx_packet(FAR struct ptp_state_s *state, ssize_t length) { + if (state->config->af == AF_PACKET) + { + /* Remove the header of ether message */ + + FAR struct ethhdr *header = (FAR struct ethhdr *)state->rxbuf.raw; + + if (htons(header->h_proto) != ETHERTYPE_PTP) + { + return -EINVAL; + } + + length -= sizeof(*header); + memmove(&state->rxbuf.raw, header + 1, length); + } + if (length < sizeof(struct ptp_header_s)) { ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n", @@ -1496,6 +1586,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) struct msghdr rxhdr; struct iovec rxiov; int timeout; + int idx = 1; int ret; memset(&rxhdr, 0, sizeof(rxhdr)); @@ -1508,7 +1599,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) } state->config = config; - if (ptp_initialize_state(state, config->interface) != OK) + if (ptp_initialize_state(state) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); goto errout; @@ -1527,8 +1618,12 @@ int ptpd_start(FAR const struct ptpd_config_s *config) pollfds[0].events = POLLIN; pollfds[0].fd = state->event_socket; - pollfds[1].events = POLLIN; - pollfds[1].fd = state->info_socket; + if (state->info_socket > 0) + { + pollfds[1].events = POLLIN; + pollfds[1].fd = state->info_socket; + idx++; + } while (!state->stop) { @@ -1546,7 +1641,7 @@ int ptpd_start(FAR const struct ptpd_config_s *config) pollfds[0].revents = 0; pollfds[1].revents = 0; - ret = poll(pollfds, 2, timeout); + ret = poll(pollfds, idx, timeout); if (pollfds[0].revents) { From f2c94dee98aceef353133013265ab2c47a2def69 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Wed, 9 Apr 2025 17:39:32 +0800 Subject: [PATCH 15/16] netutils/ptpd: using -B to control BMCA message new option -B to BMCA message Signed-off-by: dongjiuzhu1 --- include/netutils/ptpd.h | 1 + netutils/ptpd/ptpd.c | 10 ++++++---- system/ptpd/ptpd_main.c | 7 ++++++- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index ea4b9fb7baf..1d455c6be70 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -44,6 +44,7 @@ struct ptpd_config_s bool client_only; bool hardware_ts; bool delay_e2e; + bool bmca; sa_family_t af; }; diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index e08b7fab2da..010b9a2f361 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -961,7 +961,7 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_transmitted_announce, &delta); - if (timespec_to_ms(&delta) + if (state->config->bmca && timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC) { state->last_transmitted_announce = time_now; @@ -1003,7 +1003,7 @@ static int ptp_process_announce(FAR struct ptp_state_s *state, { clock_gettime(CLOCK_MONOTONIC, &state->last_received_announce); - if (is_better_clock(msg, &state->own_identity)) + if (state->conifg->bmca && is_better_clock(msg, &state->n_identity)) { if (!state->selected_source_valid || is_better_clock(msg, &state->selected_source)) @@ -1206,7 +1206,8 @@ static int ptp_process_sync(FAR struct ptp_state_s *state, { struct timespec remote_time; - if (memcmp(msg->header.sourceidentity, + if (state->config->bmca && + memcmp(msg->header.sourceidentity, state->selected_source.header.sourceidentity, sizeof(msg->header.sourceidentity)) != 0) { @@ -1240,7 +1241,8 @@ static int ptp_process_followup(FAR struct ptp_state_s *state, { struct timespec remote_time; - if (memcmp(msg->header.sourceidentity, + if (state->config->bmca && + memcmp(msg->header.sourceidentity, state->twostep_packet.header.sourceidentity, sizeof(msg->header.sourceidentity)) != 0) { diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index e659059874d..4a87ce47206 100644 --- a/system/ptpd/ptpd_main.c +++ b/system/ptpd/ptpd_main.c @@ -153,6 +153,7 @@ static void usage(FAR const char *progname) " Time Stamping:\n" " -H HARDWARE (default) depends on NET_TIMESTAMP\n" " -S SOFTWARE\n" + " -B The best master clock algorithm is used\n" " -r synchronize system (realtime) clock\n" " -E E2E, support client delay request-response\n" " -i [dev] interface device to use, for example 'eth0'\n" @@ -186,9 +187,10 @@ int main(int argc, FAR char *argv[]) #else config.hardware_ts = false; #endif + config.bmca = false; config.af = AF_INET; - while ((option = getopt(argc, argv, "p:i:t:d:rs246EHS")) != ERROR) + while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHS")) != ERROR) { switch (option) { @@ -208,6 +210,9 @@ int main(int argc, FAR char *argv[]) case '6': config.af = AF_INET6; break; + case 'B': + config.bmca = true; + break; case 'E': config.delay_e2e = true; break; From 53f8f23d33affb94b96bc6d345424fcdf84012b8 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Wed, 9 Apr 2025 09:25:38 +0800 Subject: [PATCH 16/16] netutils/ptpd: Add switch correction time change The current gPTP stack does not support path delay correction of the Switch. This patch adds the path delay correction field in the Header. Signed-off-by: dongjiuzhu1 --- netutils/ptpd/ptpd.c | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c index 010b9a2f361..3193ee33ea4 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -1236,6 +1236,31 @@ static int ptp_process_sync(FAR struct ptp_state_s *state, 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: %lld.%09ld\n", (long long)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: %lld.%09ld\n", (long long)ts->tv_sec, + ts->tv_nsec); +} + static int ptp_process_followup(FAR struct ptp_state_s *state, FAR struct ptp_follow_up_s *msg) { @@ -1264,6 +1289,13 @@ static int ptp_process_followup(FAR struct ptp_state_s *state, */ ptp_format_to_timespec(msg->origintimestamp, &remote_time); + + /* add correction time */ + + ptp_add_correction_time(msg->header.correction, &remote_time); + + /* done */ + return ptp_update_local_clock(state, &remote_time, &state->twostep_rxtime); }