xiaoxiang781216 commented on code in PR #2101: URL: https://github.com/apache/nuttx-apps/pull/2101#discussion_r1344237029
########## 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) Review Comment: remove the check ########## 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 <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <sys/socket.h> +#include <sys/time.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <sched.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> +#include <unistd.h> +#include <fcntl.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netutils/ipmsfilter.h> + +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/poll.h> +#include <nuttx/net/netconfig.h> +#include <netutils/ptpd.h> +#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) Review Comment: add static ########## 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 <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <sys/socket.h> +#include <sys/time.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <sched.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> +#include <unistd.h> +#include <fcntl.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netutils/ipmsfilter.h> + +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/poll.h> +#include <nuttx/net/netconfig.h> +#include <netutils/ptpd.h> +#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 Review Comment: indent ########## 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 <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <sys/socket.h> +#include <sys/time.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <sched.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> +#include <unistd.h> +#include <fcntl.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netutils/ipmsfilter.h> + +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/poll.h> +#include <nuttx/net/netconfig.h> +#include <netutils/ptpd.h> +#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 Review Comment: indent too ########## 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 <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <sys/socket.h> +#include <sys/time.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <sched.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> +#include <unistd.h> +#include <fcntl.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netutils/ipmsfilter.h> + +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/poll.h> +#include <nuttx/net/netconfig.h> +#include <netutils/ptpd.h> Review Comment: add blank line ########## 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 <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <sys/socket.h> +#include <sys/time.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <time.h> +#include <sched.h> +#include <assert.h> +#include <errno.h> +#include <debug.h> +#include <unistd.h> +#include <fcntl.h> + +#include <netinet/in.h> +#include <arpa/inet.h> +#include <netutils/ipmsfilter.h> + +#include <net/if.h> +#include <sys/ioctl.h> +#include <sys/poll.h> +#include <nuttx/net/netconfig.h> +#include <netutils/ptpd.h> +#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 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 -1; + } + else + { + return pid; + } +} + +/* TODO: Implement status and stop interfaces */ Review Comment: add blank line -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org For queries about this service, please contact Infrastructure at: us...@infra.apache.org
