Include clock rate ratio in delay calculation.

With the new linreg servo the frequency offset and time offset are
controlled separately. The ratio between master's frequency and the
current frequency of the local clock is known and can be used when
calculating delay or peer delay to improve their accuracy.

This greatly improves the stability of the delay when the servo is
correcting a large offset.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>

clock.c

@@ -977,6 +977,7 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
 		      Integer64 correction)
 {
 	tmv_t c1, c2, c3, pd, t1, t2, t3, t4;
+	double rr;
 
 	if (tmv_is_zero(c->t1))
 		return;
@@ -988,21 +989,27 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
 	t2 = c->t2;
 	t3 = timespec_to_tmv(req);
 	t4 = timestamp_to_tmv(rx);
+	rr = clock_rate_ratio(c);
 
 	/*
-	 * c->path_delay = (t2 - t3) + (t4 - t1);
+	 * c->path_delay = (t2 - t3) * rr + (t4 - t1);
 	 * c->path_delay -= c_sync + c_fup + c_delay_resp;
 	 * c->path_delay /= 2.0;
 	 */
-	pd = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1));
+
+	pd = tmv_sub(t2, t3);
+	if (rr != 1.0)
+		pd = dbl_tmv(tmv_dbl(pd) * rr);
+	pd = tmv_add(pd, tmv_sub(t4, t1));
 	pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3)));
 	pd = tmv_div(pd, 2);
 
 	if (pd < 0) {
 		pr_debug("negative path delay %10" PRId64, pd);
-		pr_debug("path_delay = (t2 - t3) + (t4 - t1) - (c1 + c2 + c3)");
+		pr_debug("path_delay = (t2 - t3) * rr + (t4 - t1) - (c1 + c2 + c3)");
 		pr_debug("t2 - t3 = %+10" PRId64, t2 - t3);
 		pr_debug("t4 - t1 = %+10" PRId64, t4 - t1);
+		pr_debug("rr = %.9f", rr);
 		pr_debug("c1 %10" PRId64, c1);
 		pr_debug("c2 %10" PRId64, c2);
 		pr_debug("c3 %10" PRId64, c3);
@@ -1246,3 +1253,8 @@ void clock_check_ts(struct clock *c, struct timespec ts)
 		servo_reset(c->servo);
 	}
 }
+
+double clock_rate_ratio(struct clock *c)
+{
+	return servo_rate_ratio(c->servo);
+}

clock.h

@@ -253,4 +253,11 @@ int clock_num_ports(struct clock *c);
  */
 void clock_check_ts(struct clock *c, struct timespec ts);
 
+/**
+ * Obtain ratio between master's frequency and current clock frequency.
+ * @param c  The clock instance.
+ * @return   The rate ratio, 1.0 is returned when not known.
+ */
+double clock_rate_ratio(struct clock *c);
+
 #endif

linreg.c

@@ -74,6 +74,8 @@ struct linreg_servo {
 	double clock_freq;
 	/* Expected interval between updates */
 	double update_interval;
+	/* Current ratio between remote and local frequency */
+	double frequency_ratio;
 };
 
 static void linreg_destroy(struct servo *servo)
@@ -269,6 +271,8 @@ static double linreg_sample(struct servo *servo,
 	else if (s->clock_freq < -servo->max_frequency)
 		s->clock_freq = -servo->max_frequency;
 
+	s->frequency_ratio = res->slope / (1.0 + s->clock_freq / 1e9);
+
 	return -s->clock_freq;
 }
 
@@ -286,6 +290,7 @@ static void linreg_reset(struct servo *servo)
 
 	s->num_points = 0;
 	s->last_update = 0;
+	s->frequency_ratio = 1.0;
 
 	for (i = MIN_SIZE; i < MAX_SIZE; i++) {
 		s->results[i - MIN_SIZE].slope = 0.0;
@@ -293,6 +298,13 @@ static void linreg_reset(struct servo *servo)
 	}
 }
 
+static double linreg_rate_ratio(struct servo *servo)
+{
+	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
+
+	return s->frequency_ratio;
+}
+
 struct servo *linreg_servo_create(int fadj)
 {
 	struct linreg_servo *s;
@@ -305,8 +317,10 @@ struct servo *linreg_servo_create(int fadj)
 	s->servo.sample = linreg_sample;
 	s->servo.sync_interval = linreg_sync_interval;
 	s->servo.reset = linreg_reset;
+	s->servo.rate_ratio = linreg_rate_ratio;
 
 	s->clock_freq = -fadj;
+	s->frequency_ratio = 1.0;
 
 	return &s->servo;
 }

port.c

@@ -1795,7 +1795,8 @@ static void port_peer_delay(struct port *p)
 	t3 = timestamp_to_tmv(fup->ts.pdu);
 	c2 = correction_to_tmv(fup->header.correction);
 calc:
-	adj_t41 = p->nrate.ratio * tmv_dbl(tmv_sub(t4, t1));
+	adj_t41 = p->nrate.ratio * clock_rate_ratio(p->clock) *
+			tmv_dbl(tmv_sub(t4, t1));
 	pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3, t2));
 	pd = tmv_sub(pd, c1);
 	pd = tmv_sub(pd, c2);

servo.c

@@ -95,3 +95,11 @@ void servo_reset(struct servo *servo)
 {
 	servo->reset(servo);
 }
+
+double servo_rate_ratio(struct servo *servo)
+{
+	if (servo->rate_ratio)
+		return servo->rate_ratio(servo);
+
+	return 1.0;
+}

servo.h

@@ -124,4 +124,11 @@ void servo_sync_interval(struct servo *servo, double interval);
  */
 void servo_reset(struct servo *servo);
 
+/**
+ * Obtain ratio between master's frequency and current servo frequency.
+ * @param servo   Pointer to a servo obtained via @ref servo_create().
+ * @return   The rate ratio, 1.0 is returned when not known.
+ */
+double servo_rate_ratio(struct servo *servo);
+
 #endif

servo_private.h

@@ -36,6 +36,8 @@ struct servo {
 	void (*sync_interval)(struct servo *servo, double interval);
 
 	void (*reset)(struct servo *servo);
+
+	double (*rate_ratio)(struct servo *servo);
 };
 
 #endif