diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h index 8e5a3c50850..1d455c6be70 100644 --- a/include/netutils/ptpd.h +++ b/include/netutils/ptpd.h @@ -27,6 +27,8 @@ * Included Files ****************************************************************************/ +#include + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -35,6 +37,17 @@ * Public Types ****************************************************************************/ +struct ptpd_config_s +{ + FAR const char *interface; + FAR const char *clock; + bool client_only; + bool hardware_ts; + bool delay_e2e; + bool bmca; + sa_family_t af; +}; + /* PTPD status information structure */ struct ptpd_status_s @@ -115,10 +128,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 +139,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/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() diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig index 93cd288ca56..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 @@ -17,31 +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 - ---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 @@ -58,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 @@ -161,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 @@ -180,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 @@ -198,15 +175,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 @@ -221,8 +189,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 309f78a07f1..3193ee33ea4 100644 --- a/netutils/ptpd/ptpd.c +++ b/netutils/ptpd/ptpd.c @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -44,19 +45,23 @@ #include #include +#include +#include #include #include #include #include #include +#include #include #include +#include "netutils/netlib.h" #include "ptpv2.h" /**************************************************************************** - * Private Data + * Private Types ****************************************************************************/ /* Carrier structure for querying PTPD status */ @@ -89,6 +94,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; @@ -151,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. @@ -159,36 +168,16 @@ 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 -# 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. - */ - -#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 ****************************************************************************/ /* 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, @@ -240,20 +229,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; } - else + + 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; + } + + 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) @@ -333,16 +381,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 */ @@ -350,20 +419,33 @@ 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) +static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns, + int64_t ppb) { - 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); + } + else + { + struct timex buf; + + memset(&buf, 0, sizeof(buf)); + buf.freq = (long)(-ppb * 65536 / 1000); + buf.modes = ADJ_FREQUENCY; - 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); + return clock_adjtime(state->clockid, &buf); + } } /* Get timestamp of latest received packet */ @@ -372,18 +454,22 @@ 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) { if (cmsg->cmsg_level == SOL_SOCKET && - cmsg->cmsg_type == SO_TIMESTAMP && - cmsg->cmsg_len == CMSG_LEN(sizeof(struct timeval))) + cmsg->cmsg_type == SO_TIMESTAMPNS && + 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 */ @@ -395,47 +481,168 @@ static int ptp_getrxtime(FAR struct ptp_state_s *state, } ptpwarn("CONFIG_NET_TIMESTAMP enabled but did not get packet timestamp\n"); -#endif + return ERROR; +} + +/* Unsubscribe multicast and destroy sockets */ - /* Fall back to current timestamp */ +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); - return ptp_gettime(state, ts); + 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; -#ifdef CONFIG_NET_TIMESTAMP - int arg; -#endif + 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); - 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 = dup(state->tx_socket); + state->info_socket = -1; - state->event_socket = socket(AF_INET, SOCK_DGRAM, 0); - if (state->event_socket < 0) + 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 @@ -443,13 +650,13 @@ 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, 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; @@ -461,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; @@ -488,99 +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; - } - -#ifdef CONFIG_NET_TIMESTAMP - arg = 1; - ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMP, - &arg, sizeof(arg)); - - if (ret < 0) - { - ptperr("Failed to enable SO_TIMESTAMP: %s\n", strerror(errno)); - - /* PTPD can operate without, but with worse accuracy */ - } -#endif - - /* 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; - - 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. @@ -622,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) @@ -644,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 { @@ -664,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); @@ -688,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); @@ -711,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; } @@ -761,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 { @@ -788,12 +949,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; @@ -801,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; @@ -816,10 +976,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; @@ -833,7 +992,6 @@ static int ptp_periodic_send(FAR struct ptp_state_s *state) ptp_send_delay_req(state); } } -#endif return OK; } @@ -845,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)) @@ -1016,7 +1174,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) { @@ -1041,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) { @@ -1070,12 +1236,38 @@ 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) { 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) { @@ -1097,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); } @@ -1131,12 +1330,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 { @@ -1230,6 +1427,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", @@ -1247,8 +1459,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", (long)ptp_get_sequence(&state->rxbuf.header)); @@ -1268,20 +1479,17 @@ 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", state->rxbuf.header.messagetype); return OK; - } + } } /* Signal handler for status / stop requests */ @@ -1386,43 +1594,70 @@ 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 struct ptpd_config_s *config) { - FAR const char *interface = "eth0"; FAR struct ptp_state_s *state; struct pollfd pollfds[2]; struct msghdr rxhdr; struct iovec rxiov; + int timeout; + int idx = 1; int ret; memset(&rxhdr, 0, sizeof(rxhdr)); memset(&rxiov, 0, sizeof(rxiov)); state = calloc(1, sizeof(struct ptp_state_s)); - - if (argc > 1) + if (state == NULL) { - interface = argv[1]; + return -ENOMEM; } - if (ptp_initialize_state(state, interface) != OK) + state->config = config; + if (ptp_initialize_state(state) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); + goto errout; + } - ptp_destroy_state(state); - free(state); - - 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; 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) { @@ -1440,7 +1675,7 @@ static int ptp_daemon(int argc, FAR char** argv) pollfds[0].revents = 0; pollfds[1].revents = 0; - ret = poll(pollfds, 2, PTPD_POLL_INTERVAL); + ret = poll(pollfds, idx, timeout); if (pollfds[0].revents) { @@ -1481,58 +1716,13 @@ static int ptp_daemon(int argc, FAR char** argv) ptp_process_statusreq(state); } +errout: ptp_destroy_state(state); free(state); 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 * @@ -1594,6 +1784,7 @@ int ptpd_status(int pid, FAR struct ptpd_status_s *status) ret = -errno; } + sem_destroy(&donesem); return ret; #endif /* CONFIG_BUILD_FLAT */ 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 */ diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c index 503300bcef5..4a87ce47206 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,19 +37,16 @@ * Private Functions ****************************************************************************/ -static int do_ptpd_start(FAR const char *interface) +static int do_ptpd_start(FAR const struct ptpd_config_s *config) { - int pid; + int ret; - pid = ptpd_start(interface); - if (pid < 0) - { - fprintf(stderr, "ERROR: ptpd_start() failed\n"); - return EXIT_FAILURE; - } + ret = ptpd_start(config); - 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) @@ -143,6 +142,27 @@ 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" + " -6 UDP IPV6\n" + " 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" + " -p [dev] clock device to use\n" + " -t [pid] look the status of ptp daemon\n" + " -d [pid] stop ptp daemon\n", + progname); +} + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -153,24 +173,71 @@ int do_ptpd_stop(int pid) int main(int argc, FAR char *argv[]) { - if (argc == 3 && strcmp(argv[1], "start") == 0) + struct ptpd_config_s config; + int option; + + /* Default config for ptp daemon */ + + config.interface = "eth0"; + config.clock = "realtime"; + config.client_only = false; + config.delay_e2e = false; +#ifdef CONFIG_NET_TIMESTAMP + config.hardware_ts = true; +#else + config.hardware_ts = false; +#endif + config.bmca = false; + config.af = AF_INET; + + while ((option = getopt(argc, argv, "p:i:t:d:rs246BEHS")) != ERROR) { - 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 - { - fprintf(stderr, "Usage: \n" - "ptpd start \n" - "ptpd status \n" - "ptpd stop \n"); - return EXIT_FAILURE; + switch (option) + { + case 's': + config.client_only = true; + break; + case 't': + return do_ptpd_status(atoi(optarg)); + case 'd': + 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; + case 'B': + config.bmca = true; + break; + case 'E': + config.delay_e2e = true; + 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); }