Thread: [Linuxptp-devel] [PATCH v2 2/2] ts2phc: Add PTP as a source of ToD

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Add PPS ToD source that extracts ToD from  master timestamps from
a slave event monitor of the running ptp4l instance.

It enables scenarios where reliable ToD sources, such as nmea,
may not be available, but a system is connected to different ptp
masters.

It can be used in the APTS deployment with the PPS from the local
GNSS receiver that doesn't supply the ToD information, but PTP
service is enabled in the backhaul.

To use the servo
ts2phc -s ptp -c eth0 -m --slave_event_monitor /var/run/ts2phc-mon
And run the ptp4l with slave event monitor enabled
ptp4l -i eth0 -m --free_running 1 --slave_event_monitor /var/run/ts2phc-mon

Note: Do not enable ptp4l control of the clock on the same port.

Signed-off-by: Maciek Machnikowski <ma...@ma...>
---
 makefile                |   3 +-
 ts2phc.8                |   6 ++
 ts2phc.c                |   2 +
 ts2phc_pps_source.c     |   4 +
 ts2phc_pps_source.h     |   1 +
 ts2phc_ptp_pps_source.c | 200 ++++++++++++++++++++++++++++++++++++++++
 ts2phc_ptp_pps_source.h |  15 +++
 7 files changed, 230 insertions(+), 1 deletion(-)
 create mode 100644 ts2phc_ptp_pps_source.c
 create mode 100644 ts2phc_ptp_pps_source.h

diff --git a/makefile b/makefile
index 3e3b8b3..e15c22d 100644
--- a/makefile
+++ b/makefile
@@ -27,7 +27,8 @@ FILTERS	= filter.o mave.o mmedian.o
 SERVOS	= linreg.o ntpshm.o nullf.o pi.o refclock_sock.o servo.o
 TRANSP	= raw.o transport.o udp.o udp6.o uds.o
 TS2PHC	= ts2phc.o lstab.o nmea.o serial.o sock.o ts2phc_generic_pps_source.o \
- ts2phc_nmea_pps_source.o ts2phc_phc_pps_source.o ts2phc_pps_sink.o ts2phc_pps_source.o
+ ts2phc_nmea_pps_source.o ts2phc_phc_pps_source.o ts2phc_pps_sink.o ts2phc_pps_source.o \
+ ts2phc_ptp_pps_source.o
 OBJ	= bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
  e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o monitor.o msg.o phc.o \
  port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) \
diff --git a/ts2phc.8 b/ts2phc.8
index 3c71d47..f3abb32 100644
--- a/ts2phc.8
+++ b/ts2phc.8
@@ -72,6 +72,8 @@ device (like /dev/ptp0) or its associated network interface (like
 eth0).
 Use the key word "nmea" for an external 1-PPS from a GPS providing ToD
 information via the RMC NMEA sentence.
+Use the key word "ptp" for the ToD source from the running ptp4l instance pushing
+timestamps through the slave event monitor.
 .TP
 .B \-v
 Prints the software version and exits.
@@ -259,6 +261,10 @@ Some PHC devices feature programmable pins, and this option allows
 configuration of a particular pin for the external time stamping or
 periodic output function.
 The default is pin index 0.
+.TP
+.B slave_event_monitor
+Used with ptp ToD source, specifies the UNIX domain socket to read
+SLAVE_RX_SYNC_TIMING_DATA from.
 
 .SH WARNING
 
diff --git a/ts2phc.c b/ts2phc.c
index 4393059..b998c7d 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -733,6 +733,8 @@ int main(int argc, char *argv[])
 		pps_type = TS2PHC_PPS_SOURCE_GENERIC;
 	} else if (!strcasecmp(tod_source, "nmea")) {
 		pps_type = TS2PHC_PPS_SOURCE_NMEA;
+	} else if (!strcasecmp(tod_source, "ptp")) {
+		pps_type = TS2PHC_PPS_SOURCE_PTP;
 	} else {
 		pps_type = TS2PHC_PPS_SOURCE_PHC;
 	}
diff --git a/ts2phc_pps_source.c b/ts2phc_pps_source.c
index c333f65..87b0ffa 100644
--- a/ts2phc_pps_source.c
+++ b/ts2phc_pps_source.c
@@ -8,6 +8,7 @@
 #include "ts2phc_nmea_pps_source.h"
 #include "ts2phc_phc_pps_source.h"
 #include "ts2phc_pps_source_private.h"
+#include "ts2phc_ptp_pps_source.h"
 
 struct ts2phc_pps_source *ts2phc_pps_source_create(struct ts2phc_private *priv,
 						   const char *dev,
@@ -25,6 +26,9 @@ struct ts2phc_pps_source *ts2phc_pps_source_create(struct ts2phc_private *priv,
 	case TS2PHC_PPS_SOURCE_PHC:
 		src = ts2phc_phc_pps_source_create(priv, dev);
 		break;
+	case TS2PHC_PPS_SOURCE_PTP:
+		src = ts2phc_ptp_pps_source_create(priv, dev);
+		break;
 	}
 	return src;
 }
diff --git a/ts2phc_pps_source.h b/ts2phc_pps_source.h
index 293c693..3a66219 100644
--- a/ts2phc_pps_source.h
+++ b/ts2phc_pps_source.h
@@ -23,6 +23,7 @@ enum ts2phc_pps_source_type {
 	TS2PHC_PPS_SOURCE_GENERIC,
 	TS2PHC_PPS_SOURCE_NMEA,
 	TS2PHC_PPS_SOURCE_PHC,
+	TS2PHC_PPS_SOURCE_PTP,
 };
 
 /**
diff --git a/ts2phc_ptp_pps_source.c b/ts2phc_ptp_pps_source.c
new file mode 100644
index 0000000..cc385ce
--- /dev/null
+++ b/ts2phc_ptp_pps_source.c
@@ -0,0 +1,200 @@
+/**
+ * @file ts2phc_ptp_pps_source.c
+ * @note Copyright (C) 2023 Maciek Machnikowski <ma...@ma...>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#include <pthread.h>
+#include <stdlib.h>
+#include <time.h>
+
+#include "missing.h"
+#include "pmc_agent.h"
+#include "print.h"
+#include "ts2phc_pps_source_private.h"
+#include "ts2phc_ptp_pps_source.h"
+#include "util.h"
+
+
+#define TIMESTAMP_SEC(ts) (((uint64_t)ts.seconds_lsb) | (((uint64_t)ts.seconds_msb) << 32))
+#define MAX_PTP_AGE	5000000000ULL
+
+struct ts2phc_ptp_pps_source {
+	struct ts2phc_pps_source pps_source;
+	struct ts2phc_private *priv;
+	struct pmc_agent *agent;
+
+	pthread_t worker;
+	/* Protects anonymous struct fields, below, from concurrent access. */
+	pthread_mutex_t mutex;
+	struct {
+		struct timespec local_monotime;
+		struct timespec ptp_time;
+		bool ptp_time_valid;
+	};
+};
+
+static void ts2phc_ptp_pps_source_destroy(struct ts2phc_pps_source *src)
+{
+	struct ts2phc_ptp_pps_source *s =
+		container_of(src, struct ts2phc_ptp_pps_source, pps_source);
+
+	pthread_join(s->worker, NULL);
+	pthread_mutex_destroy(&s->mutex);
+
+	pmc_agent_destroy(s->agent);
+
+	free(s);
+}
+
+static int ts2phc_ptp_pps_source_getppstime(struct ts2phc_pps_source *src,
+						struct timespec *ts)
+{
+	struct ts2phc_ptp_pps_source *s =
+		container_of(src, struct ts2phc_ptp_pps_source, pps_source);
+	tmv_t duration_since_ptp, local_t1, local_t2, sync_time;
+	struct timespec now;
+	bool data_valid;
+
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	local_t2 = timespec_to_tmv(now);
+
+	pthread_mutex_lock(&s->mutex);
+
+	local_t1 = timespec_to_tmv(s->local_monotime);
+	sync_time = timespec_to_tmv(s->ptp_time);
+	data_valid = s->ptp_time_valid;
+
+	pthread_mutex_unlock(&s->mutex);
+
+	if (!data_valid) {
+		pr_debug("ptp: no valid PTP timestamp received");
+		return -1;
+	}
+
+	duration_since_ptp = tmv_sub(local_t2, local_t1);
+	if (tmv_to_nanoseconds(duration_since_ptp) > MAX_PTP_AGE) {
+		/* Invalidate the timestamp */
+		pthread_mutex_lock(&s->mutex);
+		s->ptp_time_valid = false;
+		pthread_mutex_unlock(&s->mutex);
+
+		pr_err("ptp: time stamp stale");
+		return -1;
+	}
+	sync_time = tmv_add(sync_time, duration_since_ptp);
+
+	*ts = tmv_to_timespec(sync_time);
+
+	return 0;
+}
+
+static void *monitor_ptp_signaling_msg(void *arg)
+{
+	struct ts2phc_ptp_pps_source *s = arg;
+
+	while (is_running()) {
+		sleep(0.01);
+		pmc_agent_update(s->agent);
+	}
+
+	return NULL;
+}
+
+static int ts2phc_recv_signaling_subscribed(void *context,
+					    struct ptp_message *msg,
+					    int excluded)
+{
+	struct slave_rx_sync_timing_record *sync_record;
+	struct slave_rx_sync_timing_data_tlv *srstd;
+	struct ts2phc_ptp_pps_source *s = context;
+	struct tlv_extra *extra;
+	struct timespec rxtime;
+	int i, cnt;
+
+	TAILQ_FOREACH(extra, &msg->tlv_list, list) {
+		if (extra->tlv->type != TLV_SLAVE_RX_SYNC_TIMING_DATA) {
+			continue;
+		}
+
+		/* Read the time from the Sync/FollowUp message */
+		srstd = (struct slave_rx_sync_timing_data_tlv *)extra->tlv;
+		cnt = (srstd->length - sizeof(srstd->sourcePortIdentity)) /
+			sizeof(*sync_record);
+		sync_record = srstd->record;
+
+		for (i = 0; i < cnt; i++) {
+			clock_gettime(CLOCK_MONOTONIC, &rxtime);
+
+			pthread_mutex_lock(&s->mutex);
+
+			s->local_monotime = rxtime;
+			s->ptp_time.tv_sec = TIMESTAMP_SEC(sync_record->syncOriginTimestamp);
+			s->ptp_time.tv_nsec = sync_record->syncOriginTimestamp.nanoseconds;
+			s->ptp_time_valid = true;
+
+			pthread_mutex_unlock(&s->mutex);
+
+			pr_debug("%ld.%ld Seq: %d\n", s->ptp_time.tv_sec,
+				 s->ptp_time.tv_nsec, sync_record->sequenceId);
+
+			sync_record++;
+		}
+	}
+
+	return 1;
+}
+
+struct ts2phc_pps_source *ts2phc_ptp_pps_source_create(struct ts2phc_private *priv,
+							   const char *dev)
+{
+	char uds_local[MAX_IFNAME_SIZE + 1];
+	struct ts2phc_ptp_pps_source *s;
+	const char *path;
+	int err;
+
+	s = calloc(1, sizeof(*s));
+	if (!s) {
+		return NULL;
+	}
+	path = config_get_string(priv->cfg, NULL, "slave_event_monitor");
+	if (!path) {
+		snprintf(uds_local, sizeof(uds_local), "/var/run/ts2phc-ptpmon");
+	} else {
+		snprintf(uds_local, sizeof(uds_local), path);
+	}
+
+	s->agent = pmc_agent_create();
+	if (!s->agent) {
+		pr_err("failed to start pmc agent");
+		goto err_agent;
+	}
+
+	err = init_pmc_node(priv->cfg, s->agent, uds_local,
+			    ts2phc_recv_signaling_subscribed, s);
+	if (err) {
+		pr_err("failed to create PMC agent: %s", strerror(err));
+		goto err_pmc_init;
+	}
+	pmc_agent_enable_signaling_cb(s->agent, true);
+
+	s->pps_source.destroy = ts2phc_ptp_pps_source_destroy;
+	s->pps_source.getppstime = ts2phc_ptp_pps_source_getppstime;
+	s->priv = priv;
+
+	pthread_mutex_init(&s->mutex, NULL);
+	err = pthread_create(&s->worker, NULL, monitor_ptp_signaling_msg, s);
+	if (err) {
+		pr_err("failed to create worker thread: %s", strerror(err));
+		goto err_thread;
+	}
+
+	return &s->pps_source;
+
+err_thread:
+	pthread_mutex_destroy(&s->mutex);
+err_pmc_init:
+	pmc_agent_destroy(s->agent);
+err_agent:
+	free(s);
+	return NULL;
+}
diff --git a/ts2phc_ptp_pps_source.h b/ts2phc_ptp_pps_source.h
new file mode 100644
index 0000000..45bf7ae
--- /dev/null
+++ b/ts2phc_ptp_pps_source.h
@@ -0,0 +1,15 @@
+/**
+ * @file ts2phc_ptp_pps_source.h
+ * @note Copyright (C) 2023 Maciek Machnikowski <ma...@ma...>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_TS2PHC_PTP_PPS_SOURCE_H
+#define HAVE_TS2PHC_PTP_PPS_SOURCE_H
+
+#include "ts2phc.h"
+#include "ts2phc_pps_source.h"
+
+struct ts2phc_pps_source *ts2phc_ptp_pps_source_create(struct ts2phc_private *priv,
+							   const char *dev);
+
+#endif
-- 
2.30.2





Thread: [Linuxptp-devel] [PATCH v2 2/2] ts2phc: Add PTP as a source of ToD

PTP IEEE 1588 stack for Linux

linuxptp-devel