From a96797a7fc7de9b1a20dbd5c5df0c98bc424a944 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Wed, 17 Dec 2014 15:24:02 +0100
Subject: [PATCH] clock: Introduce a function to switch the PTP Hardware Clock.

When switching clock devices in JBOD mode, we need to be able to reset the
servo. The existing servo_reset() function will not serve us well, because
in this case we also need to seed the existing frequency offset and limit.
This patch adds a new method that simply starts the servo from scratch.

In the unlikely event of a resource allocation failure, the method will
simply continue to use the previous device, which is better than nothing
and certainly preferable to bailing out the program.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 clock.c | 37 +++++++++++++++++++++++++++++++++++++
 clock.h |  8 ++++++++
 2 files changed, 45 insertions(+)

diff --git a/clock.c b/clock.c
index 835d2c3..b841e81 100644
--- a/clock.c
+++ b/clock.c
@@ -74,6 +74,7 @@ struct clock_subscriber {
 struct clock {
 	clockid_t clkid;
 	struct servo *servo;
+	enum servo_type servo_type;
 	struct defaultDS dds;
 	struct dataset default_dataset;
 	struct currentDS cur;
@@ -862,6 +863,7 @@ struct clock *clock_create(int phc_index, struct interfaces_head *ifaces,
 		return NULL;
 	}
 	c->servo_state = SERVO_UNLOCKED;
+	c->servo_type = servo;
 	c->delay_filter = filter_create(dds->delay_filter,
 					dds->delay_filter_length);
 	if (!c->delay_filter) {
@@ -1358,6 +1360,41 @@ UInteger16 clock_steps_removed(struct clock *c)
 	return c->cur.stepsRemoved;
 }
 
+int clock_switch_phc(struct clock *c, int phc_index)
+{
+	struct servo *servo;
+	int fadj, max_adj;
+	clockid_t clkid;
+	char phc[32];
+
+	snprintf(phc, 31, "/dev/ptp%d", phc_index);
+	clkid = phc_open(phc);
+	if (clkid == CLOCK_INVALID) {
+		pr_err("Switching PHC, failed to open %s: %m", phc);
+		return -1;
+	}
+	max_adj = phc_max_adj(clkid);
+	if (!max_adj) {
+		pr_err("Switching PHC, clock is not adjustable");
+		phc_close(clkid);
+		return -1;
+	}
+	fadj = (int) clockadj_get_freq(clkid);
+	clockadj_set_freq(clkid, fadj);
+	servo = servo_create(c->servo_type, -fadj, max_adj, 0);
+	if (!servo) {
+		pr_err("Switching PHC, failed to create clock servo");
+		phc_close(clkid);
+		return -1;
+	}
+	phc_close(c->clkid);
+	servo_destroy(c->servo);
+	c->clkid = clkid;
+	c->servo = servo;
+	c->servo_state = SERVO_UNLOCKED;
+	return 0;
+}
+
 enum servo_state clock_synchronize(struct clock *c,
 				   struct timespec ingress_ts,
 				   struct timestamp origin_ts,
diff --git a/clock.h b/clock.h
index a2b46be..4834464 100644
--- a/clock.h
+++ b/clock.h
@@ -204,6 +204,14 @@ int clock_slave_only(struct clock *c);
  */
 UInteger16 clock_steps_removed(struct clock *c);
 
+/**
+ * Switch to a new PTP Hardware Clock, for use with the "jbod" mode.
+ * @param c          The clock instance.
+ * @param phc_index  The index of the PHC device to use.
+ * @return           Zero on success, non-zero otherwise.
+ */
+int clock_switch_phc(struct clock *c, int phc_index);
+
 /**
  * Provide a data point to synchronize the clock.
  * @param c            The clock instance to synchronize.