[Linuxptp-devel] [PATCH v2] phc_ctl: Implement setting frequency from system time.

[Maciek M. <ma...@ma...>]

Implement auto mode for frequency setting. In this mode the tool
will measure the frequency difference between PHC and CLOCK_REALTIME
and apply the correction to the PHC to match the system clock.

v2: rebase and change measure time to 5 seconds

Signed-off-by: Maciek Machnikowski <ma...@ma...>
---
 phc_ctl.8 | 13 +++++++-
 phc_ctl.c | 92 +++++++++++++++++++++++++++++++++++++++++++------------
 2 files changed, 84 insertions(+), 21 deletions(-)

diff --git a/phc_ctl.8 b/phc_ctl.8
index b10566e..25066f6 100644
--- a/phc_ctl.8
+++ b/phc_ctl.8
@@ -60,7 +60,8 @@ Adjust the PHC clock by an amount of seconds provided. This argument is required
 .BI freq " ppb"
 Adjust the frequency of the PHC clock by the specified parts per billion. If no
 argument is provided, it will attempt to read the current frequency and report
-it.
+it. If keyword "auto" is passed, the frequency of the PHC clock will be adjusted
+to match the frequency of the CLOCK_REALTIME.
 .TP
 .BI phase " seconds"
 Pass an amount in seconds to the PHC clock's phase control keyword. This
@@ -99,6 +100,16 @@ Set PHC clock time to 0 (seconds since Epoch)
 \f(CWphc_ctl /dev/ptp0 set 0.0\fP
 .RE
 
+Set PHC frequency to a negative value
+.RS
+\f(CWphc_ctl /dev/ptp0 -- freq -10\fP
+.RE
+
+Set PHC frequency and time from the system clock
+.RS
+\f(CWphc_ctl /dev/ptp0 -- freq auto set\fP
+.RE
+
 Quickly sanity check frequency slewing by setting slewing frequency by positive
 10%, resetting clock to 0.0 time, waiting for 10 seconds, and then reading
 time. The time read back should be (roughly) 11 seconds, since the clock was
diff --git a/phc_ctl.c b/phc_ctl.c
index 893d87d..5f71412 100644
--- a/phc_ctl.c
+++ b/phc_ctl.c
@@ -49,6 +49,7 @@
 #include "version.h"
 
 #define NSEC2SEC 1000000000.0
+#define N_SAMPLES 9
 
 /* trap the alarm signal so that pause() will wake up on receipt */
 static void handle_alarm(int s)
@@ -244,35 +245,88 @@ static int do_adj(clockid_t clkid, int cmdc, char *cmdv[])
 
 static int do_freq(clockid_t clkid, int cmdc, char *cmdv[])
 {
-	double ppb;
+	int64_t t1_sys_offset, t1_delay, t2_sys_offset, t2_delay;
+	struct timespec t1_clk, t1_sys, t2_clk, t2_sys;
+	uint64_t t1_sys_ts, t2_sys_ts;
+	int64_t clk_diff, sys_diff;
 	enum parser_result r;
+	int method, fd;
+	double ppb;
 
 	clockadj_init(clkid);
-
 	if (cmdc < 1 || name_is_a_command(cmdv[0])) {
 		ppb = clockadj_get_freq(clkid);
 		pr_notice("clock frequency offset is %lfppb", ppb);
 
 		/* no argument was used */
 		return 0;
-	}
+	} else if (strcmp(cmdv[0], "auto") == 0) {
+		/* set the PHC freq to match the system time */
+		fd = CLOCKID_TO_FD(clkid);
 
-	/* parse the double ppb argument */
-	r = get_ranged_double(cmdv[0], &ppb, -NSEC2SEC, NSEC2SEC);
-	switch (r) {
-	case PARSED_OK:
-		break;
-	case MALFORMED:
-		pr_err("freq: '%s' is not a valid double", cmdv[0]);
-		return -2;
-	case OUT_OF_RANGE:
-		pr_err("freq: '%s' is out of range.", cmdv[0]);
-		return -2;
-	default:
-		pr_err("freq: couldn't process '%s'", cmdv[0]);
-		return -2;
-	}
+		ppb = clockadj_get_freq(clkid);
+
+		/* Try sysoff first */
+		method = sysoff_probe(fd, N_SAMPLES);
+		if (method >= 0) {
+			if (sysoff_measure(fd, method, N_SAMPLES,
+					   &t1_sys_offset,
+					   &t1_sys_ts, &t1_delay)) {
+				return -1;
+			}
+
+			sleep(5);
 
+			if (sysoff_measure(fd, method, N_SAMPLES,
+					   &t2_sys_offset,
+					   &t2_sys_ts, &t2_delay)) {
+				return -1;
+		       }
+
+			sys_diff = t2_sys_ts - t1_sys_ts;
+			clk_diff = t2_sys_offset - t1_sys_offset;
+			ppb += (1e9 - ppb) * ((double)clk_diff / (double)sys_diff);
+		} else {
+			/* if sysoff is not available - try gettime */
+			if (clock_gettime(clkid, &t1_clk)) {
+				pr_err("get: failed to get clock time: %s",
+					strerror(errno));
+				return -1;
+			}
+			clock_gettime(CLOCK_REALTIME, &t1_sys);
+
+			sleep(1);
+
+			if (clock_gettime(clkid, &t2_clk)) {
+				pr_err("get: failed to get clock time: %s",
+				       strerror(errno));
+				return -1;
+			}
+			clock_gettime(CLOCK_REALTIME, &t2_sys);
+
+			clk_diff = NSEC2SEC * (t2_clk.tv_sec - t1_clk.tv_sec) +
+				   (t2_clk.tv_nsec - t1_clk.tv_nsec);
+			sys_diff = NSEC2SEC * (t2_sys.tv_sec - t1_sys.tv_sec) +
+				   (t2_sys.tv_nsec - t1_sys.tv_nsec);
+			ppb += (1e9 - ppb) * (1 - (double)clk_diff / (double)sys_diff);
+		}
+	} else {
+		/* parse the double ppb argument */
+		r = get_ranged_double(cmdv[0], &ppb, -NSEC2SEC, NSEC2SEC);
+		switch (r) {
+		case PARSED_OK:
+			break;
+		case MALFORMED:
+			pr_err("freq: '%s' is not a valid double", cmdv[0]);
+			return -2;
+		case OUT_OF_RANGE:
+			pr_err("freq: '%s' is out of range.", cmdv[0]);
+			return -2;
+		default:
+			pr_err("freq: couldn't process '%s'", cmdv[0]);
+			return -2;
+		}
+	}
 	clockadj_set_freq(clkid, ppb);
 	pr_notice("adjusted clock frequency offset to %lfppb", ppb);
 
@@ -370,8 +424,6 @@ static int do_cmp(clockid_t clkid, int cmdc, char *cmdv[])
 	uint64_t sys_ts;
 	int method, fd;
 
-#define N_SAMPLES 9
-
 	fd = CLOCKID_TO_FD(clkid);
 
 	method = sysoff_probe(fd, N_SAMPLES);
-- 
2.30.2