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 3b20797945a94daf196fcc8e108864f91e70769c 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 miss comparision in is_better_clock N/A Signed-off-by: Xiang Xiao --- 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 3cab597f01365d8f609c016cae145dcf81a148d3 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 32b4ef4dc5723e3d4df5b9e1bc645f5125ceaee1 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 10b7258a1440fbe82864dfc543db5f60186c6de3 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 757744add4a600f70fbd53347bd344d64740d9b0 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 71598b6f5dd96c2615e25aeed70bdebc3fa0d0e7 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 ebd885e1dd1f2661c07ce2489492ba8bb9a7ee58 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 8f8bc6376979bdb12714593da6ccc6159b22b073 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 f61389ed64b214c8583c85722b9fcce5139aed91 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Wed, 9 Apr 2025 09:25:38 +0800 Subject: [PATCH 16/16] add switch correction time change The current gPTP stack does not support path delay correction of the Switch. The path delay correction field in the Head is added. 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); }