From c5ce4ef764ee1207a456326aa375ccad1f2ca497 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 17 Jan 2013 18:31:39 +0100
Subject: [PATCH] phc2sys: read PHC with each PPS sample.

In the PPS loop, instead of setting the system clock from the PHC only
once on start, read PHC with each PPS sample and use the time stamp to
get the whole number of seconds in the offset. This will prevent phc2sys
from losing track of the system clock.

Also, check if the PPS is synchronized to the PHC.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
---
 phc2sys.8 |  4 ++--
 phc2sys.c | 38 +++++++++++++++++++++++++++-----------
 2 files changed, 29 insertions(+), 13 deletions(-)

diff --git a/phc2sys.8 b/phc2sys.8
index 113f30c..adce4ad 100644
--- a/phc2sys.8
+++ b/phc2sys.8
@@ -60,8 +60,8 @@ option should be used too.
 Specify the master clock by device (e.g. /dev/ptp0) or name (e.g. CLOCK_REALTIME
 for the system clock). When this option is used together with the
 .B \-d
-option, the master clock is read only on start to fix an offset over 0.5
-seconds which cannot be fixed with PPS alone.
+option, the master clock is used only to correct the offset by whole number of
+seconds, which cannot be fixed with PPS alone.
 .TP
 .BI \-i " interface"
 Similar to the
diff --git a/phc2sys.c b/phc2sys.c
index 466cc0b..1c0d8b2 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -46,6 +46,8 @@
 
 #define max_ppb  512000
 
+#define PHC_PPS_OFFSET_LIMIT 10000000
+
 static clockid_t clock_open(char *device)
 {
 	int fd;
@@ -198,8 +200,8 @@ static int read_pps(int fd, int64_t *offset, uint64_t *ts)
 static int do_pps_loop(struct clock *clock, char *pps_device,
 		       clockid_t src, int n_readings, int sync_offset)
 {
-	int64_t pps_offset;
-	uint64_t pps_ts;
+	int64_t pps_offset, phc_offset;
+	uint64_t pps_ts, phc_ts;
 	int fd;
 
 	clock->source_label = "pps";
@@ -210,19 +212,33 @@ static int do_pps_loop(struct clock *clock, char *pps_device,
 		return -1;
 	}
 
-	/* Make the initial sync from PHC if available. */
-	if (src != CLOCK_INVALID) {
-		if (!read_phc(src, clock->clkid, n_readings,
-			      &pps_offset, &pps_ts))
-			return -1;
-		pps_offset -= sync_offset * NS_PER_SEC;
-		clock_step(clock->clkid, -pps_offset);
-	}
-
 	while (1) {
 		if (!read_pps(fd, &pps_offset, &pps_ts)) {
 			continue;
 		}
+
+		/* If a PHC is available, use it to get the whole number
+		   of seconds in the offset and PPS for the rest. */
+		if (src != CLOCK_INVALID) {
+			if (!read_phc(src, clock->clkid, n_readings,
+				      &phc_offset, &phc_ts))
+				return -1;
+			
+			/* Convert the time stamp to the PHC time. */
+			phc_ts -= phc_offset;
+
+			/* Check if it is close to the start of the second. */
+			if (phc_ts % NS_PER_SEC > PHC_PPS_OFFSET_LIMIT) {
+				fprintf(stderr, "PPS is not in sync with PHC"
+					" (0.%09lld)\n", phc_ts % NS_PER_SEC);
+				continue;
+			}
+
+			phc_ts = phc_ts / NS_PER_SEC * NS_PER_SEC;
+			pps_offset = pps_ts - phc_ts;
+			pps_offset -= sync_offset * NS_PER_SEC;
+		}
+
 		update_clock(clock, pps_offset, pps_ts);
 	}
 	close(fd);