From 0ba727d111c915c28a7d609312d8c1a83c7cb35b Mon Sep 17 00:00:00 2001 From: Petteri Aimonen Date: Thu, 28 Sep 2023 13:46:47 +0300 Subject: [PATCH] Add basic IEEE-1588 PTP server/client What works: - Basic server & client operation - Transmission and reception of announce, sync and follow-up Still missing: - SO_TIMINGS for getting more precise packet timestamps - Implementation of delay_req and delay_resp packets - Status and stop interfaces for the daemon --- include/netutils/ptpd.h | 71 +++ netutils/ptpd/Kconfig | 167 ++++++++ netutils/ptpd/Make.defs | 23 + netutils/ptpd/Makefile | 29 ++ netutils/ptpd/ptpd.c | 930 ++++++++++++++++++++++++++++++++++++++++ netutils/ptpd/ptpv2.h | 117 +++++ system/ptpd/Kconfig | 24 ++ system/ptpd/Make.defs | 23 + system/ptpd/Makefile | 30 ++ system/ptpd/ptpd_main.c | 59 +++ 10 files changed, 1473 insertions(+) create mode 100644 include/netutils/ptpd.h create mode 100644 netutils/ptpd/Kconfig create mode 100644 netutils/ptpd/Make.defs create mode 100644 netutils/ptpd/Makefile create mode 100644 netutils/ptpd/ptpd.c create mode 100644 netutils/ptpd/ptpv2.h create mode 100644 system/ptpd/Kconfig create mode 100644 system/ptpd/Make.defs create mode 100644 system/ptpd/Makefile create mode 100644 system/ptpd/ptpd_main.c diff --git a/include/netutils/ptpd.h b/include/netutils/ptpd.h new file mode 100644 index 00000000000..22d36973b1e --- /dev/null +++ b/include/netutils/ptpd.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * apps/include/netutils/ptpd.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_INCLUDE_NETUTILS_PTPD_H +#define __APPS_INCLUDE_NETUTILS_PTPD_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: ptpd_start + * + * Description: + * Start the PTP daemon and bind it to specified interface. + * + * 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(const char *interface); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __APPS_INCLUDE_NETUTILS_PTPD_H */ diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig new file mode 100644 index 00000000000..bd2f5c64bb9 --- /dev/null +++ b/netutils/ptpd/Kconfig @@ -0,0 +1,167 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config NETUTILS_PTPD + bool "PTPD client/server" + default n + select NET_IPv4 + select NET_IGMP + select NET_UDP + ---help--- + Build a minimal implementation of IEEE-1588 precision time protocol. + Uses system gettimeofday() and adjtime() calls to synchronize clock + with a master clock through network, or to provide a master clock to + other systems. + +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 + +config NETUTILS_PTPD_SERVERPRIO + int "PTP daemon priority" + default 100 + +config NETUTILS_PTPD_DOMAIN + int "PTP domain selection" + default 0 + range 0 127 + ---help--- + 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 + range 0 255 + ---help--- + Set clock priority to announce when acting as a PTP server. + Lower value is higher priority. + + A higher priority1 clock will be selected without regard to announced + clock quality fields. + Refer to Best Master Clock algorithm in IEEE-1588 for details. + +config NETUTILS_PTPD_PRIORITY2 + int "PTP server priority2" + default 128 + range 0 255 + ---help--- + Set clock subpriority to announce when acting as a PTP server. + This will distinguish between two clocks that are equivalent in + priority1, class and accuracy values. + Lower value is higher priority. + +config NETUTILS_PTPD_CLASS + int "PTP server class" + default 248 + range 0 255 + ---help--- + Set master clock class to announce when acting as a PTP server. + Lower value means higher quality clock source. + 248 is the default for unknown class. + +config NETUTILS_PTPD_ACCURACY + int "PTP server accuracy" + default 254 + range 0 255 + ---help--- + Set master clock accuracy to announce when acting as a PTP server. + Logarithmic scale is defined in IEEE-1588: + 32: +- 25 ns + 33: +- 100 ns + 34: +- 250 ns + 35: +- 1 us + 36: +- 2.5 us + 37: +- 10 us + 38: +- 25 us + 39: +- 100 us + 40: +- 250 us + 41: +- 1 ms + 42: +- 2.5 ms + 43: +- 10 ms + 44: +- 25 ms + 45: +- 100 ms + 46: +- 250 ms + 47: +- 1 s + 48: +- 10 s + 49: +- more than 10 s + 254: Unknown + +config NETUTILS_PTPD_CLOCKSOURCE + int "PTP server clock source type" + default 160 + range 0 255 + ---help--- + Set clock source type to announce when acting as a PTP server. + Common values: + 32: GPS + 64: PTP + 80: NTP + 144: Other + 160: Internal oscillator + +config NETUTILS_PTPD_SYNC_INTERVAL_MSEC + int "PTP server sync transmit interval (ms)" + default 1000 + ---help--- + How often to transmit sync packets in server mode. + +config NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC + int "PTP server announce transmit interval (ms)" + default 10000 + ---help--- + How often to transmit announce packets in server mode. + +endif # NETUTILS_PTPD_SERVER + +if NETUTILS_PTPD_CLIENT + +config NETUTILS_PTPD_TIMEOUT_MS + int "PTP client timeout for changing clock source (ms)" + default 60000 + ---help--- + If no packets are being received from currently chosen clock source, + fall back to next best clock source after this many seconds. + +config NETUTILS_PTPD_SETTIME_THRESHOLD_MS + int "PTP client threshold for changing system time (ms)" + default 1000 + ---help--- + If difference between local and remote clock exceeds this threshold, + time is reset with settimeofday() instead of changing the rate with + adjtime(). + +endif # NETUTILS_PTPD_CLIENT + +endif # NETUTILS_PTPD diff --git a/netutils/ptpd/Make.defs b/netutils/ptpd/Make.defs new file mode 100644 index 00000000000..c3f5f60ffb0 --- /dev/null +++ b/netutils/ptpd/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/netutils/ptpd/Make.defs +# +# 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. +# +############################################################################ + +ifneq ($(CONFIG_NETUTILS_PTPD),) +CONFIGURED_APPS += $(APPDIR)/netutils/ptpd +endif diff --git a/netutils/ptpd/Makefile b/netutils/ptpd/Makefile new file mode 100644 index 00000000000..760b47297c9 --- /dev/null +++ b/netutils/ptpd/Makefile @@ -0,0 +1,29 @@ +############################################################################ +# apps/netutils/ptpd/Makefile +# +# 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. +# +############################################################################ + +include $(APPDIR)/Make.defs + +# PTP server/client implementation + +ifeq ($(CONFIG_NET_UDP),y) +CSRCS = ptpd.c +endif + +include $(APPDIR)/Application.mk diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c new file mode 100644 index 00000000000..85384467fad --- /dev/null +++ b/netutils/ptpd/ptpd.c @@ -0,0 +1,930 @@ +/**************************************************************************** + * apps/netutils/ptpd/ptpd.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include "ptpv2.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct ptp_state_s +{ + bool stop; + + /* Address of network interface we are operating on */ + + struct sockaddr_in interface_addr; + + /* Socket bound to interface for transmission */ + + int tx_socket; + + /* Sockets for PTP event and information ports */ + + int event_socket; + int info_socket; + + /* Our own identity as a clock source */ + + struct ptp_announce_s own_identity; + + /* Sequence number counters per message type */ + + uint16_t announce_seq; + uint16_t sync_seq; + uint16_t delay_req_seq; + + /* Identity of currently selected clock source, + * from the latest announcement message. + * + * The timestamp is used for timeout when a source + * disappears, it is from the local monotonic clock. + */ + + struct ptp_announce_s selected_source; + struct timespec last_received_sync; + + /* Last transmitted sync & announcement packets */ + + struct timespec last_transmitted_sync; + struct timespec last_transmitted_announce; + + /* Latest received packet and its timestamp */ + + struct timespec rxtime; + union + { + struct ptp_header_s header; + struct ptp_announce_s announce; + struct ptp_sync_s sync; + struct ptp_follow_up_s follow_up; + uint8_t raw[128]; + } rxbuf; + + union + { + uint8_t raw[64]; + } rxcmsg; + + /* Buffered sync packet for two-step clock setting */ + + struct ptp_sync_s twostep_packet; + struct timespec twostep_rxtime; +}; + +#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 +#ifndef CONFIG_DEBUG_NET_INFO +#define ptpinfo _info +#define ptpwarn _warn +#define ptperr _err +#else +#define ptpinfo ninfo +#define ptpwarn nwarn +#define ptperr nerr +#endif +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/* Convert from timespec to PTP format */ + +static void timespec_to_ptp_format(struct timespec *ts, uint8_t *timestamp) +{ + /* IEEE 1588 uses 48 bits for seconds and 32 bits for nanoseconds, + * both fields big-endian. + */ + +#ifdef CONFIG_SYSTEM_TIME64 + timestamp[0] = (uint8_t)(ts->tv_sec >> 40); + timestamp[1] = (uint8_t)(ts->tv_sec >> 32); +#else + timestamp[0] = 0; + timestamp[1] = 0; +#endif + timestamp[2] = (uint8_t)(ts->tv_sec >> 24); + timestamp[3] = (uint8_t)(ts->tv_sec >> 16); + timestamp[4] = (uint8_t)(ts->tv_sec >> 8); + timestamp[5] = (uint8_t)(ts->tv_sec >> 0); + + timestamp[6] = (uint8_t)(ts->tv_nsec >> 24); + timestamp[7] = (uint8_t)(ts->tv_nsec >> 16); + timestamp[8] = (uint8_t)(ts->tv_nsec >> 8); + timestamp[9] = (uint8_t)(ts->tv_nsec >> 0); +} + +/* Convert from PTP format to timespec */ + +static void ptp_format_to_timespec(uint8_t *timestamp, struct timespec *ts) +{ + ts->tv_sec = + (((int64_t)timestamp[0]) << 40) + | (((int64_t)timestamp[1]) << 32) + | (((int64_t)timestamp[2]) << 24) + | (((int64_t)timestamp[3]) << 16) + | (((int64_t)timestamp[4]) << 8) + | (((int64_t)timestamp[5]) << 0); + + ts->tv_nsec = + (((long)timestamp[6]) << 24) + | (((long)timestamp[7]) << 16) + | (((long)timestamp[8]) << 8) + | (((long)timestamp[9]) << 0); +} + +/* Returns true if A is a better clock source than B. + * Implements Best Master Clock algorithm from IEEE-1588. + */ + +static bool is_better_clock(struct ptp_announce_s *a, + 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) + { + return true; + } + else + { + return false; + } +} + +int64_t timespec_to_ms(struct timespec *ts) +{ + return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC); +} + +/* Check if the currently selected source is still valid */ + +static bool is_selected_source_valid(struct ptp_state_s *state) +{ + struct timespec time_now; + struct timespec delta; + + if ((state->selected_source.header.messagetype & PTP_MSGTYPE_MASK) + != PTP_MSGTYPE_ANNOUNCE) + { + return false; /* Uninitialized value */ + } + + /* Note: this uses monotonic clock to track the timeout even when + * system clock is adjusted. + */ + + clock_gettime(CLOCK_MONOTONIC, &time_now); + clock_timespec_subtract(&time_now, &state->last_received_sync, &delta); + + if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_TIMEOUT_MS) + { + return false; /* Too long time since received packet */ + } + + return true; +} + +/* Increment sequence number for packet type, and copy to header */ + +static void ptp_increment_sequence(uint16_t *sequence_num, + struct ptp_header_s *hdr) +{ + *sequence_num += 1; + hdr->sequenceid[0] = (uint8_t)(*sequence_num >> 8); + hdr->sequenceid[1] = (uint8_t)(*sequence_num); +} + +/* Get sequence number from received packet */ + +static uint16_t ptp_get_sequence(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 int ptp_gettime(struct ptp_state_s *state, struct timespec *ts) +{ + UNUSED(state); + return clock_gettime(CLOCK_REALTIME, ts); +} + +/* Change current system timestamp by jumping */ + +static int ptp_settime(struct ptp_state_s *state, struct timespec *ts) +{ + UNUSED(state); + return clock_settime(CLOCK_REALTIME, ts); +} + +/* Smoothly adjust timestamp. + * TODO: adjtime() limits to microsecond resolution. + */ + +static int ptp_adjtime(struct ptp_state_s *state, struct timespec *ts) +{ + struct timeval delta; + UNUSED(state); + TIMESPEC_TO_TIMEVAL(&delta, ts); + return adjtime(&delta, NULL); +} + +/* Get timestamp of latest received packet */ + +static int ptp_getrxtime(struct ptp_state_s *state, struct timespec *ts) +{ + UNUSED(state); + *ts = state->rxtime; + + /* TODO: Implement SO_TIMINGS in NuttX core, and then fetch the + * timestamp from state->cmsg. + */ + + return OK; +} + +/* Initialize PTP client/server state and create sockets */ + +static int ptp_initialize_state(struct ptp_state_s *state, + const char *interface) +{ + int ret; + struct ifreq req; + struct sockaddr_in bind_addr; + + /* Create sockets */ + + state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->tx_socket < 0) + { + ptperr("Failed to create tx socket: %d\n", errno); + return ERROR; + } + + state->event_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->event_socket < 0) + { + ptperr("Failed to create event socket: %d\n", errno); + return ERROR; + } + + state->info_socket = socket(AF_INET, SOCK_DGRAM, 0); + if (state->info_socket < 0) + { + ptperr("Failed to create info socket: %d\n", errno); + return ERROR; + } + + /* Get address information of the specified interface for binding socket + * Only supports IPv4 currently. + */ + + memset(&req, 0, sizeof(req)); + strncpy(req.ifr_name, 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->interface_addr = *(struct sockaddr_in *)&req.ifr_ifru.ifru_addr; + + /* Get hardware address to initialize the identity field in header. + * Clock identity is EUI-64, which we make from EUI-48. + */ + + 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->own_identity.header.version = 2; + state->own_identity.header.domain = CONFIG_NETUTILS_PTPD_DOMAIN; + state->own_identity.header.sourceidentity[0] = req.ifr_hwaddr.sa_data[0]; + state->own_identity.header.sourceidentity[1] = req.ifr_hwaddr.sa_data[1]; + state->own_identity.header.sourceidentity[2] = req.ifr_hwaddr.sa_data[2]; + state->own_identity.header.sourceidentity[3] = 0xff; + state->own_identity.header.sourceidentity[4] = 0xfe; + state->own_identity.header.sourceidentity[5] = req.ifr_hwaddr.sa_data[3]; + state->own_identity.header.sourceidentity[6] = req.ifr_hwaddr.sa_data[4]; + state->own_identity.header.sourceidentity[7] = req.ifr_hwaddr.sa_data[5]; + state->own_identity.header.sourceportindex[0] = 0; + state->own_identity.header.sourceportindex[1] = 1; + state->own_identity.gm_priority1 = CONFIG_NETUTILS_PTPD_PRIORITY1; + state->own_identity.gm_quality[0] = CONFIG_NETUTILS_PTPD_CLASS; + state->own_identity.gm_quality[1] = CONFIG_NETUTILS_PTPD_ACCURACY; + state->own_identity.gm_quality[2] = 0xff; /* No variance estimate */ + state->own_identity.gm_quality[3] = 0xff; + state->own_identity.gm_priority2 = CONFIG_NETUTILS_PTPD_PRIORITY2; + memcpy(state->own_identity.gm_identity, + state->own_identity.header.sourceidentity, + 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); + + 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, (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 socket for announcements */ + + bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO); + ret = bind(state->info_socket, (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, (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(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; +} + +/* Send PTP server announcement packet */ + +static int ptp_send_announce(struct ptp_state_s *state) +{ + struct ptp_announce_s msg; + struct sockaddr_in addr; + struct timespec ts; + int ret; + + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); + addr.sin_port = HTONS(PTP_UDP_PORT_INFO); + + memset(&msg, 0, sizeof(msg)); + msg = state->own_identity; + msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE; + msg.header.messagelength[1] = sizeof(msg); + + ptp_increment_sequence(&state->sync_seq, &msg.header); + ptp_gettime(state, &ts); + timespec_to_ptp_format(&ts, msg.origintimestamp); + + ret = sendto(state->tx_socket, &msg, sizeof(msg), 0, + (struct sockaddr *)&addr, sizeof(addr)); + + if (ret < 0) + { + ptperr("sendto failed: %d", errno); + } + else + { + ptpinfo("Sent announce, seq %ld\n", + (long)ptp_get_sequence(&msg.header)); + } + + return ret; +} + +/* Send PTP server synchronization packet */ + +static int ptp_send_sync(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); + + memset(&msg, 0, sizeof(msg)); + msg.header = state->own_identity.header; + msg.header.messagetype = PTP_MSGTYPE_SYNC; + msg.header.messagelength[1] = sizeof(msg); + msg.header.flags[0] = PTP_FLAGS0_TWOSTEP; + + 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); + if (ret < 0) + { + ptperr("sendmsg for sync message failed: %d\n", errno); + return ret; + } + + /* Get timestamp after send completes and send follow-up message + * + * TODO: Implement SO_TIMINGS 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, + (struct sockaddr *)&addr, sizeof(addr)); + if (ret < 0) + { + ptperr("sendto for follow-up message failed: %d\n", errno); + return ret; + } + + ptpinfo("Sent sync + follow-up, seq %ld\n", + (long)ptp_get_sequence(&msg.header)); + + return OK; +} + +/* Check if we need to send packets */ + +static int ptp_periodic_send(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 (!is_selected_source_valid(state)) + { + struct timespec time_now; + struct timespec delta; + + clock_gettime(CLOCK_MONOTONIC, &time_now); + clock_timespec_subtract(&time_now, + &state->last_transmitted_announce, &delta); + if (timespec_to_ms(&delta) + > CONFIG_NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC) + { + state->last_transmitted_announce = time_now; + ptp_send_announce(state); + } + + clock_timespec_subtract(&time_now, + &state->last_transmitted_sync, &delta); + if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC) + { + state->last_transmitted_sync = time_now; + ptp_send_sync(state); + } + } +#endif + + return OK; +} + +/* Process received PTP announcement */ + +static int ptp_process_announce(struct ptp_state_s *state, + struct ptp_announce_s *msg) +{ + if (is_better_clock(msg, &state->own_identity)) + { + if (!is_selected_source_valid(state) || + is_better_clock(msg, &state->selected_source)) + { + ptpinfo("Switching to better PTP time source\n"); + + state->selected_source = *msg; + clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync); + } + } + + return OK; +} + +/* Update local clock by delta, either by smooth adjustment or by jumping. */ + +static int ptp_update_local_clock(struct ptp_state_s *state, + struct timespec *delta) +{ + int ret; + struct timespec local_time; + + if (timespec_to_ms(delta) > CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS) + { + /* Add delta to current local time in order to account for any latency + * between packet reception and clock setting. + */ + + ptp_gettime(state, &local_time); + clock_timespec_add(&local_time, delta, &local_time); + ret = ptp_settime(state, &local_time); + + if (ret == OK) + { + ptpinfo("Jumped to timestamp %ld.%09ld s\n", + (long)local_time.tv_sec, (long)local_time.tv_nsec); + } + else + { + ptperr("ptp_settime() failed: %d\n", errno); + } + } + else + { + ret = ptp_adjtime(state, delta); + + if (ret == OK) + { + ptpinfo("Adjusting clock by %ld.%09ld s\n", (long)delta->tv_sec, + (long)delta->tv_nsec); + } + else + { + ptperr("ptp_adjtime() failed: %d\n", errno); + } + } + + return ret; +} + +/* Process received PTP sync packet */ + +static int ptp_process_sync(struct ptp_state_s *state, + struct ptp_sync_s *msg) +{ + struct timespec remote_time; + struct timespec local_time; + struct timespec delta; + + if (memcmp(msg->header.sourceidentity, + state->selected_source.header.sourceidentity, + sizeof(msg->header.sourceidentity)) != 0) + { + /* This packet wasn't from the currently selected source */ + + return OK; + } + + /* Update timeout tracking */ + + clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync); + + if (msg->header.flags[0] & PTP_FLAGS0_TWOSTEP) + { + /* We need to wait for a follow-up packet before setting the clock. */ + + ptp_getrxtime(state, &state->twostep_rxtime); + state->twostep_packet = *msg; + ptpinfo("Waiting for follow-up\n"); + return OK; + } + + /* Calculate delta between local and remote time */ + + ptp_format_to_timespec(msg->origintimestamp, &remote_time); + ptp_getrxtime(state, &local_time); + clock_timespec_subtract(&remote_time, &local_time, &delta); + + return ptp_update_local_clock(state, &delta); +} + +static int ptp_process_followup(struct ptp_state_s *state, + struct ptp_follow_up_s *msg) +{ + struct timespec remote_time; + struct timespec delta; + + if (memcmp(msg->header.sourceidentity, + state->twostep_packet.header.sourceidentity, + sizeof(msg->header.sourceidentity)) != 0) + { + return OK; /* This packet wasn't from the currently selected source */ + } + + if (ptp_get_sequence(&msg->header) + != ptp_get_sequence(&state->twostep_packet.header)) + { + ptpwarn("PTP follow-up packet sequence %ld does not match initial " + "sync packet sequence %ld, ignoring\n", + (long)ptp_get_sequence(&msg->header), + (long)ptp_get_sequence(&state->twostep_packet.header)); + return OK; + } + + ptp_format_to_timespec(msg->origintimestamp, &remote_time); + clock_timespec_subtract(&remote_time, &state->twostep_rxtime, &delta); + + return ptp_update_local_clock(state, &delta); +} + +/* Determine received packet type and process it */ + +static int ptp_process_rx_packet(struct ptp_state_s *state, ssize_t length) +{ + ptpwarn("Got packet: %d bytes\n", length); + + if (length < sizeof(struct ptp_header_s)) + { + ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n", + (int)length); + return OK; + } + + if (state->rxbuf.header.domain != CONFIG_NETUTILS_PTPD_DOMAIN) + { + /* Part of different clock domain, ignore */ + + return OK; + } + + 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)); + return ptp_process_announce(state, &state->rxbuf.announce); + + case PTP_MSGTYPE_SYNC: + ptpinfo("Got sync packet, seq %ld\n", + (long)ptp_get_sequence(&state->rxbuf.header)); + return ptp_process_sync(state, &state->rxbuf.sync); + + case PTP_MSGTYPE_FOLLOW_UP: + ptpinfo("Got follow-up packet, seq %ld\n", + (long)ptp_get_sequence(&state->rxbuf.header)); + return ptp_process_followup(state, &state->rxbuf.follow_up); +#endif + + default: + ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n", + state->rxbuf.header.messagetype); + return OK; + } +} + +static int ptp_daemon(int argc, FAR char** argv) +{ + const char *interface = "eth0"; + struct ptp_state_s *state; + struct pollfd pollfds[2]; + struct msghdr rxhdr; + struct iovec rxiov; + int ret; + + memset(&rxhdr, 0, sizeof(rxhdr)); + memset(&rxiov, 0, sizeof(rxiov)); + + 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"); + return ERROR; + } + + pollfds[0].events = POLLIN; + pollfds[0].fd = state->event_socket; + pollfds[1].events = POLLIN; + pollfds[1].fd = state->info_socket; + + while (!state->stop) + { + rxhdr.msg_name = NULL; + rxhdr.msg_namelen = 0; + rxhdr.msg_iov = &rxiov; + rxhdr.msg_iovlen = 1; + rxhdr.msg_control = &state->rxcmsg; + rxhdr.msg_controllen = sizeof(state->rxcmsg); + rxhdr.msg_flags = 0; + rxiov.iov_base = &state->rxbuf; + rxiov.iov_len = sizeof(state->rxbuf); + + pollfds[0].revents = 0; + pollfds[1].revents = 0; + ret = poll(pollfds, 2, PTPD_POLL_INTERVAL); + ptp_gettime(state, &state->rxtime); + + if (pollfds[0].revents) + { + /* Receive time-critical packet, potentially with cmsg + * indicating the timestamp. + */ + + ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT); + if (ret > 0) + { + ptp_process_rx_packet(state, ret); + } + } + + if (pollfds[1].revents) + { + /* Receive non-time-critical packet. */ + + ret = recv(state->info_socket, &state->rxbuf, sizeof(state->rxbuf), + MSG_DONTWAIT); + if (ret > 0) + { + ptp_process_rx_packet(state, ret); + } + } + + ptp_periodic_send(state); + } + + ptp_destroy_state(state); + free(state); + + return 0; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: ptpd_start + * + * Description: + * Start the PTP daemon and bind it to specified interface. + * + * 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(const char *interface) +{ + int ret; + FAR char *task_argv[] = { + (FAR char *)interface, + NULL + }; + + ret = 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(ret, 0) != OK) + { + return ERROR; + } + else + { + return OK; + } +} + +/* TODO: Implement status and stop interfaces */ \ No newline at end of file diff --git a/netutils/ptpd/ptpv2.h b/netutils/ptpd/ptpv2.h new file mode 100644 index 00000000000..13b131232d8 --- /dev/null +++ b/netutils/ptpd/ptpv2.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * apps/netutils/ptpd/ptpv2.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_NETUTILS_PTPD_PTPV2_H +#define __APPS_NETUTILS_PTPD_PTPV2_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Time-critical messages (id < 8) are sent to port 319, + * other messages to port 320. + */ + +#define PTP_UDP_PORT_EVENT 319 +#define PTP_UDP_PORT_INFO 320 + +/* Multicast address to send to: 224.0.1.129 */ + +#define PTP_MULTICAST_ADDR ((in_addr_t)0xE0000181) + +/* Message types */ + +#define PTP_MSGTYPE_MASK 0x0F +#define PTP_MSGTYPE_SYNC 0 +#define PTP_MSGTYPE_DELAY_REQ 1 +#define PTP_MSGTYPE_FOLLOW_UP 8 +#define PTP_MSGTYPE_DELAY_RESP 9 +#define PTP_MSGTYPE_ANNOUNCE 11 + +/* Message flags */ + +#define PTP_FLAGS0_TWOSTEP (1 << 1) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Defined in IEEE 1588-2008 Precision Time Protocol + * All multi-byte fields are big-endian. + */ + +/* Common header for all message types */ + +struct ptp_header_s +{ + uint8_t messagetype; + uint8_t version; + uint8_t messagelength[2]; + uint8_t domain; + uint8_t reserved1; + uint8_t flags[2]; + uint8_t correction[8]; + uint8_t reserved2[4]; + uint8_t sourceidentity[8]; + uint8_t sourceportindex[2]; + uint8_t sequenceid[2]; + uint8_t controlfield; + uint8_t logmessageinterval; +}; + +/* Announce a master clock */ + +struct ptp_announce_s +{ + struct ptp_header_s header; + uint8_t origintimestamp[10]; + uint8_t utcoffset[2]; + uint8_t reserved; + uint8_t gm_priority1; + uint8_t gm_quality[4]; + uint8_t gm_priority2; + uint8_t gm_identity[8]; + uint8_t stepsremoved[2]; + uint8_t timesource; +}; + +/* Sync: transmit timestamp from master clock */ + +struct ptp_sync_s +{ + struct ptp_header_s header; + uint8_t origintimestamp[10]; +}; + +/* FollowUp: actual timestamp of when sync message was sent */ + +struct ptp_follow_up_s +{ + struct ptp_header_s header; + uint8_t origintimestamp[10]; +}; + +#endif /* __APPS_NETUTILS_PTPD_PTPV2_H */ \ No newline at end of file diff --git a/system/ptpd/Kconfig b/system/ptpd/Kconfig new file mode 100644 index 00000000000..addaf5a7272 --- /dev/null +++ b/system/ptpd/Kconfig @@ -0,0 +1,24 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config SYSTEM_PTPD + tristate "PTP daemon commands" + default n + select NETUTILS_PTPD + ---help--- + Enable 'ptpd' command to start the PTP daemon + + +if SYSTEM_PTPD + +config SYSTEM_PTPD_PRIORITY + int "PTPD utility command task priority" + default 100 + +config SYSTEM_PTPD_STACKSIZE + int "PTPD utility command stack size" + default DEFAULT_TASK_STACKSIZE + +endif diff --git a/system/ptpd/Make.defs b/system/ptpd/Make.defs new file mode 100644 index 00000000000..065a6c94bc7 --- /dev/null +++ b/system/ptpd/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/system/ptpd/Make.defs +# +# 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. +# +############################################################################ + +ifneq ($(CONFIG_SYSTEM_PTPD),) +CONFIGURED_APPS += $(APPDIR)/system/ptpd +endif diff --git a/system/ptpd/Makefile b/system/ptpd/Makefile new file mode 100644 index 00000000000..31c8355fc58 --- /dev/null +++ b/system/ptpd/Makefile @@ -0,0 +1,30 @@ +############################################################################ +# apps/system/ptpd/Makefile +# +# 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. +# +############################################################################ + +include $(APPDIR)/Make.defs + +PROGNAME = ptpd +PRIORITY = $(CONFIG_SYSTEM_PTPD_PRIORITY) +STACKSIZE = $(CONFIG_SYSTEM_PTPD_STACKSIZE) +MODULE = $(CONFIG_SYSTEM_PTPD) + +MAINSRC = ptpd_main.c + +include $(APPDIR)/Application.mk diff --git a/system/ptpd/ptpd_main.c b/system/ptpd/ptpd_main.c new file mode 100644 index 00000000000..0b903ceb698 --- /dev/null +++ b/system/ptpd/ptpd_main.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * apps/system/ptpd/ptpd_main.c + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "netutils/ptpd.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * ptpd_main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + int pid; + + if (argc != 2) + { + fprintf(stderr, "Usage: ptpd \n"); + return 1; + } + + pid = ptpd_start(argv[1]); + if (pid < 0) + { + fprintf(stderr, "ERROR: ptpd_start() failed\n"); + return EXIT_FAILURE; + } + + printf("Started the PTP daemon as PID=%d\n", pid); + return EXIT_SUCCESS; +}