From 31feb001948f880cfadc446dd2c6c09b7c09499e Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 13 Mar 2014 18:34:17 +0100
Subject: [PATCH] Move PI step threshold and max frequency settings to common
 servo code.

These settings will be useful for all implemented servos, so move them
to the common servo code to avoid duplication. The configuration options
are renamed, but the they can be still set by their old names.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>
---
 config.c        | 15 +++++++------
 config.h        |  8 +++----
 default.cfg     |  6 +++---
 gPTP.cfg        |  6 +++---
 phc2sys.8       |  4 ++--
 phc2sys.c       |  4 ++--
 pi.c            | 57 +++++++++++++++----------------------------------
 pi.h            | 26 +---------------------
 ptp4l.8         | 22 ++++++++++++-------
 ptp4l.c         |  7 +++---
 servo.c         | 48 +++++++++++++++++++++++++++++++++++++----
 servo.h         | 24 +++++++++++++++++++++
 servo_private.h |  4 ++++
 13 files changed, 131 insertions(+), 100 deletions(-)

diff --git a/config.c b/config.c
index 03a4a45..7fdc1d8 100644
--- a/config.c
+++ b/config.c
@@ -383,23 +383,26 @@ static enum parser_result parse_global_setting(const char *option,
 			return r;
 		*cfg->pi_integral_norm_max = df;
 
-	} else if (!strcmp(option, "pi_offset_const")) {
+	} else if (!strcmp(option, "step_threshold") ||
+		   !strcmp(option, "pi_offset_const")) {
 		r = get_ranged_double(value, &df, 0.0, DBL_MAX);
 		if (r != PARSED_OK)
 			return r;
-		*cfg->pi_offset_const = df;
+		*cfg->step_threshold = df;
 
-	} else if (!strcmp(option, "pi_f_offset_const")) {
+	} else if (!strcmp(option, "first_step_threshold") ||
+		   !strcmp(option, "pi_f_offset_const")) {
 		r = get_ranged_double(value, &df, 0.0, DBL_MAX);
 		if (r != PARSED_OK)
 			return r;
-		*cfg->pi_f_offset_const = df;
+		*cfg->first_step_threshold = df;
 
-	} else if (!strcmp(option, "pi_max_frequency")) {
+	} else if (!strcmp(option, "max_frequency") ||
+		   !strcmp(option, "pi_max_frequency")) {
 		r = get_ranged_int(value, &val, 0, INT_MAX);
 		if (r != PARSED_OK)
 			return r;
-		*cfg->pi_max_frequency = val;
+		*cfg->max_frequency = val;
 
 	} else if (!strcmp(option, "sanity_freq_limit")) {
 		r = get_ranged_int(value, &val, 0, INT_MAX);
diff --git a/config.h b/config.h
index a380037..1b8cea3 100644
--- a/config.h
+++ b/config.h
@@ -77,6 +77,10 @@ struct config {
 
 	enum servo_type clock_servo;
 
+	double *step_threshold;
+	double *first_step_threshold;
+	int *max_frequency;
+
 	double *pi_proportional_const;
 	double *pi_integral_const;
 	double *pi_proportional_scale;
@@ -85,10 +89,6 @@ struct config {
 	double *pi_integral_scale;
 	double *pi_integral_exponent;
 	double *pi_integral_norm_max;
-	double *pi_offset_const;
-	double *pi_f_offset_const;
-	int *pi_max_frequency;
-
 	int *sanity_freq_limit;
 
 	unsigned char *ptp_dst_mac;
diff --git a/default.cfg b/default.cfg
index 80c20b9..7038b69 100644
--- a/default.cfg
+++ b/default.cfg
@@ -48,9 +48,9 @@ pi_proportional_norm_max	0.7
 pi_integral_scale	0.0
 pi_integral_exponent	0.4
 pi_integral_norm_max	0.3
-pi_offset_const		0.0
-pi_f_offset_const	0.0000001
-pi_max_frequency	900000000
+step_threshold		0.0
+first_step_threshold	0.0000001
+max_frequency		900000000
 clock_servo		pi
 sanity_freq_limit	200000000
 #
diff --git a/gPTP.cfg b/gPTP.cfg
index 6f09215..fac2aa0 100644
--- a/gPTP.cfg
+++ b/gPTP.cfg
@@ -48,9 +48,9 @@ pi_proportional_norm_max	0.7
 pi_integral_scale	0.0
 pi_integral_exponent	0.4
 pi_integral_norm_max	0.3
-pi_offset_const		0.0
-pi_f_offset_const	0.0000001
-pi_max_frequency	900000000
+step_threshold		0.0
+first_step_threshold	0.0000001
+max_frequency		900000000
 clock_servo		pi
 sanity_freq_limit	200000000
 #
diff --git a/phc2sys.8 b/phc2sys.8
index 39a121c..812b69e 100644
--- a/phc2sys.8
+++ b/phc2sys.8
@@ -96,8 +96,8 @@ Specify the proportional constant of the PI controller. The default is 0.7.
 Specify the integral constant of the PI controller. The default is 0.3.
 .TP
 .BI \-S " step"
-Specify the step threshold of the PI controller. It is the maximum offset that
-the controller corrects by changing the clock frequency instead of stepping the
+Specify the step threshold of the servo. It is the maximum offset that
+the servo corrects by changing the clock frequency instead of stepping the
 clock. The clock is stepped on start regardless of the option if the offset is
 larger than 100 nanoseconds (unless the
 .BI \-F
diff --git a/phc2sys.c b/phc2sys.c
index 1a045ee..d1bfd2e 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -635,12 +635,12 @@ int main(int argc, char *argv[])
 				return -1;
 			break;
 		case 'S':
-			if (get_arg_val_d(c, optarg, &configured_pi_offset,
+			if (get_arg_val_d(c, optarg, &servo_step_threshold,
 					  0.0, DBL_MAX))
 				return -1;
 			break;
 		case 'F':
-			if (get_arg_val_d(c, optarg, &configured_pi_f_offset,
+			if (get_arg_val_d(c, optarg, &servo_first_step_threshold,
 					  0.0, DBL_MAX))
 				return -1;
 			break;
diff --git a/pi.c b/pi.c
index 52d4c2f..c8b8587 100644
--- a/pi.c
+++ b/pi.c
@@ -32,7 +32,6 @@
 #define MAX_KP_NORM_MAX 1.0
 #define MAX_KI_NORM_MAX 2.0
 
-#define NSEC_PER_SEC 1000000000
 #define FREQ_EST_MARGIN 0.001
 
 /* These take their values from the configuration file. (see ptp4l.c) */
@@ -44,23 +43,16 @@ double configured_pi_kp_norm_max = 0.7;
 double configured_pi_ki_scale = 0.0;
 double configured_pi_ki_exponent = 0.4;
 double configured_pi_ki_norm_max = 0.3;
-double configured_pi_offset = 0.0;
-double configured_pi_f_offset = 0.0000001; /* 100 nanoseconds */
-int configured_pi_max_freq = 900000000;
 
 struct pi_servo {
 	struct servo servo;
 	int64_t offset[2];
 	uint64_t local[2];
 	double drift;
-	double maxppb;
 	double kp;
 	double ki;
-	double max_offset;
-	double max_f_offset;
 	double last_freq;
 	int count;
-	int first_update;
 };
 
 static void pi_destroy(struct servo *servo)
@@ -111,19 +103,21 @@ static double pi_sample(struct servo *servo,
 		/* Adjust drift by the measured frequency offset. */
 		s->drift += (1e9 - s->drift) * (s->offset[1] - s->offset[0]) /
 						(s->local[1] - s->local[0]);
-		if (s->drift < -s->maxppb)
-			s->drift = -s->maxppb;
-		else if (s->drift > s->maxppb)
-			s->drift = s->maxppb;
 
-		if ((s->first_update &&
-		     s->max_f_offset && (s->max_f_offset < fabs(offset))) ||
-		    (s->max_offset && (s->max_offset < fabs(offset))))
+		if (s->drift < -servo->max_frequency)
+			s->drift = -servo->max_frequency;
+		else if (s->drift > servo->max_frequency)
+			s->drift = servo->max_frequency;
+
+		if ((servo->first_update &&
+		     servo->first_step_threshold &&
+		     servo->first_step_threshold < fabs(offset)) ||
+		    (servo->step_threshold &&
+		     servo->step_threshold < fabs(offset)))
 			*state = SERVO_JUMP;
 		else
 			*state = SERVO_LOCKED;
 
-		s->first_update = 0;
 		ppb = s->drift;
 		s->count = 2;
 		break;
@@ -135,7 +129,8 @@ static double pi_sample(struct servo *servo,
 		 * immediately. This allows re-calculating drift as in initial
 		 * clock startup.
 		 */
-		if (s->max_offset && (s->max_offset < fabs(offset))) {
+		if (servo->step_threshold &&
+		    servo->step_threshold < fabs(offset)) {
 			*state = SERVO_UNLOCKED;
 			s->count = 0;
 			break;
@@ -143,10 +138,10 @@ static double pi_sample(struct servo *servo,
 
 		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) {
-			ppb = s->maxppb;
+		if (ppb < -servo->max_frequency) {
+			ppb = -servo->max_frequency;
+		} else if (ppb > servo->max_frequency) {
+			ppb = servo->max_frequency;
 		} else {
 			s->drift += ki_term;
 		}
@@ -181,7 +176,7 @@ static void pi_reset(struct servo *servo)
 	s->count = 0;
 }
 
-struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
+struct servo *pi_servo_create(int fadj, int sw_ts)
 {
 	struct pi_servo *s;
 
@@ -195,8 +190,6 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
 	s->servo.reset   = pi_reset;
 	s->drift         = fadj;
 	s->last_freq     = fadj;
-	s->maxppb        = max_ppb;
-	s->first_update  = 1;
 	s->kp            = 0.0;
 	s->ki            = 0.0;
 
@@ -220,21 +213,5 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
 		}
 	}
 
-	if (configured_pi_offset > 0.0) {
-		s->max_offset = configured_pi_offset * NSEC_PER_SEC;
-	} else {
-		s->max_offset = 0.0;
-	}
-
-	if (configured_pi_f_offset > 0.0) {
-		s->max_f_offset = configured_pi_f_offset * NSEC_PER_SEC;
-	} else {
-		s->max_f_offset = 0.0;
-	}
-
-	if (configured_pi_max_freq && s->maxppb > configured_pi_max_freq) {
-		s->maxppb = configured_pi_max_freq;
-	}
-
 	return &s->servo;
 }
diff --git a/pi.h b/pi.h
index 0d8994b..7b092a0 100644
--- a/pi.h
+++ b/pi.h
@@ -77,30 +77,6 @@ extern double configured_pi_ki_exponent;
  */
 extern double configured_pi_ki_norm_max;
 
-/**
- * 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;
-
-/**
- * When set to zero, the clock is not stepped on start. When set to a non-zero
- * value, the value bahaves as a threshold and the clock is stepped on start if
- * the offset is bigger than the threshold.
- *
- * Note that this variable is measured in seconds, and allows fractional values.
- */
-extern double configured_pi_f_offset;
-
-/**
- * When set to a non-zero value, this variable sets an additional limit for
- * the frequency adjustment of the clock. It's in ppb.
- */
-extern int configured_pi_max_freq;
-
-struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts);
+struct servo *pi_servo_create(int fadj, int sw_ts);
 
 #endif
diff --git a/ptp4l.8 b/ptp4l.8
index bce88f9..226123f 100644
--- a/ptp4l.8
+++ b/ptp4l.8
@@ -342,24 +342,30 @@ The ki_norm_max constant in the formula used to set the integral constant of
 the PI controller from the sync interval.
 The default is 0.3.
 .TP
-.B pi_offset_const
-The maximum offset the PI controller will correct by changing the clock
-frequency instead of stepping the clock. When set to 0.0, the controller will
+.B step_threshold
+The maximum offset the servo will correct by changing the clock
+frequency instead of stepping the clock. When set to 0.0, the servo will
 never step the clock except on start. It's specified in seconds.
 The default is 0.0.
+This option used to be called (and can still be set by)
+.BR pi_offset_const .
 .TP
-.B pi_f_offset_const
-The maximum offset the PI controller will correct by changing the clock
+.B first_step_threshold
+The maximum offset the servo will correct by changing the clock
 frequency instead of stepping the clock. This is only applied on the first
-update. It's specified in seconds. When set to 0.0, the controller won't step
+update. It's specified in seconds. When set to 0.0, the servo won't step
 the clock on start.
 The default is 0.0000001 (100 nanoseconds).
+This option used to be called (and can still be set by)
+.BR pi_f_offset_const .
 .TP
-.B pi_max_frequency
+.B max_frequency
 The maximum allowed frequency adjustment of the clock in parts per billion
 (ppb). This is an additional limit to the maximum allowed by the hardware. When
 set to 0, the hardware limit will be used.
 The default is 900000000 (90%).
+This option used to be called (and can still be set by)
+.BR pi_max_frequency .
 .TP
 .B sanity_freq_limit
 The maximum allowed frequency offset between uncorrected clock and the system
@@ -436,7 +442,7 @@ The default is 00:00:00.
 When a leap second is announced, let the kernel apply it by stepping the clock
 instead of correcting the one-second offset with servo, which would correct the
 one-second offset slowly by changing the clock frequency (unless the
-.B pi_offset_const
+.B step_threshold
 option is set to correct such offset by stepping).
 Relevant only with software time stamping. The default is 1 (enabled).
 .TP
diff --git a/ptp4l.c b/ptp4l.c
index 7e38435..bbf558d 100644
--- a/ptp4l.c
+++ b/ptp4l.c
@@ -104,6 +104,10 @@ static struct config cfg_settings = {
 
 	.clock_servo = CLOCK_SERVO_PI,
 
+	.step_threshold = &servo_step_threshold,
+	.first_step_threshold = &servo_first_step_threshold,
+	.max_frequency = &servo_max_frequency,
+
 	.pi_proportional_const = &configured_pi_kp,
 	.pi_integral_const = &configured_pi_ki,
 	.pi_proportional_scale = &configured_pi_kp_scale,
@@ -112,9 +116,6 @@ static struct config cfg_settings = {
 	.pi_integral_scale = &configured_pi_ki_scale,
 	.pi_integral_exponent = &configured_pi_ki_exponent,
 	.pi_integral_norm_max = &configured_pi_ki_norm_max,
-	.pi_offset_const = &configured_pi_offset,
-	.pi_f_offset_const = &configured_pi_f_offset,
-	.pi_max_frequency = &configured_pi_max_freq,
 
 	.ptp_dst_mac = ptp_dst_mac,
 	.p2p_dst_mac = p2p_dst_mac,
diff --git a/servo.c b/servo.c
index 5af020a..dd99d30 100644
--- a/servo.c
+++ b/servo.c
@@ -21,12 +21,45 @@
 #include "pi.h"
 #include "servo_private.h"
 
+#define NSEC_PER_SEC 1000000000
+
+double servo_step_threshold = 0.0;
+double servo_first_step_threshold = 0.0000001; /* 100 nanoseconds */
+int servo_max_frequency = 900000000;
+
 struct servo *servo_create(enum servo_type type, int fadj, int max_ppb, int sw_ts)
 {
-	if (type == CLOCK_SERVO_PI) {
-		return pi_servo_create(fadj, max_ppb, sw_ts);
+	struct servo *servo;
+
+	switch (type) {
+	case CLOCK_SERVO_PI:
+		servo = pi_servo_create(fadj, sw_ts);
+		break;
+	default:
+		return NULL;
 	}
-	return NULL;
+
+	if (servo_step_threshold > 0.0) {
+		servo->step_threshold = servo_step_threshold * NSEC_PER_SEC;
+	} else {
+		servo->step_threshold = 0.0;
+	}
+
+	if (servo_first_step_threshold > 0.0) {
+		servo->first_step_threshold =
+			servo_first_step_threshold * NSEC_PER_SEC;
+	} else {
+		servo->first_step_threshold = 0.0;
+	}
+
+	servo->max_frequency = max_ppb;
+	if (servo_max_frequency && servo->max_frequency > servo_max_frequency) {
+		servo->max_frequency = servo_max_frequency;
+	}
+
+	servo->first_update = 1;
+
+	return servo;
 }
 
 void servo_destroy(struct servo *servo)
@@ -39,7 +72,14 @@ double servo_sample(struct servo *servo,
 		    uint64_t local_ts,
 		    enum servo_state *state)
 {
-	return servo->sample(servo, offset, local_ts, state);
+	double r;
+
+	r = servo->sample(servo, offset, local_ts, state);
+
+	if (*state != SERVO_UNLOCKED)
+		servo->first_update = 0;
+
+	return r;
 }
 
 void servo_sync_interval(struct servo *servo, double interval)
diff --git a/servo.h b/servo.h
index 7a5d0d3..2439966 100644
--- a/servo.h
+++ b/servo.h
@@ -22,6 +22,30 @@
 
 #include <stdint.h>
 
+/**
+ * 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 servo_step_threshold;
+
+/**
+ * When set to zero, the clock is not stepped on start. When set to a non-zero
+ * value, the value bahaves as a threshold and the clock is stepped on start if
+ * the offset is bigger than the threshold.
+ *
+ * Note that this variable is measured in seconds, and allows fractional values.
+ */
+extern double servo_first_step_threshold;
+
+/**
+ * When set to a non-zero value, this variable sets an additional limit for
+ * the frequency adjustment of the clock. It's in ppb.
+ */
+extern int servo_max_frequency;
+
 /** Opaque type */
 struct servo;
 
diff --git a/servo_private.h b/servo_private.h
index 82e2bf5..6425d1e 100644
--- a/servo_private.h
+++ b/servo_private.h
@@ -22,6 +22,10 @@
 #include "contain.h"
 
 struct servo {
+	double max_frequency;
+	double step_threshold;
+	double first_step_threshold;
+	int first_update;
 
 	void (*destroy)(struct servo *servo);