sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
78a8dd27c9
nuttx-apps/netutils/ptpd/ptpd.c
Petteri Aimonen fcd0729e21 ptpd: Fix reserved word 'class' 
When ptpd.h is included in C++ code, the use of identifier 'class'
caused a compilation error. Changed to "clockclass".
2023-12-07 18:26:39 -08:00

1627 lines
46 KiB
C
Raw Blame History

/****************************************************************************
* 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
****************************************************************************/
/* 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;
/* 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(FAR struct timespec *ts,
FAR 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(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)
{
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(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;
#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(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];
}
/* 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(FAR struct ptp_state_s *state,
FAR struct timespec *ts)
{
UNUSED(state);
return clock_gettime(CLOCK_REALTIME, ts);
}
/* Change current system timestamp by jumping */
static int ptp_settime(FAR struct ptp_state_s *state,
FAR struct timespec *ts)
{
UNUSED(state);
return clock_settime(CLOCK_REALTIME, ts);
}
/* Smoothly adjust timestamp. */
static int ptp_adjtime(FAR 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(FAR struct ptp_state_s *state,
FAR struct msghdr *rxhdr,
FAR 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((FAR 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(FAR struct ptp_state_s *state,
FAR const char *interface)
{
int ret;
struct ifreq req;
struct sockaddr_in bind_addr;
#ifdef CONFIG_NET_TIMESTAMP
int arg;
#endif
/* 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(FAR struct ptp_state_s *state)
{
struct in_addr mcast_addr;
mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
ipmsfilter(&state->interface_addr.sin_addr,
&mcast_addr,
MCAST_EXCLUDE);
if (state->tx_socket > 0)
{
close(state->tx_socket);
state->tx_socket = -1;
}
if (state->event_socket > 0)
{
close(state->event_socket);
state->event_socket = -1;
}
if (state->info_socket > 0)
{
close(state->info_socket);
state->info_socket = -1;
}
return OK;
}
/* 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;
}
/* 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 = 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(FAR struct ptp_state_s *state)
{
struct msghdr txhdr;
struct iovec txiov;
struct ptp_sync_s msg;
struct sockaddr_in addr;
struct timespec ts;
uint8_t controlbuf[64];
int ret;
memset(&txhdr, 0, sizeof(txhdr));
memset(&txiov, 0, sizeof(txiov));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
addr.sin_port = HTONS(PTP_UDP_PORT_EVENT);
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(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 = sendto(state->tx_socket, &req, sizeof(req), 0,
(FAR struct sockaddr *)&addr, sizeof(addr));
/* Get timestamp after send completes.
* TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here.
*/
ptp_gettime(state, &state->delayreq_time);
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(FAR struct ptp_state_s *state)
{
#ifdef CONFIG_NETUTILS_PTPD_SERVER
/* If there is no better master clock on the network,
* act as the reference source and send server packets.
*/
if (!state->selected_source_valid)
{
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(FAR struct ptp_state_s *state,
FAR 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(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: %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(FAR struct ptp_state_s *state,
FAR 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(FAR struct ptp_state_s *state,
FAR 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(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 = sendto(state->tx_socket, &resp, sizeof(resp), 0,
(FAR 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(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 (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;
}
}
/* 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;
}
/* Main PTPD task */
static int ptp_daemon(int argc, FAR char** argv)
{
FAR const char *interface = "eth0";
FAR 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");
ptp_destroy_state(state);
free(state);
return ERROR;
}
ptp_setup_sighandlers(state);
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_process_statusreq(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.
*
* Input Parameters:
* interface - Name of the network interface to bind to, e.g. "eth0"
*
* Returned Value:
* On success, the non-negative task ID of the PTP daemon is returned;
* On failure, a negated errno value is returned.
*
****************************************************************************/
int ptpd_start(FAR const char *interface)
{
int pid;
FAR char *task_argv[] = {
(FAR char *)interface,
NULL
};
pid = task_create("PTPD", CONFIG_NETUTILS_PTPD_SERVERPRIO,
CONFIG_NETUTILS_PTPD_STACKSIZE, ptp_daemon, task_argv);
/* Use kill with signal 0 to check if the process is still alive
* after initialization.
*/
usleep(USEC_PER_TICK);
if (kill(pid, 0) != OK)
{
return ERROR;
}
else
{
return pid;
}
}
/****************************************************************************
* Name: ptpd_status
*
* 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;
}
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;
}
}
Reference in New Issue View Git Blame Copy Permalink