sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add basic IEEE-1588 PTP server/client

Browse Source 
What works:
- Basic server & client operation
- Transmission and reception of announce, sync and follow-up

Still missing:
- SO_TIMINGS for getting more precise packet timestamps
- Implementation of delay_req and delay_resp packets
- Status and stop interfaces for the daemon
This commit is contained in:
Petteri Aimonen 2023-09-28 13:46:47 +03:00 committed by Xiang Xiao
parent c340d735ec
commit 5a992d4e18
10 changed files with 1472 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
include/netutils/ptpd.h Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
* apps/include/netutils/ptpd.h
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
#ifndef __APPS_INCLUDE_NETUTILS_PTPD_H
#define __APPS_INCLUDE_NETUTILS_PTPD_H
/****************************************************************************
* Included Files
****************************************************************************/
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Types
****************************************************************************/
/****************************************************************************
* Public Data
****************************************************************************/
#ifdef __cplusplus
#define EXTERN extern "C"
extern "C"
{
#else
#define EXTERN extern
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
/****************************************************************************
* Name: ptpd_start
*
* Description:
* Start the PTP daemon and bind it to specified interface.
*
* Returned Value:
* On success, the non-negative task ID of the PTP daemon is returned;
* On failure, a negated errno value is returned.
*
****************************************************************************/
int ptpd_start(const char *interface);
#undef EXTERN
#ifdef __cplusplus
}
#endif
#endif /* __APPS_INCLUDE_NETUTILS_PTPD_H */

167
netutils/ptpd/Kconfig Normal file
View File

@ -0,0 +1,167 @@
#
# For a description of the syntax of this configuration file,
# see the file kconfig-language.txt in the NuttX tools repository.
#
config NETUTILS_PTPD
bool "PTPD client/server"
default n
depends on NET_IPv4
depends on NET_IGMP
depends on NET_UDP
---help---
Build a minimal implementation of IEEE-1588 precision time protocol.
Uses system gettimeofday() and adjtime() calls to synchronize clock
with a master clock through network, or to provide a master clock to
other systems.
if NETUTILS_PTPD
config NETUTILS_PTPD_DEBUG
bool "Enable PTP debug messages"
default n
depends on DEBUG_INFO
---help---
Enable PTP debug messages even if CONFIG_DEBUG_NET_INFO is not enabled.
config NETUTILS_PTPD_CLIENT
bool "Enable client support"
default y
---help---
Act as a PTP client, synchronizing the NuttX clock to a remote master
clock.
config NETUTILS_PTPD_SERVER
bool "Enable server support"
default n
---help---
Act as a PTP server, providing NuttX clock time to other systems.
Both server and client can be simultaneously enabled. NuttX will then
synchronize to a higher priority master clock, or act as a master
clock itself if it has the highest priority.
Refer to Best Master Clock algorithm in IEEE-1588 for details.
config NETUTILS_PTPD_STACKSIZE
int "PTP daemon stack stack size"
default DEFAULT_TASK_STACKSIZE
config NETUTILS_PTPD_SERVERPRIO
int "PTP daemon priority"
default 100
config NETUTILS_PTPD_DOMAIN
int "PTP domain selection"
default 0
range 0 127
---help---
Set PTP domain to participate in. Default domain is 0, other domains
can be used to isolate reference clocks from each other.
if NETUTILS_PTPD_SERVER
config NETUTILS_PTPD_PRIORITY1
int "PTP server priority1"
default 128
range 0 255
---help---
Set clock priority to announce when acting as a PTP server.
Lower value is higher priority.
A higher priority1 clock will be selected without regard to announced
clock quality fields.
Refer to Best Master Clock algorithm in IEEE-1588 for details.
config NETUTILS_PTPD_PRIORITY2
int "PTP server priority2"
default 128
range 0 255
---help---
Set clock subpriority to announce when acting as a PTP server.
This will distinguish between two clocks that are equivalent in
priority1, class and accuracy values.
Lower value is higher priority.
config NETUTILS_PTPD_CLASS
int "PTP server class"
default 248
range 0 255
---help---
Set master clock class to announce when acting as a PTP server.
Lower value means higher quality clock source.
248 is the default for unknown class.
config NETUTILS_PTPD_ACCURACY
int "PTP server accuracy"
default 254
range 0 255
---help---
Set master clock accuracy to announce when acting as a PTP server.
Logarithmic scale is defined in IEEE-1588:
32: +- 25 ns
33: +- 100 ns
34: +- 250 ns
35: +- 1 us
36: +- 2.5 us
37: +- 10 us
38: +- 25 us
39: +- 100 us
40: +- 250 us
41: +- 1 ms
42: +- 2.5 ms
43: +- 10 ms
44: +- 25 ms
45: +- 100 ms
46: +- 250 ms
47: +- 1 s
48: +- 10 s
49: +- more than 10 s
254: Unknown
config NETUTILS_PTPD_CLOCKSOURCE
int "PTP server clock source type"
default 160
range 0 255
---help---
Set clock source type to announce when acting as a PTP server.
Common values:
32: GPS
64: PTP
80: NTP
144: Other
160: Internal oscillator
config NETUTILS_PTPD_SYNC_INTERVAL_MSEC
int "PTP server sync transmit interval (ms)"
default 1000
---help---
How often to transmit sync packets in server mode.
config NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC
int "PTP server announce transmit interval (ms)"
default 10000
---help---
How often to transmit announce packets in server mode.
endif # NETUTILS_PTPD_SERVER
if NETUTILS_PTPD_CLIENT
config NETUTILS_PTPD_TIMEOUT_MS
int "PTP client timeout for changing clock source (ms)"
default 60000
---help---
If no packets are being received from currently chosen clock source,
fall back to next best clock source after this many seconds.
config NETUTILS_PTPD_SETTIME_THRESHOLD_MS
int "PTP client threshold for changing system time (ms)"
default 1000
---help---
If difference between local and remote clock exceeds this threshold,
time is reset with settimeofday() instead of changing the rate with
adjtime().
endif # NETUTILS_PTPD_CLIENT
endif # NETUTILS_PTPD

23
netutils/ptpd/Make.defs Normal file
View File

@ -0,0 +1,23 @@
############################################################################
# apps/netutils/ptpd/Make.defs
#
# Licensed to the Apache Software Foundation (ASF) under one or more
# contributor license agreements. See the NOTICE file distributed with
# this work for additional information regarding copyright ownership. The
# ASF licenses this file to you under the Apache License, Version 2.0 (the
# "License"); you may not use this file except in compliance with the
# License. You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
# License for the specific language governing permissions and limitations
# under the License.
#
############################################################################
ifneq ($(CONFIG_NETUTILS_PTPD),)
CONFIGURED_APPS += $(APPDIR)/netutils/ptpd
endif

27
netutils/ptpd/Makefile Normal file
View File

@ -0,0 +1,27 @@
############################################################################
# apps/netutils/ptpd/Makefile
#
# Licensed to the Apache Software Foundation (ASF) under one or more
# contributor license agreements. See the NOTICE file distributed with
# this work for additional information regarding copyright ownership. The
# ASF licenses this file to you under the Apache License, Version 2.0 (the
# "License"); you may not use this file except in compliance with the
# License. You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
# License for the specific language governing permissions and limitations
# under the License.
#
############################################################################
include $(APPDIR)/Make.defs
# PTP server/client implementation
CSRCS = ptpd.c
include $(APPDIR)/Application.mk

931
netutils/ptpd/ptpd.c Normal file
View File

@ -0,0 +1,931 @@
/****************************************************************************
* apps/netutils/ptpd/ptpd.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdbool.h>
#include <stdint.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sched.h>
#include <assert.h>
#include <errno.h>
#include <debug.h>
#include <unistd.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netutils/ipmsfilter.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/poll.h>
#include <nuttx/net/netconfig.h>
#include <netutils/ptpd.h>
#include "ptpv2.h"
/****************************************************************************
* Private Data
****************************************************************************/
struct ptp_state_s
{
bool stop;
/* Address of network interface we are operating on */
struct sockaddr_in interface_addr;
/* Socket bound to interface for transmission */
int tx_socket;
/* Sockets for PTP event and information ports */
int event_socket;
int info_socket;
/* Our own identity as a clock source */
struct ptp_announce_s own_identity;
/* Sequence number counters per message type */
uint16_t announce_seq;
uint16_t sync_seq;
uint16_t delay_req_seq;
/* Identity of currently selected clock source,
* from the latest announcement message.
*
* The timestamp is used for timeout when a source
* disappears, it is from the local monotonic clock.
*/
struct ptp_announce_s selected_source;
struct timespec last_received_sync;
/* Last transmitted sync & announcement packets */
struct timespec last_transmitted_sync;
struct timespec last_transmitted_announce;
/* Latest received packet and its timestamp */
struct timespec rxtime;
union
{
struct ptp_header_s header;
struct ptp_announce_s announce;
struct ptp_sync_s sync;
struct ptp_follow_up_s follow_up;
uint8_t raw[128];
} rxbuf;
union
{
uint8_t raw[64];
} rxcmsg;
/* Buffered sync packet for two-step clock setting */
struct ptp_sync_s twostep_packet;
struct timespec twostep_rxtime;
};
#ifdef CONFIG_NETUTILS_PTPD_SERVER
# define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC
#else
# define PTPD_POLL_INTERVAL CONFIG_NETUTILS_PTPD_TIMEOUT_MS
#endif
/* PTP debug messages are enabled by either CONFIG_DEBUG_NET_INFO
* or separately by CONFIG_NETUTILS_PTPD_DEBUG. This simplifies
* debugging without having excessive amount of logging from net.
*/
#ifdef CONFIG_NETUTILS_PTPD_DEBUG
# ifndef CONFIG_DEBUG_NET_INFO
# define ptpinfo _info
# define ptpwarn _warn
# define ptperr _err
# else
# define ptpinfo ninfo
# define ptpwarn nwarn
# define ptperr nerr
# endif
#endif
/****************************************************************************
* Private Functions
****************************************************************************/
/* Convert from timespec to PTP format */
static void timespec_to_ptp_format(struct timespec *ts, uint8_t *timestamp)
{
/* IEEE 1588 uses 48 bits for seconds and 32 bits for nanoseconds,
* both fields big-endian.
*/
#ifdef CONFIG_SYSTEM_TIME64
timestamp[0] = (uint8_t)(ts->tv_sec >> 40);
timestamp[1] = (uint8_t)(ts->tv_sec >> 32);
#else
timestamp[0] = 0;
timestamp[1] = 0;
#endif
timestamp[2] = (uint8_t)(ts->tv_sec >> 24);
timestamp[3] = (uint8_t)(ts->tv_sec >> 16);
timestamp[4] = (uint8_t)(ts->tv_sec >> 8);
timestamp[5] = (uint8_t)(ts->tv_sec >> 0);
timestamp[6] = (uint8_t)(ts->tv_nsec >> 24);
timestamp[7] = (uint8_t)(ts->tv_nsec >> 16);
timestamp[8] = (uint8_t)(ts->tv_nsec >> 8);
timestamp[9] = (uint8_t)(ts->tv_nsec >> 0);
}
/* Convert from PTP format to timespec */
static void ptp_format_to_timespec(uint8_t *timestamp, struct timespec *ts)
{
ts->tv_sec =
(((int64_t)timestamp[0]) << 40)
| (((int64_t)timestamp[1]) << 32)
| (((int64_t)timestamp[2]) << 24)
| (((int64_t)timestamp[3]) << 16)
| (((int64_t)timestamp[4]) << 8)
| (((int64_t)timestamp[5]) << 0);
ts->tv_nsec =
(((long)timestamp[6]) << 24)
| (((long)timestamp[7]) << 16)
| (((long)timestamp[8]) << 8)
| (((long)timestamp[9]) << 0);
}
/* Returns true if A is a better clock source than B.
* Implements Best Master Clock algorithm from IEEE-1588.
*/
static bool is_better_clock(struct ptp_announce_s *a,
struct ptp_announce_s *b)
{
if (a->gm_priority1 < b->gm_priority1 /* Main priority field */
|| a->gm_quality[0] < b->gm_quality[0] /* Clock class */
|| a->gm_quality[1] < b->gm_quality[1] /* Clock accuracy */
|| a->gm_quality[2] < b->gm_quality[2] /* Clock variance high byte */
|| a->gm_quality[3] < b->gm_quality[3] /* Clock variance low byte */
|| a->gm_priority2 < b->gm_priority2 /* Sub priority field */
|| memcmp(a->gm_identity, b->gm_identity, sizeof(a->gm_identity)) < 0)
{
return true;
}
else
{
return false;
}
}
static int64_t timespec_to_ms(struct timespec *ts)
{
return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC);
}
/* Check if the currently selected source is still valid */
static bool is_selected_source_valid(struct ptp_state_s *state)
{
struct timespec time_now;
struct timespec delta;
if ((state->selected_source.header.messagetype & PTP_MSGTYPE_MASK)
!= PTP_MSGTYPE_ANNOUNCE)
{
return false; /* Uninitialized value */
}
/* Note: this uses monotonic clock to track the timeout even when
* system clock is adjusted.
*/
clock_gettime(CLOCK_MONOTONIC, &time_now);
clock_timespec_subtract(&time_now, &state->last_received_sync, &delta);
if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_TIMEOUT_MS)
{
return false; /* Too long time since received packet */
}
return true;
}
/* Increment sequence number for packet type, and copy to header */
static void ptp_increment_sequence(uint16_t *sequence_num,
struct ptp_header_s *hdr)
{
*sequence_num += 1;
hdr->sequenceid[0] = (uint8_t)(*sequence_num >> 8);
hdr->sequenceid[1] = (uint8_t)(*sequence_num);
}
/* Get sequence number from received packet */
static uint16_t ptp_get_sequence(struct ptp_header_s *hdr)
{
return ((uint16_t)hdr->sequenceid[0] << 8) | hdr->sequenceid[1];
}
/* Get current system timestamp as a timespec
* TODO: Possibly add support for selecting different clock or using
* architecture-specific interface for clock access.
*/
static int ptp_gettime(struct ptp_state_s *state, struct timespec *ts)
{
UNUSED(state);
return clock_gettime(CLOCK_REALTIME, ts);
}
/* Change current system timestamp by jumping */
static int ptp_settime(struct ptp_state_s *state, struct timespec *ts)
{
UNUSED(state);
return clock_settime(CLOCK_REALTIME, ts);
}
/* Smoothly adjust timestamp.
* TODO: adjtime() limits to microsecond resolution.
*/
static int ptp_adjtime(struct ptp_state_s *state, struct timespec *ts)
{
struct timeval delta;
UNUSED(state);
TIMESPEC_TO_TIMEVAL(&delta, ts);
return adjtime(&delta, NULL);
}
/* Get timestamp of latest received packet */
static int ptp_getrxtime(struct ptp_state_s *state, struct timespec *ts)
{
UNUSED(state);
*ts = state->rxtime;
/* TODO: Implement SO_TIMINGS in NuttX core, and then fetch the
* timestamp from state->cmsg.
*/
return OK;
}
/* Initialize PTP client/server state and create sockets */
static int ptp_initialize_state(struct ptp_state_s *state,
const char *interface)
{
int ret;
struct ifreq req;
struct sockaddr_in bind_addr;
/* Create sockets */
state->tx_socket = socket(AF_INET, SOCK_DGRAM, 0);
if (state->tx_socket < 0)
{
ptperr("Failed to create tx socket: %d\n", errno);
return ERROR;
}
state->event_socket = socket(AF_INET, SOCK_DGRAM, 0);
if (state->event_socket < 0)
{
ptperr("Failed to create event socket: %d\n", errno);
return ERROR;
}
state->info_socket = socket(AF_INET, SOCK_DGRAM, 0);
if (state->info_socket < 0)
{
ptperr("Failed to create info socket: %d\n", errno);
return ERROR;
}
/* Get address information of the specified interface for binding socket
* Only supports IPv4 currently.
*/
memset(&req, 0, sizeof(req));
strncpy(req.ifr_name, interface, sizeof(req.ifr_name));
if (ioctl(state->event_socket, SIOCGIFADDR, (unsigned long)&req) < 0)
{
ptperr("Failed to get IP address information for interface %s\n",
interface);
return ERROR;
}
state->interface_addr = *(struct sockaddr_in *)&req.ifr_ifru.ifru_addr;
/* Get hardware address to initialize the identity field in header.
* Clock identity is EUI-64, which we make from EUI-48.
*/
if (ioctl(state->event_socket, SIOCGIFHWADDR, (unsigned long)&req) < 0)
{
ptperr("Failed to get HW address information for interface %s\n",
interface);
return ERROR;
}
state->own_identity.header.version = 2;
state->own_identity.header.domain = CONFIG_NETUTILS_PTPD_DOMAIN;
state->own_identity.header.sourceidentity[0] = req.ifr_hwaddr.sa_data[0];
state->own_identity.header.sourceidentity[1] = req.ifr_hwaddr.sa_data[1];
state->own_identity.header.sourceidentity[2] = req.ifr_hwaddr.sa_data[2];
state->own_identity.header.sourceidentity[3] = 0xff;
state->own_identity.header.sourceidentity[4] = 0xfe;
state->own_identity.header.sourceidentity[5] = req.ifr_hwaddr.sa_data[3];
state->own_identity.header.sourceidentity[6] = req.ifr_hwaddr.sa_data[4];
state->own_identity.header.sourceidentity[7] = req.ifr_hwaddr.sa_data[5];
state->own_identity.header.sourceportindex[0] = 0;
state->own_identity.header.sourceportindex[1] = 1;
state->own_identity.gm_priority1 = CONFIG_NETUTILS_PTPD_PRIORITY1;
state->own_identity.gm_quality[0] = CONFIG_NETUTILS_PTPD_CLASS;
state->own_identity.gm_quality[1] = CONFIG_NETUTILS_PTPD_ACCURACY;
state->own_identity.gm_quality[2] = 0xff; /* No variance estimate */
state->own_identity.gm_quality[3] = 0xff;
state->own_identity.gm_priority2 = CONFIG_NETUTILS_PTPD_PRIORITY2;
memcpy(state->own_identity.gm_identity,
state->own_identity.header.sourceidentity,
sizeof(state->own_identity.gm_identity));
state->own_identity.timesource = CONFIG_NETUTILS_PTPD_CLOCKSOURCE;
/* Subscribe to PTP multicast address */
bind_addr.sin_family = AF_INET;
bind_addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
ret = ipmsfilter(&state->interface_addr.sin_addr,
&bind_addr.sin_addr,
MCAST_INCLUDE);
if (ret < 0)
{
ptperr("Failed to bind multicast address: %d\n", errno);
return ERROR;
}
/* Bind socket for events */
bind_addr.sin_port = HTONS(PTP_UDP_PORT_EVENT);
ret = bind(state->event_socket, (struct sockaddr *)&bind_addr,
sizeof(bind_addr));
if (ret < 0)
{
ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port);
return ERROR;
}
/* Bind socket for announcements */
bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
ret = bind(state->info_socket, (struct sockaddr *)&bind_addr,
sizeof(bind_addr));
if (ret < 0)
{
ptperr("Failed to bind to udp port %d\n", bind_addr.sin_port);
return ERROR;
}
/* Bind TX socket to interface address (local addr cannot be multicast) */
bind_addr.sin_addr = state->interface_addr.sin_addr;
ret = bind(state->tx_socket, (struct sockaddr *)&bind_addr,
sizeof(bind_addr));
if (ret < 0)
{
ptperr("Failed to bind tx to port %d\n", bind_addr.sin_port);
return ERROR;
}
return OK;
}
/* Unsubscribe multicast and destroy sockets */
static int ptp_destroy_state(struct ptp_state_s *state)
{
struct in_addr mcast_addr;
mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
ipmsfilter(&state->interface_addr.sin_addr,
&mcast_addr,
MCAST_EXCLUDE);
if (state->tx_socket > 0)
{
close(state->tx_socket);
state->tx_socket = -1;
}
if (state->event_socket > 0)
{
close(state->event_socket);
state->event_socket = -1;
}
if (state->info_socket > 0)
{
close(state->info_socket);
state->info_socket = -1;
}
return OK;
}
/* Send PTP server announcement packet */
static int ptp_send_announce(struct ptp_state_s *state)
{
struct ptp_announce_s msg;
struct sockaddr_in addr;
struct timespec ts;
int ret;
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
memset(&msg, 0, sizeof(msg));
msg = state->own_identity;
msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE;
msg.header.messagelength[1] = sizeof(msg);
ptp_increment_sequence(&state->sync_seq, &msg.header);
ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp);
ret = sendto(state->tx_socket, &msg, sizeof(msg), 0,
(struct sockaddr *)&addr, sizeof(addr));
if (ret < 0)
{
ptperr("sendto failed: %d", errno);
}
else
{
ptpinfo("Sent announce, seq %ld\n",
(long)ptp_get_sequence(&msg.header));
}
return ret;
}
/* Send PTP server synchronization packet */
static int ptp_send_sync(struct ptp_state_s *state)
{
struct msghdr txhdr;
struct iovec txiov;
struct ptp_sync_s msg;
struct sockaddr_in addr;
struct timespec ts;
uint8_t controlbuf[64];
int ret;
memset(&txhdr, 0, sizeof(txhdr));
memset(&txiov, 0, sizeof(txiov));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
addr.sin_port = HTONS(PTP_UDP_PORT_EVENT);
memset(&msg, 0, sizeof(msg));
msg.header = state->own_identity.header;
msg.header.messagetype = PTP_MSGTYPE_SYNC;
msg.header.messagelength[1] = sizeof(msg);
msg.header.flags[0] = PTP_FLAGS0_TWOSTEP;
txhdr.msg_name = &addr;
txhdr.msg_namelen = sizeof(addr);
txhdr.msg_iov = &txiov;
txhdr.msg_iovlen = 1;
txhdr.msg_control = controlbuf;
txhdr.msg_controllen = sizeof(controlbuf);
txiov.iov_base = &msg;
txiov.iov_len = sizeof(msg);
/* Timestamp and send the sync message */
ptp_increment_sequence(&state->sync_seq, &msg.header);
ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp);
ret = sendmsg(state->tx_socket, &txhdr, 0);
if (ret < 0)
{
ptperr("sendmsg for sync message failed: %d\n", errno);
return ret;
}
/* Get timestamp after send completes and send follow-up message
*
* TODO: Implement SO_TIMINGS and use the actual tx timestamp here.
*/
ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp);
msg.header.messagetype = PTP_MSGTYPE_FOLLOW_UP;
msg.header.flags[0] = 0;
addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
ret = sendto(state->tx_socket, &msg, sizeof(msg), 0,
(struct sockaddr *)&addr, sizeof(addr));
if (ret < 0)
{
ptperr("sendto for follow-up message failed: %d\n", errno);
return ret;
}
ptpinfo("Sent sync + follow-up, seq %ld\n",
(long)ptp_get_sequence(&msg.header));
return OK;
}
/* Check if we need to send packets */
static int ptp_periodic_send(struct ptp_state_s *state)
{
#ifdef CONFIG_NETUTILS_PTPD_SERVER
/* If there is no better master clock on the network,
* act as the reference source and send server packets.
*/
if (!is_selected_source_valid(state))
{
struct timespec time_now;
struct timespec delta;
clock_gettime(CLOCK_MONOTONIC, &time_now);
clock_timespec_subtract(&time_now,
&state->last_transmitted_announce, &delta);
if (timespec_to_ms(&delta)
> CONFIG_NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC)
{
state->last_transmitted_announce = time_now;
ptp_send_announce(state);
}
clock_timespec_subtract(&time_now,
&state->last_transmitted_sync, &delta);
if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_SYNC_INTERVAL_MSEC)
{
state->last_transmitted_sync = time_now;
ptp_send_sync(state);
}
}
#endif
return OK;
}
/* Process received PTP announcement */
static int ptp_process_announce(struct ptp_state_s *state,
struct ptp_announce_s *msg)
{
if (is_better_clock(msg, &state->own_identity))
{
if (!is_selected_source_valid(state) ||
is_better_clock(msg, &state->selected_source))
{
ptpinfo("Switching to better PTP time source\n");
state->selected_source = *msg;
clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync);
}
}
return OK;
}
/* Update local clock by delta, either by smooth adjustment or by jumping. */
static int ptp_update_local_clock(struct ptp_state_s *state,
struct timespec *delta)
{
int ret;
struct timespec local_time;
if (timespec_to_ms(delta) > CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS)
{
/* Add delta to current local time in order to account for any latency
* between packet reception and clock setting.
*/
ptp_gettime(state, &local_time);
clock_timespec_add(&local_time, delta, &local_time);
ret = ptp_settime(state, &local_time);
if (ret == OK)
{
ptpinfo("Jumped to timestamp %ld.%09ld s\n",
(long)local_time.tv_sec, (long)local_time.tv_nsec);
}
else
{
ptperr("ptp_settime() failed: %d\n", errno);
}
}
else
{
ret = ptp_adjtime(state, delta);
if (ret == OK)
{
ptpinfo("Adjusting clock by %ld.%09ld s\n", (long)delta->tv_sec,
(long)delta->tv_nsec);
}
else
{
ptperr("ptp_adjtime() failed: %d\n", errno);
}
}
return ret;
}
/* Process received PTP sync packet */
static int ptp_process_sync(struct ptp_state_s *state,
struct ptp_sync_s *msg)
{
struct timespec remote_time;
struct timespec local_time;
struct timespec delta;
if (memcmp(msg->header.sourceidentity,
state->selected_source.header.sourceidentity,
sizeof(msg->header.sourceidentity)) != 0)
{
/* This packet wasn't from the currently selected source */
return OK;
}
/* Update timeout tracking */
clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync);
if (msg->header.flags[0] & PTP_FLAGS0_TWOSTEP)
{
/* We need to wait for a follow-up packet before setting the clock. */
ptp_getrxtime(state, &state->twostep_rxtime);
state->twostep_packet = *msg;
ptpinfo("Waiting for follow-up\n");
return OK;
}
/* Calculate delta between local and remote time */
ptp_format_to_timespec(msg->origintimestamp, &remote_time);
ptp_getrxtime(state, &local_time);
clock_timespec_subtract(&remote_time, &local_time, &delta);
return ptp_update_local_clock(state, &delta);
}
static int ptp_process_followup(struct ptp_state_s *state,
struct ptp_follow_up_s *msg)
{
struct timespec remote_time;
struct timespec delta;
if (memcmp(msg->header.sourceidentity,
state->twostep_packet.header.sourceidentity,
sizeof(msg->header.sourceidentity)) != 0)
{
return OK; /* This packet wasn't from the currently selected source */
}
if (ptp_get_sequence(&msg->header)
!= ptp_get_sequence(&state->twostep_packet.header))
{
ptpwarn("PTP follow-up packet sequence %ld does not match initial "
"sync packet sequence %ld, ignoring\n",
(long)ptp_get_sequence(&msg->header),
(long)ptp_get_sequence(&state->twostep_packet.header));
return OK;
}
ptp_format_to_timespec(msg->origintimestamp, &remote_time);
clock_timespec_subtract(&remote_time, &state->twostep_rxtime, &delta);
return ptp_update_local_clock(state, &delta);
}
/* Determine received packet type and process it */
static int ptp_process_rx_packet(struct ptp_state_s *state, ssize_t length)
{
ptpwarn("Got packet: %d bytes\n", length);
if (length < sizeof(struct ptp_header_s))
{
ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n",
(int)length);
return OK;
}
if (state->rxbuf.header.domain != CONFIG_NETUTILS_PTPD_DOMAIN)
{
/* Part of different clock domain, ignore */
return OK;
}
switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK)
{
#ifdef CONFIG_NETUTILS_PTPD_CLIENT
case PTP_MSGTYPE_ANNOUNCE:
ptpinfo("Got announce packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_announce(state, &state->rxbuf.announce);
case PTP_MSGTYPE_SYNC:
ptpinfo("Got sync packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_sync(state, &state->rxbuf.sync);
case PTP_MSGTYPE_FOLLOW_UP:
ptpinfo("Got follow-up packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_followup(state, &state->rxbuf.follow_up);
#endif
default:
ptpinfo("Ignoring unknown PTP packet type: 0x%02x\n",
state->rxbuf.header.messagetype);
return OK;
}
}
static int ptp_daemon(int argc, FAR char** argv)
{
const char *interface = "eth0";
struct ptp_state_s *state;
struct pollfd pollfds[2];
struct msghdr rxhdr;
struct iovec rxiov;
int ret;
memset(&rxhdr, 0, sizeof(rxhdr));
memset(&rxiov, 0, sizeof(rxiov));
state = calloc(1, sizeof(struct ptp_state_s));
if (argc > 1)
{
interface = argv[1];
}
if (ptp_initialize_state(state, interface) != OK)
{
ptperr("Failed to initialize PTP state, exiting\n");
return ERROR;
}
pollfds[0].events = POLLIN;
pollfds[0].fd = state->event_socket;
pollfds[1].events = POLLIN;
pollfds[1].fd = state->info_socket;
while (!state->stop)
{
rxhdr.msg_name = NULL;
rxhdr.msg_namelen = 0;
rxhdr.msg_iov = &rxiov;
rxhdr.msg_iovlen = 1;
rxhdr.msg_control = &state->rxcmsg;
rxhdr.msg_controllen = sizeof(state->rxcmsg);
rxhdr.msg_flags = 0;
rxiov.iov_base = &state->rxbuf;
rxiov.iov_len = sizeof(state->rxbuf);
pollfds[0].revents = 0;
pollfds[1].revents = 0;
ret = poll(pollfds, 2, PTPD_POLL_INTERVAL);
ptp_gettime(state, &state->rxtime);
if (pollfds[0].revents)
{
/* Receive time-critical packet, potentially with cmsg
* indicating the timestamp.
*/
ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT);
if (ret > 0)
{
ptp_process_rx_packet(state, ret);
}
}
if (pollfds[1].revents)
{
/* Receive non-time-critical packet. */
ret = recv(state->info_socket, &state->rxbuf, sizeof(state->rxbuf),
MSG_DONTWAIT);
if (ret > 0)
{
ptp_process_rx_packet(state, ret);
}
}
ptp_periodic_send(state);
}
ptp_destroy_state(state);
free(state);
return 0;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: ptpd_start
*
* Description:
* Start the PTP daemon and bind it to specified interface.
*
* Returned Value:
* On success, the non-negative task ID of the PTP daemon is returned;
* On failure, a negated errno value is returned.
*
****************************************************************************/
int ptpd_start(const char *interface)
{
int pid;
FAR char *task_argv[] = {
(FAR char *)interface,
NULL
};
pid = task_create("PTPD", CONFIG_NETUTILS_PTPD_SERVERPRIO,
CONFIG_NETUTILS_PTPD_STACKSIZE, ptp_daemon, task_argv);
/* Use kill with signal 0 to check if the process is still alive
* after initialization.
*/
usleep(USEC_PER_TICK);
if (kill(pid, 0) != OK)
{
return -1;
}
else
{
return pid;
}
}
/* TODO: Implement status and stop interfaces */

117
netutils/ptpd/ptpv2.h Normal file
View File

@ -0,0 +1,117 @@
/****************************************************************************
* apps/netutils/ptpd/ptpv2.h
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
#ifndef __APPS_NETUTILS_PTPD_PTPV2_H
#define __APPS_NETUTILS_PTPD_PTPV2_H
/****************************************************************************
* Included Files
****************************************************************************/
#include <stdint.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Time-critical messages (id < 8) are sent to port 319,
* other messages to port 320.
*/
#define PTP_UDP_PORT_EVENT 319
#define PTP_UDP_PORT_INFO 320
/* Multicast address to send to: 224.0.1.129 */
#define PTP_MULTICAST_ADDR ((in_addr_t)0xE0000181)
/* Message types */
#define PTP_MSGTYPE_MASK 0x0F
#define PTP_MSGTYPE_SYNC 0
#define PTP_MSGTYPE_DELAY_REQ 1
#define PTP_MSGTYPE_FOLLOW_UP 8
#define PTP_MSGTYPE_DELAY_RESP 9
#define PTP_MSGTYPE_ANNOUNCE 11
/* Message flags */
#define PTP_FLAGS0_TWOSTEP (1 << 1)
/****************************************************************************
* Public Types
****************************************************************************/
/* Defined in IEEE 1588-2008 Precision Time Protocol
* All multi-byte fields are big-endian.
*/
/* Common header for all message types */
struct ptp_header_s
{
uint8_t messagetype;
uint8_t version;
uint8_t messagelength[2];
uint8_t domain;
uint8_t reserved1;
uint8_t flags[2];
uint8_t correction[8];
uint8_t reserved2[4];
uint8_t sourceidentity[8];
uint8_t sourceportindex[2];
uint8_t sequenceid[2];
uint8_t controlfield;
uint8_t logmessageinterval;
};
/* Announce a master clock */
struct ptp_announce_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
uint8_t utcoffset[2];
uint8_t reserved;
uint8_t gm_priority1;
uint8_t gm_quality[4];
uint8_t gm_priority2;
uint8_t gm_identity[8];
uint8_t stepsremoved[2];
uint8_t timesource;
};
/* Sync: transmit timestamp from master clock */
struct ptp_sync_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
};
/* FollowUp: actual timestamp of when sync message was sent */
struct ptp_follow_up_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
};
#endif /* __APPS_NETUTILS_PTPD_PTPV2_H */

24
system/ptpd/Kconfig Normal file
View File

@ -0,0 +1,24 @@
#
# For a description of the syntax of this configuration file,
# see the file kconfig-language.txt in the NuttX tools repository.
#
config SYSTEM_PTPD
tristate "PTP daemon commands"
default n
select NETUTILS_PTPD
---help---
Enable 'ptpd' command to start the PTP daemon
if SYSTEM_PTPD
config SYSTEM_PTPD_PRIORITY
int "PTPD utility command task priority"
default 100
config SYSTEM_PTPD_STACKSIZE
int "PTPD utility command stack size"
default DEFAULT_TASK_STACKSIZE
endif # SYSTEM_PTPD

23
system/ptpd/Make.defs Normal file
View File

@ -0,0 +1,23 @@
############################################################################
# apps/system/ptpd/Make.defs
#
# Licensed to the Apache Software Foundation (ASF) under one or more
# contributor license agreements. See the NOTICE file distributed with
# this work for additional information regarding copyright ownership. The
# ASF licenses this file to you under the Apache License, Version 2.0 (the
# "License"); you may not use this file except in compliance with the
# License. You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
# License for the specific language governing permissions and limitations
# under the License.
#
############################################################################
ifneq ($(CONFIG_SYSTEM_PTPD),)
CONFIGURED_APPS += $(APPDIR)/system/ptpd
endif

30
system/ptpd/Makefile Normal file
View File

@ -0,0 +1,30 @@
############################################################################
# apps/system/ptpd/Makefile
#
# Licensed to the Apache Software Foundation (ASF) under one or more
# contributor license agreements. See the NOTICE file distributed with
# this work for additional information regarding copyright ownership. The
# ASF licenses this file to you under the Apache License, Version 2.0 (the
# "License"); you may not use this file except in compliance with the
# License. You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
# License for the specific language governing permissions and limitations
# under the License.
#
############################################################################
include $(APPDIR)/Make.defs
PROGNAME = ptpd
PRIORITY = $(CONFIG_SYSTEM_PTPD_PRIORITY)
STACKSIZE = $(CONFIG_SYSTEM_PTPD_STACKSIZE)
MODULE = $(CONFIG_SYSTEM_PTPD)
MAINSRC = ptpd_main.c
include $(APPDIR)/Application.mk

59
system/ptpd/ptpd_main.c Normal file
View File

@ -0,0 +1,59 @@
/****************************************************************************
* apps/system/ptpd/ptpd_main.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdlib.h>
#include <stdio.h>
#include "netutils/ptpd.h"
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* ptpd_main
****************************************************************************/
int main(int argc, FAR char *argv[])
{
int pid;
if (argc != 2)
{
fprintf(stderr, "Usage: ptpd <interface>\n");
return 1;
}
pid = ptpd_start(argv[1]);
if (pid < 0)
{
fprintf(stderr, "ERROR: ptpd_start() failed\n");
return EXIT_FAILURE;
}
printf("Started the PTP daemon as PID=%d\n", pid);
return EXIT_SUCCESS;
}