From 61de819d563157e05d095d52903b8577811f0d79 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 13 Mar 2014 18:34:19 +0100
Subject: [PATCH] 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         | 18 +++++++++++++++---
 clock.h         |  7 +++++++
 linreg.c        | 14 ++++++++++++++
 port.c          |  3 ++-
 servo.c         |  8 ++++++++
 servo.h         |  7 +++++++
 servo_private.h |  2 ++
 7 files changed, 55 insertions(+), 4 deletions(-)

diff --git a/clock.c b/clock.c
index 7ef2852..3aa2407 100644
--- a/clock.c
+++ b/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);
+}
diff --git a/clock.h b/clock.h
index 52d50b2..804640b 100644
--- a/clock.h
+++ b/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
diff --git a/linreg.c b/linreg.c
index add81e4..4513dd1 100644
--- a/linreg.c
+++ b/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;
 }
diff --git a/port.c b/port.c
index 96a7eb2..1505aca 100644
--- a/port.c
+++ b/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);
diff --git a/servo.c b/servo.c
index 5b8b0ed..41e5c9f 100644
--- a/servo.c
+++ b/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;
+}
diff --git a/servo.h b/servo.h
index 7346167..1536c9e 100644
--- a/servo.h
+++ b/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
diff --git a/servo_private.h b/servo_private.h
index 6425d1e..ebb7e3b 100644
--- a/servo_private.h
+++ b/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