From b8160f2760f073d5fffe8b628c87d561026015bf Mon Sep 17 00:00:00 2001
From: Jacob Keller <jacob.e.keller@intel.com>
Date: Fri, 28 Sep 2012 11:46:00 -0700
Subject: [PATCH] ptp4l: add maximum offset to pi servo

this patch modifies the pi servo to add a configurable max offset (default
infinity). When ever the detected offset is larger than this value, the clock
will jump and reset the servo state. The value of this feature is for decreasing
time to stabalize when clock is off by a large ammount during late running. This
can occur when the upstream master changes, or when the clock is reset due to
outside forces. The method used to reset clock is simply to reset the pi servo
to the unlocked state.

Signed-off-by: Jacob Keller <jacob.e.keller@intel.com>
---
 config.c |  5 +++++
 config.h |  2 ++
 pi.c     | 24 ++++++++++++++++++++++++
 pi.h     |  9 +++++++++
 ptp4l.c  |  2 ++
 5 files changed, 42 insertions(+)

diff --git a/config.c b/config.c
index 79341b6..f12ebc5 100644
--- a/config.c
+++ b/config.c
@@ -223,6 +223,11 @@ static void scan_global_line(const char *s, struct config *cfg)
 		if (df > 0.0 && df < 1.0)
 			*cfg->pi_integral_const = df;
 
+	} else if (1 == sscanf(s, " pi_offset_const %lf", &df)) {
+
+		if (df > 0.0)
+			*cfg->pi_offset_const = df;
+
 	} else if (MAC_LEN == sscanf(s, " ptp_dst_mac %hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
 			&mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5])) {
 
diff --git a/config.h b/config.h
index 9691478..8840b41 100644
--- a/config.h
+++ b/config.h
@@ -66,6 +66,8 @@ struct config {
 
 	double *pi_proportional_const;
 	double *pi_integral_const;
+	double *pi_offset_const;
+
 	unsigned char *ptp_dst_mac;
 	unsigned char *p2p_dst_mac;
 
diff --git a/pi.c b/pi.c
index d003587..cf83f9f 100644
--- a/pi.c
+++ b/pi.c
@@ -18,6 +18,7 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 #include <stdlib.h>
+#include <math.h>
 
 #include "pi.h"
 #include "servo_private.h"
@@ -28,9 +29,12 @@
 #define SWTS_KP 0.1
 #define SWTS_KI 0.001
 
+#define NSEC_PER_SEC 1000000000
+
 /* These two take their values from the configuration file. (see ptp4l.c) */
 double configured_pi_kp;
 double configured_pi_ki;
+double configured_pi_offset;
 
 struct pi_servo {
 	struct servo servo;
@@ -40,6 +44,7 @@ struct pi_servo {
 	double maxppb;
 	double kp;
 	double ki;
+	double max_offset;
 	int count;
 };
 
@@ -81,6 +86,19 @@ static double pi_sample(struct servo *servo,
 		s->count = 4;
 		break;
 	case 4:
+		/*
+		 * reset the clock servo when offset is greater than the max
+		 * offset value. Note that the clock jump will be performed in
+		 * step 3, so it is not necessary to have clock jump
+		 * immediately. This allows re-calculating drift as in initial
+		 * clock startup.
+		 */
+		if (s->max_offset && (s->max_offset < fabs(offset))) {
+			*state = SERVO_UNLOCKED;
+			s->count = 0;
+			break;
+		}
+
 		ki_term = s->ki * offset;
 		ppb = s->kp * offset + s->drift + ki_term;
 		if (ppb < -s->maxppb) {
@@ -121,5 +139,11 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
 		s->ki = HWTS_KI;
 	}
 
+	if (configured_pi_offset > 0.0) {
+		s->max_offset = configured_pi_offset * NSEC_PER_SEC;
+	} else {
+		s->max_offset = 0.0;
+	}
+
 	return &s->servo;
 }
diff --git a/pi.h b/pi.h
index 2208832..eff90aa 100644
--- a/pi.h
+++ b/pi.h
@@ -33,6 +33,15 @@ extern double configured_pi_kp;
  */
 extern double configured_pi_ki;
 
+/**
+ * When set to a non-zero value, this variable controls the maximum allowed
+ * offset before a clock jump occurs instead of the default clock-slewing
+ * mechanism
+ *
+ * Note that this variable is measured in seconds, and allows fractional values.
+ */
+extern double configured_pi_offset;
+
 struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts);
 
 #endif
diff --git a/ptp4l.c b/ptp4l.c
index 7a2ef61..9a9ba9e 100644
--- a/ptp4l.c
+++ b/ptp4l.c
@@ -70,6 +70,8 @@ static struct config cfg_settings = {
 
 	.pi_proportional_const = &configured_pi_kp,
 	.pi_integral_const = &configured_pi_ki,
+	.pi_offset_const = &configured_pi_offset,
+
 	.ptp_dst_mac = ptp_dst_mac,
 	.p2p_dst_mac = p2p_dst_mac,