/**************************************************************************** * apps/netutils/ptpd/ptpd.c * * SPDX-License-Identifier: Apache-2.0 * * 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 #include #include #include #include "netutils/netlib.h" #include "ptpv2.h" /**************************************************************************** * Private Types ****************************************************************************/ /* Carrier structure for querying PTPD status */ struct ptpd_statusreq_s { FAR sem_t *done; FAR struct ptpd_status_s *dest; }; /* Main PTPD state storage */ struct ptp_state_s { /* Request for PTPD task to stop or report status */ bool stop; struct ptpd_statusreq_s status_req; /* 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; /* The ptp device file descriptor */ clockid_t clockid; /* 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; /* Previous measurement and estimated clock drift rate */ struct timespec last_delta_timestamp; int64_t last_delta_ns; int64_t last_adjtime_ns; long drift_avg_total_ms; long drift_ppb; /* Identity of currently selected clock source, * from the latest announcement message. * * The timestamps are used for timeout when a source disappears. * They are from the local CLOCK_MONOTONIC. */ bool selected_source_valid; /* True if operating as client */ struct ptp_announce_s selected_source; /* Currently selected server */ struct timespec last_received_multicast; /* Any multicast packet */ struct timespec last_received_announce; /* Announce from any server */ struct timespec last_received_sync; /* Sync from selected source */ /* Last transmitted packet timestamps (CLOCK_MONOTONIC) * Used to set transmission interval. */ struct timespec last_transmitted_sync; struct timespec last_transmitted_announce; struct timespec last_transmitted_delayresp; struct timespec last_transmitted_delayreq; /* Timestamps related to path delay calculation (CLOCK_REALTIME) */ bool can_send_delayreq; struct timespec delayreq_time; int path_delay_avgcount; long path_delay_ns; long delayreq_interval; /* Latest received packet and its timestamp (CLOCK_REALTIME) */ 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; struct ptp_delay_req_s delay_req; struct ptp_delay_resp_s delay_resp; uint8_t raw[128]; } rxbuf; 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. */ struct ptp_sync_s twostep_packet; struct timespec twostep_rxtime; FAR const struct ptpd_config_s *config; }; /**************************************************************************** * Private Functions ****************************************************************************/ /* Convert from timespec to PTP format */ 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, * both fields big-endian. */ timestamp[0] = (uint8_t)(ts->tv_sec >> 40); timestamp[1] = (uint8_t)(ts->tv_sec >> 32); 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(FAR const uint8_t *timestamp, FAR 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(FAR const struct ptp_announce_s *a, FAR const struct ptp_announce_s *b) { /* Main priority field */ if (a->gm_priority1 < b->gm_priority1) { return true; } if (a->gm_priority1 > b->gm_priority1) { return false; } /* Clock class */ if (a->gm_quality[0] < b->gm_quality[0]) { return true; } if (a->gm_quality[0] > b->gm_quality[0]) { return false; } /* Clock accuracy */ if (a->gm_quality[1] < b->gm_quality[1]) { return true; } if (a->gm_quality[1] > b->gm_quality[1]) { return false; } /* Clock variance high byte */ if (a->gm_quality[2] < b->gm_quality[2]) { return true; } if (a->gm_quality[2] > b->gm_quality[2]) { return false; } /* Clock variance low byte */ if (a->gm_quality[3] < b->gm_quality[3]) { return true; } if (a->gm_quality[3] > b->gm_quality[3]) { return false; } /* Sub priority field */ if (a->gm_priority2 < b->gm_priority2) { return true; } 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) { return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC); } /* Get positive or negative delta between two timespec values. * If value would exceed int64 limit (292 years), return INT64_MAX/MIN. */ static int64_t timespec_delta_ns(FAR const struct timespec *ts1, FAR const struct timespec *ts2) { int64_t delta_s; delta_s = ts1->tv_sec - ts2->tv_sec; /* Conversion to nanoseconds could overflow if the system time is 64-bit */ if (delta_s >= INT64_MAX / NSEC_PER_SEC) { return INT64_MAX; } else if (delta_s <= INT64_MIN / NSEC_PER_SEC) { return INT64_MIN; } return delta_s * NSEC_PER_SEC + (ts1->tv_nsec - ts2->tv_nsec); } /* Check if the currently selected source is still valid */ static bool is_selected_source_valid(FAR 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(FAR uint16_t *sequence_num, FAR 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(FAR const struct ptp_header_s *hdr) { return ((uint16_t)hdr->sequenceid[0] << 8) | hdr->sequenceid[1]; } 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) { return clock_gettime(state->clockid, ts); } /* Change current system timestamp by jumping */ static int ptp_settime(FAR struct ptp_state_s *state, FAR struct timespec *ts) { return clock_settime(state->clockid, ts); } /* Smoothly adjust timestamp. */ static int ptp_adjtime(FAR struct ptp_state_s *state, int64_t delta_ns, int64_t ppb) { if (state->clockid == CLOCK_REALTIME) { struct timeval delta; delta.tv_sec = delta_ns / NSEC_PER_SEC; delta_ns -= 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; return clock_adjtime(state->clockid, &buf); } } /* Get timestamp of latest received packet */ 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 */ 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_TIMESTAMPNS && cmsg->cmsg_len == CMSG_LEN(sizeof(struct timespec))) { memcpy(ts, CMSG_DATA(cmsg), sizeof(*ts)); /* Sanity-check the value */ if (ts->tv_sec > 0 || ts->tv_nsec > 0) { return OK; } } } ptpwarn("CONFIG_NET_TIMESTAMP enabled but did not get packet timestamp\n"); return ERROR; } /* Unsubscribe multicast and destroy sockets */ static int ptp_destroy_state(FAR struct ptp_state_s *state) { struct in_addr mcast_addr; ptp_close(state->clockid); mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); ipmsfilter(&state->interface_addr.sin_addr, &mcast_addr, MCAST_EXCLUDE); if (state->tx_socket > 0) { close(state->tx_socket); state->tx_socket = -1; } if (state->event_socket > 0) { close(state->event_socket); state->event_socket = -1; } if (state->info_socket > 0) { close(state->info_socket); state->info_socket = -1; } return OK; } /* Initialize PTP client/server state and create sockets */ static int ptp_initialize_state(FAR struct ptp_state_s *state) { int ret; int arg = 1; struct ifreq req; 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 */ if (state->config->af == AF_PACKET) { 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; 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) { 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; } } if (state->config->hardware_ts) { ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMPNS, &arg, sizeof(arg)); if (ret < 0) { ptperr("Failed to enable SO_TIMESTAMPNS: %s\n", strerror(errno)); goto errout; } } /* Get address information of the specified interface for binding socket * Only supports IPv4 currently. */ memset(&req, 0, sizeof(req)); 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", state->config->interface); goto errout; } state->interface_addr = *(FAR 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", state->config->interface); goto errout; } 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; clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast); return OK; errout: ptp_destroy_state(state); return ERROR; } /* Re-subscribe multicast address. * This can become necessary if Ethernet interface gets reset or if external * IGMP-compliant Ethernet switch gets plugged in. */ static int ptp_check_multicast_status(FAR struct ptp_state_s *state) { #if CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS > 0 struct in_addr mcast_addr; struct timespec time_now; struct timespec delta; clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_received_multicast, &delta); if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS) { /* Remove and re-add the multicast group */ state->last_received_multicast = time_now; mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); ipmsfilter(&state->interface_addr.sin_addr, &mcast_addr, MCAST_EXCLUDE); return ipmsfilter(&state->interface_addr.sin_addr, &mcast_addr, MCAST_INCLUDE); } #else UNUSED(state); #endif /* CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS */ 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) + sizeof(struct ptp_announce_s)]; FAR struct ether_header *header; struct msghdr msg; struct iovec iov; DEBUGASSERT(sizeof(struct ptp_announce_s) >= buflen); 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) { 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->announce_seq, &msg.header); ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); ret = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL); if (ret < 0) { ptperr("ptp sendmsg 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(FAR struct ptp_state_s *state) { struct ptp_sync_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_EVENT); memset(&msg, 0, sizeof(msg)); msg.header = state->own_identity.header; msg.header.messagetype = PTP_MSGTYPE_SYNC; msg.header.messagelength[1] = sizeof(msg); #ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC msg.header.flags[0] = PTP_FLAGS0_TWOSTEP; #endif /* 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 = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), &ts); if (ret < 0) { ptperr("sendmsg for sync message failed: %d\n", errno); return ret; } #ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC 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 = ptp_sendmsg(state, &msg, sizeof(msg), &addr, sizeof(addr), NULL); if (ret < 0) { ptperr("ptp sendmsg for follow-up message failed: %d\n", errno); return ret; } ptpinfo("Sent sync + follow-up, seq %ld\n", (long)ptp_get_sequence(&msg.header)); #else ptpinfo("Sent sync, seq %ld\n", (long)ptp_get_sequence(&msg.header)); #endif /* CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC */ return OK; } /* Send delay request packet to selected source */ static int ptp_send_delay_req(FAR struct ptp_state_s *state) { struct ptp_delay_req_s req; struct sockaddr_in addr; int ret; addr.sin_family = AF_INET; addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); memset(&req, 0, sizeof(req)); req.header = state->own_identity.header; req.header.messagetype = PTP_MSGTYPE_DELAY_REQ; req.header.messagelength[1] = sizeof(req); ptp_increment_sequence(&state->delay_req_seq, &req.header); ptp_gettime(state, &state->delayreq_time); timespec_to_ptp_format(&state->delayreq_time, req.origintimestamp); ret = ptp_sendmsg(state, &req, sizeof(req), &addr, sizeof(addr), &state->delayreq_time); if (ret < 0) { ptperr("ptp sendmsg failed: %d", errno); } else { clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayreq); ptpinfo("Sent delay req, seq %ld\n", (long)ptp_get_sequence(&req.header)); } return ret; } /* Check if we need to send packets */ static int ptp_periodic_send(FAR struct ptp_state_s *state) { /* If there is no better master clock on the network, * act as the reference source and send server packets. */ if (!state->config->client_only && !state->selected_source_valid) { struct timespec time_now; struct timespec delta; clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_transmitted_announce, &delta); if (state->config->bmca && 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); } } if (state->config->delay_e2e && state->selected_source_valid && state->can_send_delayreq) { struct timespec time_now; struct timespec delta; clock_gettime(CLOCK_MONOTONIC, &time_now); clock_timespec_subtract(&time_now, &state->last_transmitted_delayreq, &delta); if (timespec_to_ms(&delta) > state->delayreq_interval * MSEC_PER_SEC) { ptp_send_delay_req(state); } } return OK; } /* Process received PTP announcement */ static int ptp_process_announce(FAR struct ptp_state_s *state, FAR struct ptp_announce_s *msg) { clock_gettime(CLOCK_MONOTONIC, &state->last_received_announce); if (state->conifg->bmca && is_better_clock(msg, &state->n_identity)) { if (!state->selected_source_valid || is_better_clock(msg, &state->selected_source)) { ptpinfo("Switching to better PTP time source\n"); state->selected_source = *msg; state->last_received_sync = state->last_received_announce; state->path_delay_avgcount = 0; state->path_delay_ns = 0; state->delayreq_time.tv_sec = 0; } } return OK; } /* Update local clock either by smooth adjustment or by jumping. * Remote time was remote_timestamp at local_timestamp. */ static int ptp_update_local_clock(FAR struct ptp_state_s *state, FAR struct timespec *remote_timestamp, FAR struct timespec *local_timestamp) { int ret; int64_t delta_ns; int64_t absdelta_ns; const int64_t adj_limit_ns = CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS * (int64_t)NSEC_PER_MSEC; ptpinfo("Local time: %jd.%09ld, remote time %jd.%09ld\n", (intmax_t)local_timestamp->tv_sec, local_timestamp->tv_nsec, (intmax_t)remote_timestamp->tv_sec, remote_timestamp->tv_nsec); delta_ns = timespec_delta_ns(remote_timestamp, local_timestamp); delta_ns += state->path_delay_ns; absdelta_ns = (delta_ns < 0) ? -delta_ns : delta_ns; if (absdelta_ns > adj_limit_ns) { /* Large difference, move by jumping. * Account for delay since packet was received. */ struct timespec new_time; ptp_gettime(state, &new_time); clock_timespec_subtract(&new_time, local_timestamp, &new_time); clock_timespec_add(&new_time, remote_timestamp, &new_time); ret = ptp_settime(state, &new_time); /* Reinitialize drift adjustment parameters */ state->last_delta_timestamp = new_time; state->last_delta_ns = 0; state->last_adjtime_ns = 0; state->drift_avg_total_ms = 0; state->drift_ppb = 0; if (ret == OK) { ptpinfo("Jumped to timestamp %jd.%09ld s\n", (intmax_t)new_time.tv_sec, new_time.tv_nsec); } else { ptperr("ptp_settime() failed: %d\n", errno); } } else { /* Track drift rate based on two consecutive measurements and * the adjustment that was made previously. */ int64_t drift_ppb; struct timespec interval; int interval_ms; int max_avg_period_ms; int64_t adjustment_ns; clock_timespec_subtract(local_timestamp, &state->last_delta_timestamp, &interval); interval_ms = timespec_to_ms(&interval); if (interval_ms > 0 && interval_ms < CONFIG_NETUTILS_PTPD_TIMEOUT_MS) { drift_ppb = (delta_ns - state->last_delta_ns) * MSEC_PER_SEC / interval_ms; } else { ptpwarn("Measurement interval out of range: %d ms\n", interval_ms); drift_ppb = 0; interval_ms = 1; } /* Account for the adjustment previously made */ drift_ppb += state->last_adjtime_ns * MSEC_PER_SEC / CONFIG_CLOCK_ADJTIME_PERIOD_MS; if (drift_ppb > CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000 || drift_ppb < -CONFIG_CLOCK_ADJTIME_SLEWLIMIT_PPM * 1000) { ptpwarn("Drift estimate out of range: %lld\n", (long long)drift_ppb); drift_ppb = state->drift_ppb; } /* Take direct average of drift estimate for first measurements, * after that update the exponential sliding average. * Measurements are weighted according to the interval, because * drift estimate is more accurate over longer timespan. */ state->drift_avg_total_ms += interval_ms; max_avg_period_ms = CONFIG_NETUTILS_PTPD_DRIFT_AVERAGE_S * MSEC_PER_SEC; if (state->drift_avg_total_ms > max_avg_period_ms) { state->drift_avg_total_ms = max_avg_period_ms; } state->drift_ppb += (drift_ppb - state->drift_ppb) * interval_ms / state->drift_avg_total_ms; /* Compute the value we need to give to adjtime() to match the * drift rate. */ adjustment_ns = state->drift_ppb * CONFIG_CLOCK_ADJTIME_PERIOD_MS / MSEC_PER_SEC; /* Drift estimation ensures local clock runs at same rate as remote. * * Adding the current clock offset to adjustment brings the clocks * to match. To avoid individual outliers from causing jitter, we * take the larger signed value of two previous deltas. This is based * on the logic that packets can get delayed in transit, but do not * travel backwards in time. * * Clock offset is applied over ADJTIME_PERIOD. If there is significant * noise in measurements, increasing ADJTIME_PERIOD will reduce its * effect on the local clock run rate. */ if (state->last_delta_ns > delta_ns) { adjustment_ns += state->last_delta_ns; } else { adjustment_ns += delta_ns; } /* Apply adjustment and store information for next time */ state->last_delta_ns = delta_ns; state->last_delta_timestamp = *local_timestamp; state->last_adjtime_ns = adjustment_ns; ptpinfo("Delta: %+lld ns, adjustment %+lld ns, drift rate %+lld ppb\n", (long long)delta_ns, (long long)state->last_adjtime_ns, (long long)state->drift_ppb); 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) { ptperr("ptp_adjtime() failed: %d\n", errno); } /* Check if clock is stable enough for sending delay requests */ if (absdelta_ns < CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS) { state->can_send_delayreq = true; } } return ret; } /* Process received PTP sync packet */ static int ptp_process_sync(FAR struct ptp_state_s *state, FAR struct ptp_sync_s *msg) { struct timespec remote_time; if (state->config->bmca && 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. */ state->twostep_rxtime = state->rxtime; state->twostep_packet = *msg; ptpinfo("Waiting for follow-up\n"); return OK; } /* Update local clock */ ptp_format_to_timespec(msg->origintimestamp, &remote_time); 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: %jd.%09ld\n", (intmax_t)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: %jd.%09ld\n", (intmax_t)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 (state->config->bmca && 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; } /* Update local clock based on the remote timestamp we received now * and the local timestamp of when the sync packet was received. */ 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); } static int ptp_process_delay_req(FAR struct ptp_state_s *state, FAR struct ptp_delay_req_s *msg) { struct ptp_delay_resp_s resp; struct sockaddr_in addr; int ret; if (state->selected_source_valid) { /* We are operating as a client, ignore delay requests */ return OK; } addr.sin_family = AF_INET; addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR); addr.sin_port = HTONS(PTP_UDP_PORT_INFO); memset(&resp, 0, sizeof(resp)); resp.header = state->own_identity.header; resp.header.messagetype = PTP_MSGTYPE_DELAY_RESP; resp.header.messagelength[1] = sizeof(resp); timespec_to_ptp_format(&state->rxtime, resp.receivetimestamp); memcpy(resp.reqidentity, msg->header.sourceidentity, sizeof(resp.reqidentity)); memcpy(resp.reqportindex, msg->header.sourceportindex, sizeof(resp.reqportindex)); memcpy(resp.header.sequenceid, msg->header.sequenceid, sizeof(resp.header.sequenceid)); resp.header.logmessageinterval = CONFIG_NETUTILS_PTPD_DELAYRESP_INTERVAL; ret = ptp_sendmsg(state, &resp, sizeof(resp), &addr, sizeof(addr), NULL); if (ret < 0) { ptperr("ptp sendmsg failed: %d", errno); } else { clock_gettime(CLOCK_MONOTONIC, &state->last_transmitted_delayresp); ptpinfo("Sent delay resp, seq %ld\n", (long)ptp_get_sequence(&msg->header)); } return ret; } static int ptp_process_delay_resp(FAR struct ptp_state_s *state, FAR struct ptp_delay_resp_s *msg) { int64_t path_delay; int64_t sync_delay; struct timespec remote_rxtime; uint16_t sequence; int interval; if (!state->selected_source_valid || memcmp(msg->header.sourceidentity, state->selected_source.header.sourceidentity, sizeof(msg->header.sourceidentity)) != 0 || memcmp(msg->reqidentity, state->own_identity.header.sourceidentity, sizeof(msg->reqidentity)) != 0) { return OK; /* This packet wasn't for us */ } sequence = ptp_get_sequence(&msg->header); if (sequence != state->delay_req_seq) { ptpwarn("Ignoring out-of-sequence delay resp (%d vs. expected %d)\n", (int)sequence, (int)state->delay_req_seq); return OK; } /* Path delay is calculated as the average between delta for sync * message and delta for delay req message. * (IEEE-1588 section 11.3: Delay request-response mechanism) */ ptp_format_to_timespec(msg->receivetimestamp, &remote_rxtime); path_delay = timespec_delta_ns(&remote_rxtime, &state->delayreq_time); sync_delay = state->path_delay_ns - state->last_delta_ns; path_delay = (path_delay + sync_delay) / 2; if (path_delay >= 0 && path_delay < CONFIG_NETUTILS_PTPD_MAX_PATH_DELAY_NS) { if (state->path_delay_avgcount < CONFIG_NETUTILS_PTPD_DELAYREQ_AVGCOUNT) { state->path_delay_avgcount++; } state->path_delay_ns += (path_delay - state->path_delay_ns) / state->path_delay_avgcount; ptpinfo("Path delay: %ld ns (avg: %ld ns)\n", (long)path_delay, (long)state->path_delay_ns); } else { ptpwarn("Path delay out of range: %lld ns\n", (long long)path_delay); } /* Calculate interval until next packet */ if (msg->header.logmessageinterval <= 12) { interval = (1 << msg->header.logmessageinterval); } else { interval = 4096; /* Refuse to obey excessively long intervals */ } /* Randomize up to 2x nominal delay) */ state->delayreq_interval = interval + (random() % interval); return OK; } /* Determine received packet type and process it */ 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", (int)length); return OK; } if (state->rxbuf.header.domain != CONFIG_NETUTILS_PTPD_DOMAIN) { /* Part of different clock domain, ignore */ return OK; } clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast); switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK) { 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); case PTP_MSGTYPE_DELAY_RESP: ptpinfo("Got delay-resp, seq %ld\n", (long)ptp_get_sequence(&state->rxbuf.header)); return ptp_process_delay_resp(state, &state->rxbuf.delay_resp); 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); default: ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n", state->rxbuf.header.messagetype); return OK; } } /* Signal handler for status / stop requests */ static void ptp_signal_handler(int signo, FAR siginfo_t *siginfo, FAR void *context) { FAR struct ptp_state_s *state = (FAR struct ptp_state_s *)siginfo->si_user; if (signo == SIGHUP) { state->stop = true; } else if (signo == SIGUSR1 && siginfo->si_value.sival_ptr) { state->status_req = *(FAR struct ptpd_statusreq_s *)siginfo->si_value.sival_ptr; } } static void ptp_setup_sighandlers(FAR struct ptp_state_s *state) { struct sigaction act; act.sa_sigaction = ptp_signal_handler; sigfillset(&act.sa_mask); act.sa_flags = SA_SIGINFO; act.sa_user = state; sigaction(SIGHUP, &act, NULL); sigaction(SIGUSR1, &act, NULL); } /* Process status information request */ static void ptp_process_statusreq(FAR struct ptp_state_s *state) { FAR struct ptpd_status_s *status; if (!state->status_req.dest) { return; /* No active request */ } status = state->status_req.dest; status->clock_source_valid = state->selected_source_valid; if (status->clock_source_valid) { /* Copy relevant parts of announce info to status struct */ FAR struct ptp_announce_s *s = &state->selected_source; memcpy(status->clock_source_info.id, s->header.sourceidentity, sizeof(status->clock_source_info.id)); status->clock_source_info.utcoffset = (int16_t)(((uint16_t)s->utcoffset[0] << 8) | s->utcoffset[1]); status->clock_source_info.priority1 = s->gm_priority1; status->clock_source_info.clockclass = s->gm_quality[0]; status->clock_source_info.accuracy = s->gm_quality[1]; status->clock_source_info.priority2 = s->gm_priority2; status->clock_source_info.variance = ((uint16_t)s->gm_quality[2] << 8) | s->gm_quality[3]; memcpy(status->clock_source_info.gm_id, s->gm_identity, sizeof(status->clock_source_info.gm_id)); status->clock_source_info.stepsremoved = ((uint16_t)s->stepsremoved[0] << 8) | s->stepsremoved[1]; status->clock_source_info.timesource = s->timesource; } /* Copy latest adjustment info */ status->last_clock_update = state->last_delta_timestamp; status->last_delta_ns = state->last_delta_ns; status->last_adjtime_ns = state->last_adjtime_ns; status->drift_ppb = state->drift_ppb; status->path_delay_ns = state->path_delay_ns; /* Copy timestamps */ status->last_received_multicast = state->last_received_multicast; status->last_received_announce = state->last_received_announce; status->last_received_sync = state->last_received_sync; status->last_transmitted_sync = state->last_transmitted_sync; status->last_transmitted_announce = state->last_transmitted_announce; status->last_transmitted_delayresp = state->last_transmitted_delayresp; status->last_transmitted_delayreq = state->last_transmitted_delayreq; /* Post semaphore to inform that we are done */ if (state->status_req.done) { sem_post(state->status_req.done); } state->status_req.done = NULL; state->status_req.dest = NULL; } /**************************************************************************** * 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 struct ptpd_config_s *config) { 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 (state == NULL) { return -ENOMEM; } state->config = config; if (ptp_initialize_state(state) != OK) { ptperr("Failed to initialize PTP state, exiting\n"); goto errout; } 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; if (state->info_socket > 0) { pollfds[1].events = POLLIN; pollfds[1].fd = state->info_socket; idx++; } while (!state->stop) { state->can_send_delayreq = false; 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, idx, timeout); 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_getrxtime(state, &rxhdr, &state->rxtime); 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); } } if (pollfds[0].revents == 0 && pollfds[1].revents == 0) { /* No packets received, check for multicast timeout */ ptp_check_multicast_status(state); } ptp_periodic_send(state); state->selected_source_valid = is_selected_source_valid(state); ptp_process_statusreq(state); } errout: ptp_destroy_state(state); free(state); return 0; } /**************************************************************************** * Name: ptpd_status * * Description: * Query status from a running PTP daemon. * * Input Parameters: * pid - Process ID previously returned by ptpd_start() * status - Pointer to storage for status information. * * Returned Value: * On success, returns OK. * On failure, a negated errno value is returned. * * Assumptions/Limitations: * Multiple threads with priority less than CONFIG_NETUTILS_PTPD_SERVERPRIO * can request status simultaneously. If higher priority threads request * status simultaneously, some of the requests may timeout. * ****************************************************************************/ int ptpd_status(int pid, FAR struct ptpd_status_s *status) { #ifndef CONFIG_BUILD_FLAT /* TODO: Use SHM memory to pass the status information if processes * do not share the same memory space. */ return -ENOTSUP; #else int ret = OK; sem_t donesem; struct ptpd_statusreq_s req; union sigval val; struct timespec timeout; /* Fill in the status request */ memset(status, 0, sizeof(struct ptpd_status_s)); sem_init(&donesem, 0, 0); req.done = &donesem; req.dest = status; val.sival_ptr = &req; if (sigqueue(pid, SIGUSR1, val) != OK) { return -errno; } /* Wait for status request to be handled */ clock_gettime(CLOCK_MONOTONIC, &timeout); timeout.tv_sec += 1; if (sem_clockwait(&donesem, CLOCK_MONOTONIC, &timeout) != 0) { ret = -errno; } sem_destroy(&donesem); return ret; #endif /* CONFIG_BUILD_FLAT */ } /**************************************************************************** * Name: ptpd_stop * * Description: * Stop PTP daemon * * Input Parameters: * pid - Process ID previously returned by ptpd_start() * * Returned Value: * On success, returns OK. * On failure, a negated errno value is returned. * ****************************************************************************/ int ptpd_stop(int pid) { if (kill(pid, SIGHUP) == OK) { return OK; } else { return -errno; } }