[Linuxptp-devel] [PATCH v5 10/13] ts2phc: split PPS sink poll from servo loop

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

The previous patches have established that:

- a PPS sink deals with extts events
- a clock deals with synchronization via a servo loop

Therefore, the code for synchronization should not be part of the
implementation of a PPS sink. Move it to the main ts2phc.c.
This allows it to be used by a PPS source as well (the PHC kind).

Signed-off-by: Vladimir Oltean <ol...@gm...>
---
v4->v5:
- Rebase on top of the variable renaming.
v3->v4:
- Use bool for boolean types.
v2->v3:
- None.

 ts2phc.c          |  94 +++++++++++++++++++++-
 ts2phc.h          |   3 +
 ts2phc_pps_sink.c | 199 ++++++++++++++++++++++------------------------
 3 files changed, 190 insertions(+), 106 deletions(-)

diff --git a/ts2phc.c b/ts2phc.c
index 1c577af7768e..f84950947712 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -21,6 +21,9 @@
 #include "ts2phc.h"
 #include "version.h"
 
+#define NS_PER_SEC		1000000000LL
+#define SAMPLE_WEIGHT		1.0
+
 struct interface {
 	STAILQ_ENTRY(interface) list;
 };
@@ -154,6 +157,30 @@ static struct servo *ts2phc_servo_create(struct ts2phc_private *priv,
 	return servo;
 }
 
+void ts2phc_clock_add_tstamp(struct ts2phc_clock *clock, tmv_t t)
+{
+	struct timespec ts = tmv_to_timespec(t);
+
+	pr_debug("adding tstamp %ld.%09ld to clock %s",
+		 ts.tv_sec, ts.tv_nsec, clock->name);
+	clock->last_ts = t;
+	clock->is_ts_available = true;
+}
+
+static int ts2phc_clock_get_tstamp(struct ts2phc_clock *clock, tmv_t *ts)
+{
+	if (!clock->is_ts_available)
+		return 0;
+	clock->is_ts_available = false;
+	*ts = clock->last_ts;
+	return 1;
+}
+
+static void ts2phc_clock_flush_tstamp(struct ts2phc_clock *clock)
+{
+	clock->is_ts_available = false;
+}
+
 struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
 				      const char *device)
 {
@@ -303,6 +330,64 @@ static int auto_init_ports(struct ts2phc_private *priv)
 	return 0;
 }
 
+static void ts2phc_synchronize_clocks(struct ts2phc_private *priv)
+{
+	struct timespec source_ts;
+	tmv_t source_tmv;
+	struct ts2phc_clock *c;
+	int valid, err;
+
+	err = ts2phc_pps_source_getppstime(priv->src, &source_ts);
+	if (err < 0) {
+		pr_err("source ts not valid");
+		return;
+	}
+	if (source_ts.tv_nsec > NS_PER_SEC / 2)
+		source_ts.tv_sec++;
+	source_ts.tv_nsec = 0;
+
+	source_tmv = timespec_to_tmv(source_ts);
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		int64_t offset;
+		double adj;
+		tmv_t ts;
+
+		valid = ts2phc_clock_get_tstamp(c, &ts);
+		if (!valid) {
+			pr_debug("%s timestamp not valid, skipping", c->name);
+			continue;
+		}
+
+		offset = tmv_to_nanoseconds(tmv_sub(ts, source_tmv));
+
+		if (c->no_adj) {
+			pr_info("%s offset %10" PRId64, c->name,
+				offset);
+			continue;
+		}
+
+		adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ts),
+				   SAMPLE_WEIGHT, &c->servo_state);
+
+		pr_info("%s offset %10" PRId64 " s%d freq %+7.0f",
+			c->name, offset, c->servo_state, adj);
+
+		switch (c->servo_state) {
+		case SERVO_UNLOCKED:
+			break;
+		case SERVO_JUMP:
+			clockadj_set_freq(c->clkid, -adj);
+			clockadj_step(c->clkid, -offset);
+			break;
+		case SERVO_LOCKED:
+		case SERVO_LOCKED_STABLE:
+			clockadj_set_freq(c->clkid, -adj);
+			break;
+		}
+	}
+}
+
 static void usage(char *progname)
 {
 	fprintf(stderr,
@@ -501,11 +586,18 @@ int main(int argc, char *argv[])
 	}
 
 	while (is_running()) {
+		struct ts2phc_clock *c;
+
+		LIST_FOREACH(c, &priv.clocks, list)
+			ts2phc_clock_flush_tstamp(c);
+
 		err = ts2phc_pps_sink_poll(&priv);
-		if (err) {
+		if (err < 0) {
 			pr_err("poll failed");
 			break;
 		}
+		if (err > 0)
+			ts2phc_synchronize_clocks(&priv);
 	}
 
 	ts2phc_cleanup(&priv);
diff --git a/ts2phc.h b/ts2phc.h
index 1fab09dbf130..f52a2f3d3cae 100644
--- a/ts2phc.h
+++ b/ts2phc.h
@@ -27,6 +27,8 @@ struct ts2phc_clock {
 	enum servo_state servo_state;
 	char *name;
 	bool no_adj;
+	bool is_ts_available;
+	tmv_t last_ts;
 };
 
 struct ts2phc_port {
@@ -50,6 +52,7 @@ struct ts2phc_private {
 struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
 				      const char *device);
 void ts2phc_clock_destroy(struct ts2phc_clock *clock);
+void ts2phc_clock_add_tstamp(struct ts2phc_clock *clock, tmv_t ts);
 
 #include "ts2phc_pps_source.h"
 #include "ts2phc_pps_sink.h"
diff --git a/ts2phc_pps_sink.c b/ts2phc_pps_sink.c
index 69cc97179e36..92bb8dd47107 100644
--- a/ts2phc_pps_sink.c
+++ b/ts2phc_pps_sink.c
@@ -24,42 +24,29 @@
 #include "ts2phc.h"
 #include "util.h"
 
-#define NS_PER_SEC		1000000000LL
-#define SAMPLE_WEIGHT		1.0
-
 struct ts2phc_pps_sink {
 	char *name;
 	STAILQ_ENTRY(ts2phc_pps_sink) list;
 	struct ptp_pin_desc pin_desc;
 	unsigned int polarity;
-	int32_t correction;
+	tmv_t correction;
 	uint32_t ignore_lower;
 	uint32_t ignore_upper;
 	struct ts2phc_clock *clock;
-	int no_adj;
 };
 
 struct ts2phc_sink_array {
 	struct ts2phc_pps_sink **sink;
+	int *collected_events;
 	struct pollfd *pfd;
 };
 
-struct ts2phc_source_timestamp {
-	struct timespec ts;
-	bool valid;
-};
-
 enum extts_result {
 	EXTTS_ERROR	= -1,
 	EXTTS_OK	= 0,
 	EXTTS_IGNORE	= 1,
 };
 
-static enum extts_result
-ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
-		       struct ts2phc_source_timestamp ts,
-		       int64_t *offset, uint64_t *local_ts);
-
 static int ts2phc_pps_sink_array_create(struct ts2phc_private *priv)
 {
 	struct ts2phc_sink_array *polling_array;
@@ -86,6 +73,15 @@ static int ts2phc_pps_sink_array_create(struct ts2phc_private *priv)
 		polling_array->sink = NULL;
 		return -1;
 	}
+	polling_array->collected_events = malloc(priv->n_sinks * sizeof(int));
+	if (!polling_array->collected_events) {
+		pr_err("low memory");
+		free(polling_array->sink);
+		free(polling_array->pfd);
+		polling_array->pfd = NULL;
+		polling_array->sink = NULL;
+		return -1;
+	}
 	i = 0;
 	STAILQ_FOREACH(sink, &priv->sinks, list) {
 		polling_array->sink[i] = sink;
@@ -107,8 +103,12 @@ static void ts2phc_pps_sink_array_destroy(struct ts2phc_private *priv)
 {
 	struct ts2phc_sink_array *polling_array = priv->polling_array;
 
+	if (!polling_array)
+		return;
+
 	free(polling_array->sink);
 	free(polling_array->pfd);
+	free(polling_array->collected_events);
 	free(polling_array);
 	priv->polling_array = NULL;
 }
@@ -153,6 +153,7 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 	struct ptp_extts_request extts;
 	struct ts2phc_pps_sink *sink;
 	int err, pulsewidth;
+	int32_t correction;
 
 	sink = calloc(1, sizeof(*sink));
 	if (!sink) {
@@ -169,7 +170,8 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri
 	sink->pin_desc.func = PTP_PF_EXTTS;
 	sink->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel");
 	sink->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity");
-	sink->correction = config_get_int(cfg, device, "ts2phc.extts_correction");
+	correction = config_get_int(cfg, device, "ts2phc.extts_correction");
+	sink->correction = nanoseconds_to_tmv(correction);
 
 	pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth");
 	pulsewidth /= 2;
@@ -234,71 +236,32 @@ static void ts2phc_pps_sink_destroy(struct ts2phc_pps_sink *sink)
 	free(sink);
 }
 
-static int ts2phc_pps_sink_event(struct ts2phc_pps_sink *sink,
-				 struct ts2phc_source_timestamp source_ts)
+static enum extts_result ts2phc_pps_sink_event(struct ts2phc_private *priv,
+					       struct ts2phc_pps_sink *sink)
 {
-	enum extts_result result;
-	uint64_t extts_ts;
-	int64_t offset;
-	double adj;
-
-	result = ts2phc_pps_sink_offset(sink, source_ts, &offset, &extts_ts);
-	switch (result) {
-	case EXTTS_ERROR:
-		return -1;
-	case EXTTS_OK:
-		break;
-	case EXTTS_IGNORE:
-		return 0;
-	}
-
-	if (sink->no_adj) {
-		pr_info("%s source offset %10" PRId64, sink->name, offset);
-		return 0;
-	}
-
-	adj = servo_sample(sink->clock->servo, offset, extts_ts,
-			   SAMPLE_WEIGHT, &sink->clock->servo_state);
-
-	pr_info("%s source offset %10" PRId64 " s%d freq %+7.0f",
-		sink->name, offset, sink->clock->servo_state, adj);
-
-	switch (sink->clock->servo_state) {
-	case SERVO_UNLOCKED:
-		break;
-	case SERVO_JUMP:
-		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->clock->clkid, -adj);
-		break;
-	}
-	return 0;
-}
-
-static enum extts_result
-ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
-		       struct ts2phc_source_timestamp src,
-		       int64_t *offset, uint64_t *local_ts)
-{
-	struct timespec source_ts = src.ts;
+	enum extts_result result = EXTTS_OK;
 	struct ptp_extts_event event;
-	uint64_t event_ns, source_ns;
-	int cnt;
+	struct timespec source_ts;
+	int err, cnt;
+	tmv_t ts;
 
 	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;
+		result = EXTTS_ERROR;
+		goto out;
 	}
 	if (event.index != sink->pin_desc.chan) {
 		pr_err("extts on unexpected channel");
-		return EXTTS_ERROR;
+		result = EXTTS_ERROR;
+		goto out;
+	}
+
+	err = ts2phc_pps_source_getppstime(priv->src, &source_ts);
+	if (err < 0) {
+		pr_debug("source ts not valid");
+		return 0;
 	}
-	event_ns = event.t.sec * NS_PER_SEC;
-	event_ns += event.t.nsec;
 
 	if (sink->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
 	    source_ts.tv_nsec > sink->ignore_lower &&
@@ -308,20 +271,17 @@ ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
 		 sink->name, event.index, event.t.sec, event.t.nsec,
 		 (int64_t) source_ts.tv_sec, source_ts.tv_nsec);
 
-		return EXTTS_IGNORE;
-	}
-	if (source_ts.tv_nsec > 500000000) {
-		source_ts.tv_sec++;
+		result = EXTTS_IGNORE;
+		goto out;
 	}
-	source_ns = source_ts.tv_sec * NS_PER_SEC;
-	*offset = event_ns + sink->correction - source_ns;
-	*local_ts = event_ns + sink->correction;
 
-	pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64
-		 ".%ld diff %" PRId64,
-		 sink->name, event.index, event.t.sec, event.t.nsec,
-		 sink->correction,
-		 (int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset);
+out:
+	if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
+		return result;
+
+	ts = pct_to_tmv(event.t);
+	ts = tmv_add(ts, sink->correction);
+	ts2phc_clock_add_tstamp(sink->clock, ts);
 
 	return EXTTS_OK;
 }
@@ -397,35 +357,64 @@ void ts2phc_pps_sink_cleanup(struct ts2phc_private *priv)
 int ts2phc_pps_sink_poll(struct ts2phc_private *priv)
 {
 	struct ts2phc_sink_array *polling_array = priv->polling_array;
-	struct ts2phc_source_timestamp source_ts;
+	bool all_sinks_have_events = false;
+	bool ignore_any = false;
 	unsigned int i;
-	int cnt, err;
+	int cnt;
+
+	for (i = 0; i < priv->n_sinks; i++)
+		polling_array->collected_events[i] = 0;
+
+	while (!all_sinks_have_events) {
+		struct ts2phc_pps_sink *sink;
 
-	cnt = poll(polling_array->pfd, priv->n_sinks, 2000);
-	if (cnt < 0) {
-		if (EINTR == errno) {
+		cnt = poll(polling_array->pfd, priv->n_sinks, 2000);
+		if (cnt < 0) {
+			if (errno == -EINTR) {
+				return 0;
+			} else {
+				pr_emerg("poll failed");
+				return -1;
+			}
+		} else if (!cnt) {
+			pr_debug("poll returns zero, no events");
 			return 0;
-		} else {
-			pr_emerg("poll failed");
-			return -1;
 		}
-	} else if (!cnt) {
-		pr_debug("poll returns zero, no events");
-		return 0;
-	}
 
-	err = ts2phc_pps_source_getppstime(priv->src, &source_ts.ts);
-	source_ts.valid = err ? false : true;
+		for (i = 0; i < priv->n_sinks; i++) {
+			if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
+				enum extts_result result;
+
+				sink = polling_array->sink[i];
+
+				result = ts2phc_pps_sink_event(priv, sink);
+				if (result == EXTTS_ERROR)
+					return -EIO;
+				if (result == EXTTS_IGNORE)
+					ignore_any = true;
+
+				/*
+				 * Collect the events anyway, even if we'll
+				 * ignore this source edge anyway. We don't
+				 * want sink events from different edges
+				 * to pile up and mix.
+				 */
+				polling_array->collected_events[i]++;
+			}
+		}
 
-	if (!source_ts.valid) {
-		pr_debug("ignoring invalid source time stamp");
-		return 0;
-	}
+		all_sinks_have_events = true;
 
-	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);
+		for (i = 0; i < priv->n_sinks; i++) {
+			if (!polling_array->collected_events[i]) {
+				all_sinks_have_events = false;
+				break;
+			}
 		}
 	}
-	return 0;
+
+	if (ignore_any)
+		return 0;
+
+	return 1;
 }
-- 
2.25.1