From 87e61f97707e7c25ea152696a081788a85b5db6a Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Tue, 13 Dec 2011 04:57:28 +0100
Subject: [PATCH] Let the clock servo know about the expected time stamp
 quality.

If software time stamping is to be used, then the servo will want to
have appropriate filtering.

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

diff --git a/clock.c b/clock.c
index cf0d936..0d89948 100644
--- a/clock.c
+++ b/clock.c
@@ -158,7 +158,7 @@ UInteger8 clock_class(struct clock *c)
 struct clock *clock_create(char *phc, struct interface *iface, int count,
 			   struct defaultDS *ds)
 {
-	int i, max_adj;
+	int i, max_adj, sw_ts = 0;
 	struct clock *c = &the_clock;
 
 	srandom(time(NULL));
@@ -182,7 +182,14 @@ struct clock *clock_create(char *phc, struct interface *iface, int count,
 		max_adj = 512000;
 	}
 
-	c->servo = servo_create("pi", max_adj);
+	for (i = 0; i < count; i++) {
+		if (iface[i].timestamping == TS_SOFTWARE) {
+			sw_ts = 1;
+			break;
+		}
+	}
+
+	c->servo = servo_create("pi", 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 2bab166..5ef2337 100644
--- a/pi.c
+++ b/pi.c
@@ -22,8 +22,11 @@
 #include "pi.h"
 #include "servo_private.h"
 
-#define KP 0.7
-#define KI 0.3
+#define HWTS_KP 0.7
+#define HWTS_KI 0.3
+
+#define SWTS_KP 0.1
+#define SWTS_KI 0.001
 
 struct pi_servo {
 	struct servo servo;
@@ -31,6 +34,8 @@ struct pi_servo {
 	double local[2];
 	double drift;
 	double maxppb;
+	double kp;
+	double ki;
 	int count;
 };
 
@@ -72,8 +77,8 @@ static double pi_sample(struct servo *servo,
 		s->count = 4;
 		break;
 	case 4:
-		ki_term = KI * offset;
-		ppb = KP * offset + s->drift + ki_term;
+		ki_term = s->ki * offset;
+		ppb = s->kp * offset + s->drift + ki_term;
 		if (ppb < -s->maxppb) {
 			ppb = -s->maxppb;
 		} else if (ppb > s->maxppb) {
@@ -88,7 +93,7 @@ static double pi_sample(struct servo *servo,
 	return ppb;
 }
 
-struct servo *pi_servo_create(int max_ppb)
+struct servo *pi_servo_create(int max_ppb, int sw_ts)
 {
 	struct pi_servo *s;
 
@@ -100,5 +105,13 @@ struct servo *pi_servo_create(int max_ppb)
 	s->servo.sample  = pi_sample;
 	s->maxppb        = max_ppb;
 
+	if (sw_ts) {
+		s->kp = SWTS_KP;
+		s->ki = SWTS_KI;
+	} else {
+		s->kp = HWTS_KP;
+		s->ki = HWTS_KI;
+	}
+
 	return &s->servo;
 }
diff --git a/pi.h b/pi.h
index 4782543..aa2bd85 100644
--- a/pi.h
+++ b/pi.h
@@ -21,6 +21,6 @@
 
 #include "servo.h"
 
-struct servo *pi_servo_create(int max_ppb);
+struct servo *pi_servo_create(int max_ppb, int sw_ts);
 
 #endif
diff --git a/servo.c b/servo.c
index a976af8..de8ec27 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)
+struct servo *servo_create(char *name, int max_ppb, int sw_ts)
 {
 	if (!strncmp(name, "pi", 2)) {
-		return pi_servo_create(max_ppb);
+		return pi_servo_create(max_ppb, sw_ts);
 	}
 	return NULL;
 }
diff --git a/servo.h b/servo.h
index c63aa86..5e88179 100644
--- a/servo.h
+++ b/servo.h
@@ -51,9 +51,11 @@ enum servo_state {
  * @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.
+ * @param sw_ts   Indicates that software time stamping will be used,
+ *                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);
+struct servo *servo_create(char *name, int max_ppb, int sw_ts);
 
 /**
  * Destroy an instance of a clock servo.