From 4ef70c3d2ef70cb7368cb7e5fbe55d91706e7bc0 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Mon, 26 Nov 2012 13:25:35 +0100
Subject: [PATCH] phc2sys: use the system offset method if available

We use the PTP_SYS_OFFSET ioctl method in preference to doing
clock_gettime from user space, the assumption being that the kernel space
measurement will always be more accurate.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 makefile  |  4 ++--
 phc2sys.c | 26 ++++++++++++++++++++++++++
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/makefile b/makefile
index 43a58b3..afa5021 100644
--- a/makefile
+++ b/makefile
@@ -31,7 +31,7 @@ PRG	= ptp4l pmc phc2sys hwstamp_ctl
 OBJ	= bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \
  print.o raw.o servo.o sk.o tlv.o tmtab.o transport.o udp.o udp6.o uds.o util.o
 
-OBJECTS	= $(OBJ) pmc.o phc2sys.o hwstamp_ctl.o
+OBJECTS	= $(OBJ) pmc.o phc2sys.o hwstamp_ctl.o sysoff.o
 SRC	= $(OBJECTS:.o=.c)
 DEPEND	= $(OBJECTS:.o=.d)
 srcdir	:= $(dir $(lastword $(MAKEFILE_LIST)))
@@ -48,7 +48,7 @@ ptp4l: $(OBJ)
 
 pmc: pmc.o msg.o print.o raw.o sk.o tlv.o transport.o udp.o udp6.o uds.o util.o
 
-phc2sys: phc2sys.o sk.o print.o
+phc2sys: phc2sys.o sk.o sysoff.o print.o
 
 hwstamp_ctl: hwstamp_ctl.o
 
diff --git a/phc2sys.c b/phc2sys.c
index a6586a2..31e88ed 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -34,6 +34,7 @@
 
 #include "missing.h"
 #include "sk.h"
+#include "sysoff.h"
 
 #define KP 0.7
 #define KI 0.3
@@ -228,6 +229,26 @@ static int do_pps_loop(char *pps_device, double kp, double ki, clockid_t dst)
 	return 0;
 }
 
+static int do_sysoff_loop(clockid_t src, clockid_t dst,
+			  int rate, int n_readings, int sync_offset,
+			  double kp, double ki)
+{
+	uint64_t ts;
+	int64_t offset;
+	int err = 0, fd = CLOCKID_TO_FD(src);
+	while (1) {
+		usleep(1000000 / rate);
+		if (sysoff_measure(fd, n_readings, &offset, &ts)) {
+			err = -1;
+			break;
+		}
+		offset -= sync_offset * NS_PER_SEC;
+		do_servo(&servo, dst, offset, ts, kp, ki);
+		show_servo(stdout, "sys", offset, ts);
+	}
+	return err;
+}
+
 static void usage(char *progname)
 {
 	fprintf(stderr,
@@ -323,6 +344,11 @@ int main(int argc, char *argv[])
 	if (device)
 		return do_pps_loop(device, kp, ki, dst);
 
+	if (dst == CLOCK_REALTIME &&
+	    SYSOFF_SUPPORTED == sysoff_probe(CLOCKID_TO_FD(src), phc_readings))
+		return do_sysoff_loop(src, dst, phc_rate,
+				      phc_readings, sync_offset, kp, ki);
+
 	while (1) {
 		usleep(1000000 / phc_rate);
 		if (!read_phc(src, dst, phc_readings, &phc_offset, &phc_ts)) {