[Linuxptp-devel] [PATCH v5 06/13] ts2phc: instantiate a full clock structure for every PPS sink

[Vladimir O. <ol...@gm...>]

Currently in ts2phc, PPS sinks are also the only candidates for the
clocks that get synchronized. There are 2 aspects that make this too
restrictive:

- Not all PPS sinks may be target clocks for synchronization. Consider a
  dynamic environment of multiple DSA switches using boundary_clock_jbod,
  and only one port is in the PS_SLAVE state. In that case, the clock of
  that port should be the reference for synchronization, regardless of
  whether it supports the extts API or not.

- Not all target clocks for synchronization may be PPS sinks.
  Specifically, the "PHC" type of PPS master in ts2phc can also be,
  fundamentally, treated as a target clock, and disciplined.

The code base should be prepared to handle the distinction that exists
between these concepts, by recognizing that things that can be
disciplined by a servo should be represented by a "struct ts2phc_clock",
and things that can timestamp external events should be represented by a
"struct ts2phc_pps_sink".

Signed-off-by: Vladimir Oltean <ol...@gm...>
---
v4->v5:
- Rebased on top of data structure renames
v3->v4:
- rebased, needed to pass priv->cfg in ts2phc_nmea_master_create
- setting clock->is_destination can be delayed to a future patch
v2->v3:
None.

 ts2phc.c          | 81 ++++++++++++++++++++++++++++++++++++++++++
 ts2phc.h          | 21 +++++++++++
 ts2phc_pps_sink.c | 90 ++++++++++++++++++-----------------------------
 3 files changed, 137 insertions(+), 55 deletions(-)

diff --git a/ts2phc.c b/ts2phc.c
index f228351bae3a..f63e5503637d 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -6,10 +6,15 @@
  * @note Copyright (C) 2012 Richard Cochran <ric...@gm...>
  * @note SPDX-License-Identifier: GPL-2.0+
  */
+#include <net/if.h>
 #include <stdlib.h>
+#include <sys/types.h>
+#include <unistd.h>
 
+#include "clockadj.h"
 #include "config.h"
 #include "interface.h"
+#include "phc.h"
 #include "print.h"
 #include "ts2phc.h"
 #include "version.h"
@@ -27,6 +32,82 @@ static void ts2phc_cleanup(struct ts2phc_private *priv)
 		config_destroy(priv->cfg);
 }
 
+static struct servo *ts2phc_servo_create(struct ts2phc_private *priv,
+					 struct ts2phc_clock *clock)
+{
+	enum servo_type type = config_get_int(priv->cfg, NULL, "clock_servo");
+	struct servo *servo;
+	int fadj, max_adj;
+
+	fadj = (int) clockadj_get_freq(clock->clkid);
+	/* Due to a bug in older kernels, the reading may silently fail
+	 * and return 0. Set the frequency back to make sure fadj is
+	 * the actual frequency of the clock.
+	 */
+	if (!clock->no_adj) {
+		clockadj_set_freq(clock->clkid, fadj);
+	}
+
+	max_adj = phc_max_adj(clock->clkid);
+
+	servo = servo_create(priv->cfg, type, -fadj, max_adj, 0);
+	if (!servo)
+		return NULL;
+
+	servo_sync_interval(servo, SERVO_SYNC_INTERVAL);
+
+	return servo;
+}
+
+struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
+				      const char *device)
+{
+	clockid_t clkid = CLOCK_INVALID;
+	struct ts2phc_clock *c;
+	int phc_index = -1;
+	int err;
+
+	clkid = posix_clock_open(device, &phc_index);
+	if (clkid == CLOCK_INVALID)
+		return NULL;
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		if (c->phc_index == phc_index) {
+			/* Already have the clock, don't add it again */
+			posix_clock_close(clkid);
+			return c;
+		}
+	}
+
+	c = calloc(1, sizeof(*c));
+	if (!c) {
+		pr_err("failed to allocate memory for a clock");
+		return NULL;
+	}
+	c->clkid = clkid;
+	c->phc_index = phc_index;
+	c->servo_state = SERVO_UNLOCKED;
+	c->servo = ts2phc_servo_create(priv, c);
+	c->no_adj = config_get_int(priv->cfg, NULL, "free_running");
+	err = asprintf(&c->name, "/dev/ptp%d", phc_index);
+	if (err < 0) {
+		free(c);
+		posix_clock_close(clkid);
+		return NULL;
+	}
+
+	LIST_INSERT_HEAD(&priv->clocks, c, list);
+	return c;
+}
+
+void ts2phc_clock_destroy(struct ts2phc_clock *c)
+{
+	servo_destroy(c->servo);
+	posix_clock_close(c->clkid);
+	free(c->name);
+	free(c);
+}
+
 static void usage(char *progname)
 {
 	fprintf(stderr,
diff --git a/ts2phc.h b/ts2phc.h
index 14cb2b0c21a3..043215a51609 100644
--- a/ts2phc.h
+++ b/ts2phc.h
@@ -7,16 +7,37 @@
 #ifndef HAVE_TS2PHC_H
 #define HAVE_TS2PHC_H
 
+#include <sys/queue.h>
+#include <time.h>
+#include "servo.h"
+
 struct ts2phc_sink_array;
 
+#define SERVO_SYNC_INTERVAL    1.0
+
+struct ts2phc_clock {
+	LIST_ENTRY(ts2phc_clock) list;
+	clockid_t clkid;
+	int phc_index;
+	struct servo *servo;
+	enum servo_state servo_state;
+	char *name;
+	bool no_adj;
+};
+
 struct ts2phc_private {
 	struct ts2phc_pps_source *src;
 	STAILQ_HEAD(sink_ifaces_head, ts2phc_pps_sink) sinks;
 	unsigned int n_sinks;
 	struct ts2phc_sink_array *polling_array;
 	struct config *cfg;
+	LIST_HEAD(clock_head, ts2phc_clock) clocks;
 };
 
+struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
+				      const char *device);
+void ts2phc_clock_destroy(struct ts2phc_clock *clock);
+
 #include "ts2phc_pps_source.h"
 #include "ts2phc_pps_sink.h"
 
diff --git a/ts2phc_pps_sink.c b/ts2phc_pps_sink.c
index 6a537d08c21b..e272155eb73d 100644
--- a/ts2phc_pps_sink.c
+++ b/ts2phc_pps_sink.c
@@ -26,21 +26,17 @@
 
 #define NS_PER_SEC		1000000000LL
 #define SAMPLE_WEIGHT		1.0
-#define SERVO_SYNC_INTERVAL	1.0
 
 struct ts2phc_pps_sink {
 	char *name;
 	STAILQ_ENTRY(ts2phc_pps_sink) list;
 	struct ptp_pin_desc pin_desc;
-	enum servo_state state;
 	unsigned int polarity;
 	int32_t correction;
 	uint32_t ignore_lower;
 	uint32_t ignore_upper;
-	struct servo *servo;
-	clockid_t clk;
+	struct ts2phc_clock *clock;
 	int no_adj;
-	int fd;
 };
 
 struct ts2phc_sink_array {
@@ -96,8 +92,10 @@ static int ts2phc_pps_sink_array_create(struct ts2phc_private *priv)
 		i++;
 	}
 	for (i = 0; i < priv->n_sinks; i++) {
+		struct ts2phc_pps_sink *sink = polling_array->sink[i];
+
 		polling_array->pfd[i].events = POLLIN | POLLPRI;
-		polling_array->pfd[i].fd = polling_array->sink[i]->fd;
+		polling_array->pfd[i].fd = CLOCKID_TO_FD(sink->clock->clkid);
 	}
 
 	priv->polling_array = polling_array;
@@ -111,15 +109,15 @@ static void ts2phc_pps_sink_array_destroy(struct ts2phc_private *priv)
 
 	free(polling_array->sink);
 	free(polling_array->pfd);
-	polling_array->sink = NULL;
-	polling_array->pfd = NULL;
+	free(polling_array);
+	priv->polling_array = NULL;
 }
 
 static int ts2phc_pps_sink_clear_fifo(struct ts2phc_pps_sink *sink)
 {
 	struct pollfd pfd = {
 		.events = POLLIN | POLLPRI,
-		.fd = sink->fd,
+		.fd = CLOCKID_TO_FD(sink->clock->clkid),
 	};
 	struct ptp_extts_event event;
 	int cnt, size;
@@ -152,10 +150,9 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 						      const char *device)
 {
 	struct config *cfg = priv->cfg;
-	enum servo_type servo = config_get_int(cfg, NULL, "clock_servo");
-	int err, fadj, junk, max_adj, pulsewidth;
 	struct ptp_extts_request extts;
 	struct ts2phc_pps_sink *sink;
+	int err, pulsewidth;
 
 	sink = calloc(1, sizeof(*sink));
 	if (!sink) {
@@ -179,34 +176,17 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 	sink->ignore_upper = 1000000000 - pulsewidth;
 	sink->ignore_lower = pulsewidth;
 
-	sink->clk = posix_clock_open(device, &junk);
-	if (sink->clk == CLOCK_INVALID) {
+	sink->clock = ts2phc_clock_add(priv, device);
+	if (!sink->clock) {
 		pr_err("failed to open clock");
 		goto no_posix_clock;
 	}
-	sink->no_adj = config_get_int(cfg, NULL, "free_running");
-	sink->fd = CLOCKID_TO_FD(sink->clk);
-
-	pr_debug("PPS sink %s has ptp index %d", device, junk);
 
-	fadj = (int) clockadj_get_freq(sink->clk);
-	/* Due to a bug in older kernels, the reading may silently fail
-	   and return 0. Set the frequency back to make sure fadj is
-	   the actual frequency of the clock. */
-	if (!sink->no_adj) {
-		clockadj_set_freq(sink->clk, fadj);
-	}
-	max_adj = phc_max_adj(sink->clk);
+	pr_debug("PPS sink %s has ptp index %d", device,
+		 sink->clock->phc_index);
 
-	sink->servo = servo_create(cfg, servo, -fadj, max_adj, 0);
-	if (!sink->servo) {
-		pr_err("failed to create servo");
-		goto no_servo;
-	}
-	servo_sync_interval(sink->servo, SERVO_SYNC_INTERVAL);
-
-	if (phc_number_pins(sink->clk) > 0) {
-		err = phc_pin_setfunc(sink->clk, &sink->pin_desc);
+	if (phc_number_pins(sink->clock->clkid) > 0) {
+		err = phc_pin_setfunc(sink->clock->clkid, &sink->pin_desc);
 		if (err < 0) {
 			pr_err("PTP_PIN_SETFUNC request failed");
 			goto no_pin_func;
@@ -220,7 +200,8 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 	memset(&extts, 0, sizeof(extts));
 	extts.index = sink->pin_desc.chan;
 	extts.flags = 0;
-	if (ioctl(sink->fd, PTP_EXTTS_REQUEST2, &extts)) {
+	if (ioctl(CLOCKID_TO_FD(sink->clock->clkid), PTP_EXTTS_REQUEST2,
+		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
 	if (ts2phc_pps_sink_clear_fifo(sink)) {
@@ -230,9 +211,7 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 	return sink;
 no_ext_ts:
 no_pin_func:
-	servo_destroy(sink->servo);
-no_servo:
-	posix_clock_close(sink->clk);
+	ts2phc_clock_destroy(sink->clock);
 no_posix_clock:
 	free(sink->name);
 	free(sink);
@@ -246,11 +225,11 @@ static void ts2phc_pps_sink_destroy(struct ts2phc_pps_sink *sink)
 	memset(&extts, 0, sizeof(extts));
 	extts.index = sink->pin_desc.chan;
 	extts.flags = 0;
-	if (ioctl(sink->fd, PTP_EXTTS_REQUEST2, &extts)) {
+	if (ioctl(CLOCKID_TO_FD(sink->clock->clkid), PTP_EXTTS_REQUEST2,
+		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
-	servo_destroy(sink->servo);
-	posix_clock_close(sink->clk);
+	ts2phc_clock_destroy(sink->clock);
 	free(sink->name);
 	free(sink);
 }
@@ -278,27 +257,22 @@ static int ts2phc_pps_sink_event(struct ts2phc_pps_sink *sink,
 		return 0;
 	}
 
-	if (!source_ts.valid) {
-		pr_debug("%s ignoring invalid source time stamp", sink->name);
-		return 0;
-	}
-
-	adj = servo_sample(sink->servo, offset, extts_ts,
-			   SAMPLE_WEIGHT, &sink->state);
+	adj = servo_sample(sink->clock->servo, offset, extts_ts,
+			   SAMPLE_WEIGHT, &sink->clock->servo_state);
 
 	pr_debug("%s source offset %10" PRId64 " s%d freq %+7.0f",
-		 sink->name, offset, sink->state, adj);
+		 sink->name, offset, sink->clock->servo_state, adj);
 
-	switch (sink->state) {
+	switch (sink->clock->servo_state) {
 	case SERVO_UNLOCKED:
 		break;
 	case SERVO_JUMP:
-		clockadj_set_freq(sink->clk, -adj);
-		clockadj_step(sink->clk, -offset);
+		clockadj_set_freq(sink->clock->clkid, -adj);
+		clockadj_step(sink->clock->clkid, -offset);
 		break;
 	case SERVO_LOCKED:
 	case SERVO_LOCKED_STABLE:
-		clockadj_set_freq(sink->clk, -adj);
+		clockadj_set_freq(sink->clock->clkid, -adj);
 		break;
 	}
 	return 0;
@@ -314,7 +288,7 @@ ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
 	uint64_t event_ns, source_ns;
 	int cnt;
 
-	cnt = read(sink->fd, &event, sizeof(event));
+	cnt = read(CLOCKID_TO_FD(sink->clock->clkid), &event, sizeof(event));
 	if (cnt != sizeof(event)) {
 		pr_err("read extts event failed: %m");
 		return EXTTS_ERROR;
@@ -386,7 +360,8 @@ int ts2phc_pps_sink_arm(struct ts2phc_private *priv)
 	STAILQ_FOREACH(sink, &priv->sinks, list) {
 		extts.index = sink->pin_desc.chan;
 		extts.flags = sink->polarity | PTP_ENABLE_FEATURE;
-		err = ioctl(sink->fd, PTP_EXTTS_REQUEST2, &extts);
+		err = ioctl(CLOCKID_TO_FD(sink->clock->clkid),
+			    PTP_EXTTS_REQUEST2, &extts);
 		if (err < 0) {
 			pr_err(PTP_EXTTS_REQUEST_FAILED);
 			return -1;
@@ -442,6 +417,11 @@ int ts2phc_pps_sink_poll(struct ts2phc_private *priv)
 	err = ts2phc_pps_source_getppstime(priv->src, &source_ts.ts);
 	source_ts.valid = err ? false : true;
 
+	if (!source_ts.valid) {
+		pr_debug("ignoring invalid source time stamp");
+		return 0;
+	}
+
 	for (i = 0; i < priv->n_sinks; i++) {
 		if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
 			ts2phc_pps_sink_event(polling_array->sink[i], source_ts);
-- 
2.25.1