From e63a6ea89b8fac5ee6ef12a18b6fdbd81ed76dd6 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 19 Dec 2013 18:39:11 +0100
Subject: [PATCH] Adjust tick length with system clock.

This increases the maximum frequency adjustment of the system clock
from 500 ppm to 10%.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
---
 clock.c    |  2 ++
 clockadj.c | 43 ++++++++++++++++++++++++++++++++++++++++---
 clockadj.h |  6 ++++++
 phc2sys.c  |  1 +
 4 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/clock.c b/clock.c
index 30a3ffc..bd41080 100644
--- a/clock.c
+++ b/clock.c
@@ -616,9 +616,11 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count,
 			pr_err("clock is not adjustable");
 			return NULL;
 		}
+		clockadj_init(c->clkid);
 	} else {
 		c->clkid = CLOCK_REALTIME;
 		c->utc_timescale = 1;
+		clockadj_init(c->clkid);
 		max_adj = sysclk_max_freq();
 		sysclk_set_leap(0);
 	}
diff --git a/clockadj.c b/clockadj.c
index b0505b7..e0e82f0 100644
--- a/clockadj.c
+++ b/clockadj.c
@@ -17,7 +17,9 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
+#include <math.h>
 #include <string.h>
+#include <unistd.h>
 
 #include "clockadj.h"
 #include "missing.h"
@@ -26,12 +28,37 @@
 #define NS_PER_SEC 1000000000LL
 
 static int realtime_leap_bit;
+static long realtime_hz;
+static long realtime_nominal_tick;
+
+void clockadj_init(clockid_t clkid)
+{
+#ifdef _SC_CLK_TCK
+	if (clkid == CLOCK_REALTIME) {
+		/* This is USER_HZ in the kernel. */
+		realtime_hz = sysconf(_SC_CLK_TCK);
+		if (realtime_hz > 0) {
+			/* This is TICK_USEC in the kernel. */
+			realtime_nominal_tick =
+				(1000000 + realtime_hz / 2) / realtime_hz;
+		}
+	}
+#endif
+}
 
 void clockadj_set_freq(clockid_t clkid, double freq)
 {
 	struct timex tx;
 	memset(&tx, 0, sizeof(tx));
-	tx.modes = ADJ_FREQUENCY;
+
+	/* With system clock set also the tick length. */
+	if (clkid == CLOCK_REALTIME && realtime_nominal_tick) {
+		tx.modes |= ADJ_TICK;
+		tx.tick = round(freq / 1e3 / realtime_hz) + realtime_nominal_tick;
+		freq -= 1e3 * realtime_hz * (tx.tick - realtime_nominal_tick);
+	}
+
+	tx.modes |= ADJ_FREQUENCY;
 	tx.freq = (long) (freq * 65.536);
 	if (clock_adjtime(clkid, &tx) < 0)
 		pr_err("failed to adjust the clock: %m");
@@ -42,10 +69,13 @@ double clockadj_get_freq(clockid_t clkid)
 	double f = 0.0;
 	struct timex tx;
 	memset(&tx, 0, sizeof(tx));
-	if (clock_adjtime(clkid, &tx) < 0)
+	if (clock_adjtime(clkid, &tx) < 0) {
 		pr_err("failed to read out the clock frequency adjustment: %m");
-	else
+	} else {
 		f = tx.freq / 65.536;
+		if (clkid == CLOCK_REALTIME && realtime_nominal_tick)
+			f += 1e3 * realtime_hz * (tx.tick - realtime_nominal_tick);
+	}
 	return f;
 }
 
@@ -111,6 +141,13 @@ int sysclk_max_freq(void)
 		f = tx.tolerance / 65.536;
 	if (!f)
 		f = 500000;
+
+	/* The kernel allows the tick length to be adjusted up to 10%. But use
+	   it only if the overall frequency of the clock can be adjusted
+	   continuously with the tick and freq fields (i.e. hz <= 1000). */
+	if (realtime_nominal_tick && 2 * f >= 1000 * realtime_hz)
+		f = realtime_nominal_tick / 10 * 1000 * realtime_hz;
+
 	return f;
 }
 
diff --git a/clockadj.h b/clockadj.h
index 79ae80d..7578e84 100644
--- a/clockadj.h
+++ b/clockadj.h
@@ -23,6 +23,12 @@
 #include <inttypes.h>
 #include <time.h>
 
+/**
+ * Initialize state needed when adjusting or reading the clock.
+ * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME.
+ */
+void clockadj_init(clockid_t clkid);
+
 /**
  * Set clock's frequency offset.
  * @param clkid A clock ID obtained using phc_open() or CLOCK_REALTIME.
diff --git a/phc2sys.c b/phc2sys.c
index 2b8af49..1a045ee 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -779,6 +779,7 @@ int main(int argc, char *argv[])
 			close_pmc(&dst_clock);
 	}
 
+	clockadj_init(dst_clock.clkid);
 	ppb = clockadj_get_freq(dst_clock.clkid);
 	/* The reading may silently fail and return 0, reset the frequency to
 	   make sure ppb is the actual frequency of the clock. */