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

@@ -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])) {
 

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;
 

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;
 }

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

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,