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

ptpd: Implement delay reqs, fix timing bugs

Browse Source 
Fixed bug where negative offsets were not properly handled because
clock_timespec_subtract clamps values to zero.

Implement support for SO_TIMESTAMP to get accurate packet
reception timestamp.

Implemented delay requests for measuring packet transfer delay.

Implemented clock drift estimation to bring the clocks closer to
sync and to filter out measurement jitter.
This commit is contained in:
Petteri Aimonen 2023-11-24 10:03:20 +02:00 committed by Xiang Xiao
parent 8dbf26016a
commit e2168abe9a
3 changed files with 549 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
netutils/ptpd/Kconfig
View File

@ -143,6 +143,24 @@ config NETUTILS_PTPD_ANNOUNCE_INTERVAL_MSEC
---help--- ---help---
How often to transmit announce packets in server mode. How often to transmit announce packets in server mode.
config NETUTILS_PTPD_TWOSTEP_SYNC
bool "PTP server sends two-step synchronization packets"
default y
---help---
If enabled, sends a follow-up packet after every sync packet.
This helps compensate for the time taken to initiate the transmission.
config NETUTILS_PTPD_DELAYRESP_INTERVAL
int "PTP server suggested interval of delay requests"
range 0 255
default 4
---help---
When responding to a delay request, the server can inform the client
how often it should test path delay. This is done using header field
logMessageInterval. The delay will be 2^N seconds.
Default value 4 results in 16 second interval.
endif # NETUTILS_PTPD_SERVER endif # NETUTILS_PTPD_SERVER
if NETUTILS_PTPD_CLIENT if NETUTILS_PTPD_CLIENT
@ -171,6 +189,40 @@ config NETUTILS_PTPD_MULTICAST_TIMEOUT_MS
depending on hardware, after some error recovery events. depending on hardware, after some error recovery events.
Set to 0 to disable. Set to 0 to disable.
config NETUTILS_PTPD_DRIFT_AVERAGE_S
int "PTP client clock drift rate averaging time (s)"
default 600
range 10 86400
---help---
Clock drift rate is averaged over this time pediod. Larger value
gives more stable estimate but reacts slower to crystal oscillator speed
changes (such as caused by temperature changes).
config NETUTILS_PTPD_SEND_DELAYREQ
bool "PTP client enable delay requests"
default n
---help---
If enabled, sends delay request messages to measure the network delay
to server. If disabled, assumes zero delay.
if NETUTILS_PTPD_SEND_DELAYREQ
config NETUTILS_PTPD_MAX_PATH_DELAY_NS
int "PTP client maximum path delay (ns)"
default 100000
range 1 1000000000
---help---
Measured path delay longer than this is ignored. Delay requests are
also not transmitted until clock synchronization is better than this.
config NETUTILS_PTPD_DELAYREQ_AVGCOUNT
int "PTP client path delay averaging count"
default 100
---help---
Measured path delay is averaged over this many samples.
endif # NETUTILS_PTPD_SEND_DELAYREQ
endif # NETUTILS_PTPD_CLIENT endif # NETUTILS_PTPD_CLIENT
endif # NETUTILS_PTPD endif # NETUTILS_PTPD

539
netutils/ptpd/ptpd.c
View File

@ -84,23 +84,45 @@ struct ptp_state_s
uint16_t sync_seq; uint16_t sync_seq;
uint16_t delay_req_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, /* Identity of currently selected clock source,
* from the latest announcement message. * from the latest announcement message.
* *
* The timestamp is used for timeout when a source * The timestamps are used for timeout when a source disappears.
* disappears, it is from the local monotonic clock. * They are from the local CLOCK_MONOTONIC.
*/ */
struct ptp_announce_s selected_source; bool selected_source_valid; /* True if operating as client */
struct timespec last_received_sync; struct ptp_announce_s selected_source; /* Currently selected server */
struct timespec last_received_multicast; 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 sync & announcement packets */ /* Last transmitted packet timestamps (CLOCK_MONOTONIC)
* Used to set transmission interval.
*/
struct timespec last_transmitted_sync; struct timespec last_transmitted_sync;
struct timespec last_transmitted_announce; struct timespec last_transmitted_announce;
struct timespec last_transmitted_delayresp;
struct timespec last_transmitted_delayreq;
/* Latest received packet and its timestamp */ /* 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; struct timespec rxtime;
union union
@ -109,15 +131,16 @@ struct ptp_state_s
struct ptp_announce_s announce; struct ptp_announce_s announce;
struct ptp_sync_s sync; struct ptp_sync_s sync;
struct ptp_follow_up_s follow_up; 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]; uint8_t raw[128];
} rxbuf; } rxbuf;
union uint8_t rxcmsg[CMSG_LEN(sizeof(struct timeval))];
{
uint8_t raw[64];
} rxcmsg;
/* Buffered sync packet for two-step clock setting */ /* 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 ptp_sync_s twostep_packet;
struct timespec twostep_rxtime; struct timespec twostep_rxtime;
@ -223,6 +246,33 @@ static int64_t timespec_to_ms(struct timespec *ts)
return ts->tv_sec * MSEC_PER_SEC + (ts->tv_nsec / NSEC_PER_MSEC); 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 */ /* Check if the currently selected source is still valid */
static bool is_selected_source_valid(struct ptp_state_s *state) static bool is_selected_source_valid(struct ptp_state_s *state)
@ -287,30 +337,51 @@ static int ptp_settime(struct ptp_state_s *state, struct timespec *ts)
return clock_settime(CLOCK_REALTIME, ts); return clock_settime(CLOCK_REALTIME, ts);
} }
/* Smoothly adjust timestamp. /* Smoothly adjust timestamp. */
* TODO: adjtime() limits to microsecond resolution.
*/
static int ptp_adjtime(struct ptp_state_s *state, struct timespec *ts) static int ptp_adjtime(struct ptp_state_s *state, int64_t delta_ns)
{ {
struct timeval delta; struct timeval delta;
UNUSED(state);
TIMESPEC_TO_TIMEVAL(&delta, ts); 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); return adjtime(&delta, NULL);
} }
/* Get timestamp of latest received packet */ /* Get timestamp of latest received packet */
static int ptp_getrxtime(struct ptp_state_s *state, struct timespec *ts) static int ptp_getrxtime(struct ptp_state_s *state, struct msghdr *rxhdr,
struct timespec *ts)
{ {
UNUSED(state); /* Get hardware or kernel timestamp if available */
*ts = state->rxtime;
/* TODO: Implement SO_TIMINGS in NuttX core, and then fetch the #ifdef CONFIG_NET_TIMESTAMP
* timestamp from state->cmsg. struct cmsghdr *cmsg;
*/
return OK; 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 */ /* Initialize PTP client/server state and create sockets */
@ -319,6 +390,7 @@ static int ptp_initialize_state(struct ptp_state_s *state,
const char *interface) const char *interface)
{ {
int ret; int ret;
int arg;
struct ifreq req; struct ifreq req;
struct sockaddr_in bind_addr; struct sockaddr_in bind_addr;
@ -422,6 +494,19 @@ static int ptp_initialize_state(struct ptp_state_s *state,
return ERROR; 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 socket for announcements */
bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO); bind_addr.sin_port = HTONS(PTP_UDP_PORT_INFO);
@ -536,7 +621,7 @@ static int ptp_send_announce(struct ptp_state_s *state)
msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE; msg.header.messagetype = PTP_MSGTYPE_ANNOUNCE;
msg.header.messagelength[1] = sizeof(msg); msg.header.messagelength[1] = sizeof(msg);
ptp_increment_sequence(&state->sync_seq, &msg.header); ptp_increment_sequence(&state->announce_seq, &msg.header);
ptp_gettime(state, &ts); ptp_gettime(state, &ts);
timespec_to_ptp_format(&ts, msg.origintimestamp); timespec_to_ptp_format(&ts, msg.origintimestamp);
@ -579,7 +664,10 @@ static int ptp_send_sync(struct ptp_state_s *state)
msg.header = state->own_identity.header; msg.header = state->own_identity.header;
msg.header.messagetype = PTP_MSGTYPE_SYNC; msg.header.messagetype = PTP_MSGTYPE_SYNC;
msg.header.messagelength[1] = sizeof(msg); msg.header.messagelength[1] = sizeof(msg);
#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC
msg.header.flags[0] = PTP_FLAGS0_TWOSTEP; msg.header.flags[0] = PTP_FLAGS0_TWOSTEP;
#endif
txhdr.msg_name = &addr; txhdr.msg_name = &addr;
txhdr.msg_namelen = sizeof(addr); txhdr.msg_namelen = sizeof(addr);
@ -603,9 +691,10 @@ static int ptp_send_sync(struct ptp_state_s *state)
return ret; return ret;
} }
#ifdef CONFIG_NETUTILS_PTPD_TWOSTEP_SYNC
/* Get timestamp after send completes and send follow-up message /* Get timestamp after send completes and send follow-up message
* *
* TODO: Implement SO_TIMINGS and use the actual tx timestamp here. * TODO: Implement SO_TIMESTAMPING and use the actual tx timestamp here.
*/ */
ptp_gettime(state, &ts); ptp_gettime(state, &ts);
@ -624,10 +713,58 @@ static int ptp_send_sync(struct ptp_state_s *state)
ptpinfo("Sent sync + follow-up, seq %ld\n", ptpinfo("Sent sync + follow-up, seq %ld\n",
(long)ptp_get_sequence(&msg.header)); (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; 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 */ /* Check if we need to send packets */
static int ptp_periodic_send(struct ptp_state_s *state) static int ptp_periodic_send(struct ptp_state_s *state)
@ -637,7 +774,7 @@ static int ptp_periodic_send(struct ptp_state_s *state)
* act as the reference source and send server packets. * act as the reference source and send server packets.
*/ */
if (!is_selected_source_valid(state)) if (!state->selected_source_valid)
{ {
struct timespec time_now; struct timespec time_now;
struct timespec delta; struct timespec delta;
@ -660,6 +797,23 @@ static int ptp_periodic_send(struct ptp_state_s *state)
ptp_send_sync(state); 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 #endif
return OK; return OK;
@ -670,43 +824,74 @@ static int ptp_periodic_send(struct ptp_state_s *state)
static int ptp_process_announce(struct ptp_state_s *state, static int ptp_process_announce(struct ptp_state_s *state,
struct ptp_announce_s *msg) struct ptp_announce_s *msg)
{ {
clock_gettime(CLOCK_MONOTONIC, &state->last_received_announce);
if (is_better_clock(msg, &state->own_identity)) if (is_better_clock(msg, &state->own_identity))
{ {
if (!is_selected_source_valid(state) || if (!state->selected_source_valid ||
is_better_clock(msg, &state->selected_source)) is_better_clock(msg, &state->selected_source))
{ {
ptpinfo("Switching to better PTP time source\n"); ptpinfo("Switching to better PTP time source\n");
state->selected_source = *msg; state->selected_source = *msg;
clock_gettime(CLOCK_MONOTONIC, &state->last_received_sync); 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; return OK;
} }
/* Update local clock by delta, either by smooth adjustment or by jumping. */ /* 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, static int ptp_update_local_clock(struct ptp_state_s *state,
struct timespec *delta) struct timespec *remote_timestamp,
struct timespec *local_timestamp)
{ {
int ret; int ret;
struct timespec local_time; 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;
if (timespec_to_ms(delta) > CONFIG_NETUTILS_PTPD_SETTIME_THRESHOLD_MS) 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)
{ {
/* Add delta to current local time in order to account for any latency /* Large difference, move by jumping.
* between packet reception and clock setting. * Account for delay since packet was received.
*/ */
ptp_gettime(state, &local_time); struct timespec new_time;
clock_timespec_add(&local_time, delta, &local_time); ptp_gettime(state, &new_time);
ret = ptp_settime(state, &local_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) if (ret == OK)
{ {
ptpinfo("Jumped to timestamp %ld.%09ld s\n", ptpinfo("Jumped to timestamp %lld.%09ld s\n",
(long)local_time.tv_sec, (long)local_time.tv_nsec); (long long)new_time.tv_sec, (long)new_time.tv_nsec);
} }
else else
{ {
@ -715,17 +900,116 @@ static int ptp_update_local_clock(struct ptp_state_s *state,
} }
else else
{ {
ret = ptp_adjtime(state, delta); /* Track drift rate based on two consecutive measurements and
* the adjustment that was made previously.
*/
if (ret == OK) 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)
{ {
ptpinfo("Adjusting clock by %ld.%09ld s\n", (long)delta->tv_sec, drift_ppb = (delta_ns - state->last_delta_ns) * MSEC_PER_SEC
(long)delta->tv_nsec); / interval_ms;
} }
else 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); 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; return ret;
@ -737,8 +1021,6 @@ static int ptp_process_sync(struct ptp_state_s *state,
struct ptp_sync_s *msg) struct ptp_sync_s *msg)
{ {
struct timespec remote_time; struct timespec remote_time;
struct timespec local_time;
struct timespec delta;
if (memcmp(msg->header.sourceidentity, if (memcmp(msg->header.sourceidentity,
state->selected_source.header.sourceidentity, state->selected_source.header.sourceidentity,
@ -757,26 +1039,22 @@ static int ptp_process_sync(struct ptp_state_s *state,
{ {
/* We need to wait for a follow-up packet before setting the clock. */ /* We need to wait for a follow-up packet before setting the clock. */
ptp_getrxtime(state, &state->twostep_rxtime); state->twostep_rxtime = state->rxtime;
state->twostep_packet = *msg; state->twostep_packet = *msg;
ptpinfo("Waiting for follow-up\n"); ptpinfo("Waiting for follow-up\n");
return OK; return OK;
} }
/* Calculate delta between local and remote time */ /* Update local clock */
ptp_format_to_timespec(msg->origintimestamp, &remote_time); ptp_format_to_timespec(msg->origintimestamp, &remote_time);
ptp_getrxtime(state, &local_time); return ptp_update_local_clock(state, &remote_time, &state->rxtime);
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, static int ptp_process_followup(struct ptp_state_s *state,
struct ptp_follow_up_s *msg) struct ptp_follow_up_s *msg)
{ {
struct timespec remote_time; struct timespec remote_time;
struct timespec delta;
if (memcmp(msg->header.sourceidentity, if (memcmp(msg->header.sourceidentity,
state->twostep_packet.header.sourceidentity, state->twostep_packet.header.sourceidentity,
@ -795,18 +1073,143 @@ static int ptp_process_followup(struct ptp_state_s *state,
return OK; return OK;
} }
ptp_format_to_timespec(msg->origintimestamp, &remote_time); /* Update local clock based on the remote timestamp we received now
clock_timespec_subtract(&remote_time, &state->twostep_rxtime, &delta); * and the local timestamp of when the sync packet was received.
*/
return ptp_update_local_clock(state, &delta); 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 */ /* Determine received packet type and process it */
static int ptp_process_rx_packet(struct ptp_state_s *state, ssize_t length) 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)) if (length < sizeof(struct ptp_header_s))
{ {
ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n", ptpwarn("Ignoring invalid PTP packet, length only %d bytes\n",
@ -840,6 +1243,18 @@ static int ptp_process_rx_packet(struct ptp_state_s *state, ssize_t length)
ptpinfo("Got follow-up packet, seq %ld\n", ptpinfo("Got follow-up packet, seq %ld\n",
(long)ptp_get_sequence(&state->rxbuf.header)); (long)ptp_get_sequence(&state->rxbuf.header));
return ptp_process_followup(state, &state->rxbuf.follow_up); 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 #endif
default: default:
@ -881,6 +1296,8 @@ static int ptp_daemon(int argc, FAR char** argv)
while (!state->stop) while (!state->stop)
{ {
state->can_send_delayreq = false;
rxhdr.msg_name = NULL; rxhdr.msg_name = NULL;
rxhdr.msg_namelen = 0; rxhdr.msg_namelen = 0;
rxhdr.msg_iov = &rxiov; rxhdr.msg_iov = &rxiov;
@ -894,7 +1311,6 @@ static int ptp_daemon(int argc, FAR char** argv)
pollfds[0].revents = 0; pollfds[0].revents = 0;
pollfds[1].revents = 0; pollfds[1].revents = 0;
ret = poll(pollfds, 2, PTPD_POLL_INTERVAL); ret = poll(pollfds, 2, PTPD_POLL_INTERVAL);
ptp_gettime(state, &state->rxtime);
if (pollfds[0].revents) if (pollfds[0].revents)
{ {
@ -905,6 +1321,7 @@ static int ptp_daemon(int argc, FAR char** argv)
ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT); ret = recvmsg(state->event_socket, &rxhdr, MSG_DONTWAIT);
if (ret > 0) if (ret > 0)
{ {
ptp_getrxtime(state, &rxhdr, &state->rxtime);
ptp_process_rx_packet(state, ret); ptp_process_rx_packet(state, ret);
} }
} }
@ -929,6 +1346,8 @@ static int ptp_daemon(int argc, FAR char** argv)
} }
ptp_periodic_send(state); ptp_periodic_send(state);
state->selected_source_valid = is_selected_source_valid(state);
} }
ptp_destroy_state(state); ptp_destroy_state(state);

18
netutils/ptpd/ptpv2.h
View File

@ -114,4 +114,22 @@ struct ptp_follow_up_s
uint8_t origintimestamp[10]; uint8_t origintimestamp[10];
}; };
/* DelayReq: request delay measurement */
struct ptp_delay_req_s
{
struct ptp_header_s header;
uint8_t origintimestamp[10];
};
/* DelayResp: response to DelayReq */
struct ptp_delay_resp_s
{
struct ptp_header_s header;
uint8_t receivetimestamp[10];
uint8_t reqidentity[8];
uint8_t reqportindex[2];
};
#endif /* __APPS_NETUTILS_PTPD_PTPV2_H */ #endif /* __APPS_NETUTILS_PTPD_PTPV2_H */