From 8f00d29265532785b394329eaaf9d6a3069ac274 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Fri, 21 Sep 2012 09:27:53 +0200
Subject: [PATCH] Discover and utilize the initial clock frequency adjustment.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 clock.c | 19 +++++++++++++++++--
 pi.c    |  5 +++--
 pi.h    |  2 +-
 servo.c |  4 ++--
 servo.h |  3 ++-
 5 files changed, 25 insertions(+), 8 deletions(-)

diff --git a/clock.c b/clock.c
index eeefd39..dc0e6c6 100644
--- a/clock.c
+++ b/clock.c
@@ -270,6 +270,18 @@ static void clock_ppb(clockid_t clkid, double ppb)
 		pr_err("failed to adjust the clock: %m");
 }
 
+static double clock_ppb_read(clockid_t clkid)
+{
+	double f = 0.0;
+	struct timex tx;
+	memset(&tx, 0, sizeof(tx));
+	if (clock_adjtime(clkid, &tx) < 0)
+		pr_err("failed to read out the clock frequency adjustment: %m");
+	else
+		f = tx.freq / 65.536;
+	return f;
+}
+
 static void clock_step(clockid_t clkid, int64_t ns)
 {
 	struct timex tx;
@@ -388,7 +400,7 @@ UInteger8 clock_class(struct clock *c)
 struct clock *clock_create(int phc_index, struct interface *iface, int count,
 			   enum timestamp_type timestamping, struct defaultDS *ds)
 {
-	int i, max_adj, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0;
+	int i, fadj = 0, max_adj, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0;
 	struct clock *c = &the_clock;
 	char phc[32];
 	struct interface udsif;
@@ -422,7 +434,10 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count,
 		max_adj = 512000;
 	}
 
-	c->servo = servo_create("pi", max_adj, sw_ts);
+	if (c->clkid != CLOCK_INVALID) {
+		fadj = (int) clock_ppb_read(c->clkid);
+	}
+	c->servo = servo_create("pi", -fadj, max_adj, sw_ts);
 	if (!c->servo) {
 		pr_err("Failed to create clock servo");
 		return NULL;
diff --git a/pi.c b/pi.c
index 6917899..d003587 100644
--- a/pi.c
+++ b/pi.c
@@ -71,7 +71,7 @@ static double pi_sample(struct servo *servo,
 		s->count = 2;
 		break;
 	case 2:
-		s->drift = (s->offset[1] - s->offset[0]) /
+		s->drift += (s->offset[1] - s->offset[0]) /
 			(s->local[1] - s->local[0]);
 		*state = SERVO_UNLOCKED;
 		s->count = 3;
@@ -97,7 +97,7 @@ static double pi_sample(struct servo *servo,
 	return ppb;
 }
 
-struct servo *pi_servo_create(int max_ppb, int sw_ts)
+struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
 {
 	struct pi_servo *s;
 
@@ -107,6 +107,7 @@ struct servo *pi_servo_create(int max_ppb, int sw_ts)
 
 	s->servo.destroy = pi_destroy;
 	s->servo.sample  = pi_sample;
+	s->drift         = fadj;
 	s->maxppb        = max_ppb;
 
 	if (configured_pi_kp && configured_pi_ki) {
diff --git a/pi.h b/pi.h
index e1babd3..2208832 100644
--- a/pi.h
+++ b/pi.h
@@ -33,6 +33,6 @@ extern double configured_pi_kp;
  */
 extern double configured_pi_ki;
 
-struct servo *pi_servo_create(int max_ppb, int sw_ts);
+struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts);
 
 #endif
diff --git a/servo.c b/servo.c
index de8ec27..8a644aa 100644
--- a/servo.c
+++ b/servo.c
@@ -21,10 +21,10 @@
 #include "pi.h"
 #include "servo_private.h"
 
-struct servo *servo_create(char *name, int max_ppb, int sw_ts)
+struct servo *servo_create(char *name, int fadj, int max_ppb, int sw_ts)
 {
 	if (!strncmp(name, "pi", 2)) {
-		return pi_servo_create(max_ppb, sw_ts);
+		return pi_servo_create(fadj, max_ppb, sw_ts);
 	}
 	return NULL;
 }
diff --git a/servo.h b/servo.h
index 5e88179..0147d79 100644
--- a/servo.h
+++ b/servo.h
@@ -48,6 +48,7 @@ enum servo_state {
 /**
  * Create a new instance of a clock servo.
  * @param name    The name of the servo flavor to create.
+ * @param fadj    The clock's current adjustment in parts per billion.
  * @param max_ppb The absolute maxinum adjustment allowed by the clock
  *                in parts per billion. The clock servo will clamp its
  *                output according to this limit.
@@ -55,7 +56,7 @@ enum servo_state {
  *                and the servo should use more aggressive filtering.
  * @return A pointer to a new servo on success, NULL otherwise.
  */
-struct servo *servo_create(char *name, int max_ppb, int sw_ts);
+struct servo *servo_create(char *name, int fadj, int max_ppb, int sw_ts);
 
 /**
  * Destroy an instance of a clock servo.