/**************************************************************************** * 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; /* 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 timeval))]; /* 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; }; #ifdef CONFIG_NETUTILS_PTPD_SERVER # define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC #else # define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_TIMEOUT_MS #endif /* PTP debug messages are enabled by either CONFIG_DEBUG_NET_INFO * or separately by CONFIG_NETUTILS_PTPD_DEBUG. This simplifies * debugging without having excessive amount of logging from net. */ #ifdef CONFIG_NETUTILS_PTPD_DEBUG # define ptpinfo _info # define ptpwarn _warn # define ptperr _err #else # define ptpinfo ninfo # define ptpwarn nwarn # define ptperr nerr #endif /**************************************************************************** * Private Functions ****************************************************************************/ /* Convert from timespec to PTP format */ static void timespec_to_ptp_format(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; } } static int64_t timespec_to_ms(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(const struct timespec *ts1, const struct timespec *ts2) { int64_t delta_s; delta_s = ts1->tv_sec - ts2->tv_sec; #ifdef CONFIG_SYSTEM_TIME64 /* 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; } #endif 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(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. */ static int ptp_adjtime(struct ptp_state_s *state, int64_t delta_ns) { struct timeval delta; delta.tv_sec = delta_ns / NSEC_PER_SEC; delta_ns -= (int64_t)delta.tv_sec * NSEC_PER_SEC; delta.tv_usec = delta_ns / NSEC_PER_USEC; return adjtime(&delta, NULL); } /* Get timestamp of latest received packet */ static int ptp_getrxtime(struct ptp_state_s *state, struct msghdr *rxhdr, struct timespec *ts) { /* Get hardware or kernel timestamp if available */ #ifdef CONFIG_NET_TIMESTAMP struct cmsghdr *cmsg; for_each_cmsghdr(cmsg, rxhdr) { if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP && cmsg->cmsg_len == CMSG_LEN(sizeof(struct timeval))) { TIMEVAL_TO_TIMESPEC((struct timeval *)CMSG_DATA(cmsg), 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"); #endif /* Fall back to current timestamp */ return ptp_gettime(state, ts); } /* Initialize PTP client/server state and create sockets */ static int ptp_initialize_state(struct ptp_state_s *state, const char *interface) { int ret; int arg; 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); clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast); ret = ipmsfilter(&state->interface_addr.sin_addr, &bind_addr.sin_addr, MCAST_INCLUDE); if (ret < 0) { ptperr("Failed to bind multicast address: %d\n", errno); return ERROR; } /* Bind socket for events */ bind_addr.sin_port = HTONS(PTP_UDP_PORT_EVENT); ret = bind(state->event_socket, (struct sockaddr *)&bind_addr, sizeof(bind_addr)); if (ret < 0) { ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port); return ERROR; } #ifdef CONFIG_NET_TIMESTAMP arg = 1; ret = setsockopt(state->event_socket, SOL_SOCKET, SO_TIMESTAMP, &arg, sizeof(arg)); if (ret < 0) { ptperr("Failed to enable SO_TIMESTAMP: %s\n", strerror(errno)); /* PTPD can operate without, but with worse accuracy */ } #endif /* Bind socket for announcements */ bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO); ret = bind(state->info_socket, (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; } /* 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(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; } /* 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->announce_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); #ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC msg.header.flags[0] = PTP_FLAGS0_TWOSTEP; #endif txhdr.msg_name = &addr; txhdr.msg_namelen = sizeof(addr); txhdr.msg_iov = &txiov; txhdr.msg_iovlen = 1; txhdr.msg_control = controlbuf; txhdr.msg_controllen = sizeof(controlbuf); txiov.iov_base = &msg; txiov.iov_len = sizeof(msg); /* Timestamp and send the sync message */ ptp_increment_sequence(&state->sync_seq, &msg.header); ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); ret = sendmsg(state->tx_socket, &txhdr, 0); if (ret < 0) { ptperr("sendmsg for sync message failed: %d\n", errno); return ret; } #ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC /* Get timestamp after send completes and send follow-up message * * TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here. */ ptp_gettime(state, &ts); timespec_to_ptp_format(&ts, msg.origintimestamp); msg.header.messagetype = PTP_MSGTYPE_FOLLOW_UP; msg.header.flags[0] = 0; addr.sin_port = HTONS(PTP_UDP_PORT_INFO); ret = sendto(state->tx_socket, &msg, sizeof(msg), 0, (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)); #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(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 = sendto(state->tx_socket, &req, sizeof(req), 0, (struct sockaddr *)&addr, sizeof(addr)); /* Get timestamp after send completes. * TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here. */ ptp_gettime(state, &state->delayreq_time); if (ret < 0) { ptperr("sendto 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(struct ptp_state_s *state) { #ifdef CONFIG_NETUTILS_PTPD_SERVER /* If there is no better master clock on the network, * act as the reference source and send server packets. */ if (!state->selected_source_valid) { 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 /* CONFIG_NETUTILS_PTPD_SERVER */ #ifdef CONFIG_NETUTILS_PTPD_SEND_DELAYREQ if (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); } } #endif return OK; } /* Process received PTP announcement */ static int ptp_process_announce(struct ptp_state_s *state, struct ptp_announce_s *msg) { clock_gettime(CLOCK_MONOTONIC, &state->last_received_announce); if (is_better_clock(msg, &state->own_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(struct ptp_state_s *state, struct timespec *remote_timestamp, 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: %lld.%09ld, remote time %lld.%09ld\n", (long long)local_timestamp->tv_sec, (long)local_timestamp->tv_nsec, (long long)remote_timestamp->tv_sec, (long)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 %lld.%09ld s\n", (long long)new_time.tv_sec, (long)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); ret = ptp_adjtime(state, adjustment_ns); 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(struct ptp_state_s *state, struct ptp_sync_s *msg) { struct timespec remote_time; 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. */ 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 int ptp_process_followup(struct ptp_state_s *state, struct ptp_follow_up_s *msg) { struct timespec remote_time; 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; } /* 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); return ptp_update_local_clock(state, &remote_time, &state->twostep_rxtime); } static int ptp_process_delay_req(struct ptp_state_s *state, 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 = sendto(state->tx_socket, &resp, sizeof(resp), 0, (struct sockaddr *)&addr, sizeof(addr)); if (ret < 0) { ptperr("sendto 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(struct ptp_state_s *state, 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(struct ptp_state_s *state, ssize_t 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) { #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); 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); #endif #ifdef CONFIG_NETUTILS_PTPD_SERVER case PTP_MSGTYPE_DELAY_REQ: ptpinfo("Got delay req, seq %ld\n", (long)ptp_get_sequence(&state->rxbuf.header)); return ptp_process_delay_req(state, &state->rxbuf.delay_req); #endif default: ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n", state->rxbuf.header.messagetype); return OK; } } 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) { 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, 2, PTPD_POLL_INTERVAL); 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_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 */