time_operations: add init_servo() method

This function allows a servo to initialize its hardware and return the current "observed drift" value that is in charge. In unix-time this is used to return the current value for frequency correction -- being consistent with current naming and use of values, which unfortunately is not really correct. Signed-off-by: Alessandro Rubini <rubini@gnudd.com>

time_operations: add init_servo() method
This function allows a servo to initialize its hardware and return the current "observed drift" value that is in charge. In unix-time this is used to return the current value for frequency correction -- being consistent with current naming and use of values, which unfortunately is not really correct. Signed-off-by: Alessandro Rubini <rubini@gnudd.com>
6dcafdcf · Alessandro Rubini · 5927cf03 · 6dcafdcf · 6dcafdcf · 6dcafdcf
Commit 6dcafdcf authored Sep 16, 2013 by Alessandro Rubini
Hide whitespace changes
Inline Side-by-side

Showing with 33 additions and 7 deletions

ppsi.h include/ppsi/ppsi.h +1 -0

servo.c proto-standard/servo.c +19 -7

unix-time.c time-unix/unix-time.c +13 -0

No files found.
--- a/include/ppsi/ppsi.h
+++ b/include/ppsi/ppsi.h
@@ -164,6 +164,7 @@ struct pp_time_operations {
 	int (*adjust)(struct pp_instance *ppi, long offset_ns, long freq_ppm);
 	int (*adjust_offset)(struct pp_instance *ppi, long offset_ns);
 	int (*adjust_freq)(struct pp_instance *ppi, long freq_ppm);
+	int (*init_servo)(struct pp_instance *ppi);
 	/* calc_timeout cannot return zero */
 	unsigned long (*calc_timeout)(struct pp_instance *ppi, int millisec);
 };

--- a/proto-standard/servo.c
+++ b/proto-standard/servo.c
@@ -10,17 +10,29 @@

 void pp_servo_init(struct pp_instance *ppi)
 {
-	pp_diag(ppi, servo, 1, "Initializing\n");
+	int d;

-	/* clear vars */
-	SRV(ppi)->obs_drift = 0;	/* clears clock servo accumulator (the
-					 * I term) */
 	SRV(ppi)->owd_fltr.s_exp = 0;	/* clears one-way delay filter */
 	SRV(ppi)->ofm_fltr.s_exp = 0;	/* clears offset-from-master filter */

-	/* level clock */
-	if (!OPTS(ppi)->no_adjust)
-		ppi->t_ops->adjust(ppi, 0, 0);
+
+	if (ppi->t_ops->init_servo) {
+		/* The system may pre-set us to keep current frequency */
+		d = ppi->t_ops->init_servo(ppi);
+		if (d == -1) {
+			pp_diag(ppi, servo, 1, "error in t_ops->servo_init");
+			d = 0;
+		}
+		SRV(ppi)->obs_drift = -d; /* note "-" */
+	} else {
+		/* level clock */
+		if (!OPTS(ppi)->no_adjust)
+			ppi->t_ops->adjust(ppi, 0, 0);
+		SRV(ppi)->obs_drift = 0;
+	}
+
+	pp_diag(ppi, servo, 1, "Initialized: obs_drift %i\n",
+		SRV(ppi)->obs_drift);
 }

 /* internal helper, retuerning static storage to be used immediately */

--- a/time-unix/unix-time.c
+++ b/time-unix/unix-time.c
@@ -45,6 +45,18 @@ static int32_t unix_time_set(struct pp_instance *ppi, TimeInternal *t)
 	return 0;
 }

+static int unix_time_init_servo(struct pp_instance *ppi)
+{
+	struct timex t;
+
+	/* We must set MOD_PLL and recover the current frequency value */
+	t.modes = MOD_STATUS;
+	t.status = STA_PLL;
+	if (adjtimex(&t) < 0)
+		return -1;
+	return (t.freq >> 16) * 1000; /* positive or negative, not -1 */
+}
+
 static int unix_time_adjust(struct pp_instance *ppi, long offset_ns, long freq_ppm)
 {
 	struct timex t;
@@ -103,5 +115,6 @@ struct pp_time_operations unix_time_ops = {
 	.adjust = unix_time_adjust,
 	.adjust_offset = unix_time_adjust_offset,
 	.adjust_freq = unix_time_adjust_freq,
+	.init_servo = unix_time_init_servo,
 	.calc_timeout = unix_calc_timeout,
 };