From 3045a49855e50eb8a962b9c54104af0fd103d124 Mon Sep 17 00:00:00 2001
From: Vladimir Oltean <olteanv@gmail.com>
Date: Tue, 28 Jul 2020 03:01:16 +0300
Subject: [PATCH] ts2phc: reconfigure sync direction by subscribing to ptp4l
 port events

Monitor the port state change events from ptp4l, and use that
information to determine the "source" clock.

Then synchronize all other clocks in our list to that source, by feeding
into their respective servo loop an offset equal to the delta between
their timestamp and the timestamp of the source clock. All timestamps
are representative of the same event, which is the most recent perout
pulse of the ts2phc master.

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
---
 ts2phc.c       | 129 ++++++++++++++++++++++++++++++++++++++++++++-----
 ts2phc_slave.c |   1 +
 2 files changed, 118 insertions(+), 12 deletions(-)

diff --git a/ts2phc.c b/ts2phc.c
index de1e7d0..75a4010 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -329,29 +329,119 @@ static int auto_init_ports(struct ts2phc_private *priv)
 	return 0;
 }
 
-static void ts2phc_synchronize_clocks(struct ts2phc_private *priv)
+static void ts2phc_reconfigure(struct ts2phc_private *priv)
+{
+	struct clock *c, *src = NULL, *last = NULL;
+	int src_cnt = 0, dst_cnt = 0;
+
+	pr_info("reconfiguring after port state change");
+	priv->state_changed = 0;
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		if (c->new_state) {
+			c->state = c->new_state;
+			c->new_state = 0;
+		}
+
+		switch (c->state) {
+		case PS_FAULTY:
+		case PS_DISABLED:
+		case PS_LISTENING:
+		case PS_PRE_MASTER:
+		case PS_MASTER:
+		case PS_PASSIVE:
+			if (!c->is_destination) {
+				pr_info("selecting %s for synchronization",
+					c->name);
+				c->is_destination = 1;
+			}
+			dst_cnt++;
+			break;
+		case PS_UNCALIBRATED:
+			src_cnt++;
+			break;
+		case PS_SLAVE:
+			src = c;
+			src_cnt++;
+			break;
+		}
+		last = c;
+	}
+	if (dst_cnt >= 1 && !src) {
+		priv->source = last;
+		priv->source->is_destination = 0;
+		/* Reset to original state in next reconfiguration. */
+		priv->source->new_state = priv->source->state;
+		priv->source->state = PS_SLAVE;
+		pr_info("no source, selecting %s as the default clock",
+			last->name);
+		return;
+	}
+	if (src_cnt > 1) {
+		pr_info("multiple source clocks available, postponing sync...");
+		priv->source = NULL;
+		return;
+	}
+	if (src_cnt > 0 && !src) {
+		pr_info("source clock not ready, waiting...");
+		priv->source = NULL;
+		return;
+	}
+	if (!src_cnt && !dst_cnt) {
+		pr_info("no PHC ready, waiting...");
+		priv->source = NULL;
+		return;
+	}
+	if (!src_cnt) {
+		pr_info("nothing to synchronize");
+		priv->source = NULL;
+		return;
+	}
+	src->is_destination = 0;
+	priv->source = src;
+	pr_info("selecting %s as the source clock", src->name);
+}
+
+static void ts2phc_synchronize_clocks(struct ts2phc_private *priv, int autocfg)
 {
-	struct timespec source_ts;
 	tmv_t source_tmv;
 	struct clock *c;
 	int valid, err;
 
-	err = ts2phc_master_getppstime(priv->master, &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;
+	if (autocfg) {
+		if (!priv->source) {
+			pr_debug("no source, skipping");
+			return;
+		}
+		valid = clock_get_tstamp(priv->source, &source_tmv);
+		if (!valid) {
+			pr_err("source clock (%s) timestamp not valid, skipping",
+				priv->source->name);
+			return;
+		}
+	} else {
+		struct timespec source_ts;
 
-	source_tmv = timespec_to_tmv(source_ts);
+		err = ts2phc_master_getppstime(priv->master, &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;
 
+		if (!c->is_destination)
+			continue;
+
 		valid = clock_get_tstamp(c, &ts);
 		if (!valid) {
 			pr_debug("%s timestamp not valid, skipping", c->name);
@@ -582,6 +672,21 @@ int main(int argc, char *argv[])
 	while (is_running()) {
 		struct clock *c;
 
+		if (autocfg) {
+			/*
+			 * Make sure ptp4l sees us as alive and doesn't prune
+			 * us from the list of subscribers
+			 */
+			err = update_pmc_node(&priv.node, 1);
+			if (err < 0) {
+				pr_err("update_pmc_node returned %d", err);
+				break;
+			}
+			run_pmc_events(&priv.node);
+			if (priv.state_changed)
+				ts2phc_reconfigure(&priv);
+		}
+
 		LIST_FOREACH(c, &priv.clocks, list)
 			clock_flush_tstamp(c);
 
@@ -591,7 +696,7 @@ int main(int argc, char *argv[])
 			break;
 		}
 		if (err > 0)
-			ts2phc_synchronize_clocks(&priv);
+			ts2phc_synchronize_clocks(&priv, autocfg);
 	}
 
 	ts2phc_cleanup(&priv);
diff --git a/ts2phc_slave.c b/ts2phc_slave.c
index f5c5dc0..031362a 100644
--- a/ts2phc_slave.c
+++ b/ts2phc_slave.c
@@ -187,6 +187,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
 		pr_err("failed to open clock");
 		goto no_posix_clock;
 	}
+	slave->clock->is_destination = 1;
 
 	pr_debug("PHC slave %s has ptp index %d", device,
 		 slave->clock->phc_index);