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