diff --git a/clock.c b/clock.c
index c2d3ca0..02f79bb 100644
--- a/clock.c
+++ b/clock.c
@@ -1712,6 +1712,12 @@ void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx)
stats_add_value(c->stats.delay, tmv_dbl(c->path_delay));
}
+void clock_path_delay_corr(struct clock *c, tmv_t req, tmv_t rx, tmv_t c3)
+{
+ tsproc_up_ts_corr(c->tsproc, req, rx, c3);
+ clock_path_delay(c, req, rx);
+}
+
void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx,
double nrr)
{
@@ -1848,6 +1854,9 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
}
offset = tmv_to_nanoseconds(c->master_offset);
+ if (offset * tmv_sign(c->master_offset) > 10000) {
+ tsproc_dump_state(c->tsproc);
+ }
adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ingress),
weight, &state);
c->servo_state = state;
@@ -1898,6 +1907,12 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
return state;
}
+enum servo_state clock_synchronize_corr(struct clock *c, tmv_t ingress, tmv_t origin, tmv_t c2, tmv_t c1)
+{
+ tsproc_down_ts_corr(c->tsproc, ingress, origin, c1, c2);
+ return clock_synchronize(c, ingress, origin);
+}
+
void clock_sync_interval(struct clock *c, int n)
{
int shift;
diff --git a/clock.h b/clock.h
index 0534f21..17b2e3b 100644
--- a/clock.h
+++ b/clock.h
@@ -228,6 +228,7 @@ struct PortIdentity clock_parent_identity(struct clock *c);
* correction.
*/
void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx);
+void clock_path_delay_corr(struct clock *c, tmv_t req, tmv_t rx, tmv_t c3);
/**
* Provide the estimated peer delay from a slave port.
@@ -331,7 +332,7 @@ int clock_switch_phc(struct clock *c, int phc_index);
*/
enum servo_state clock_synchronize(struct clock *c, tmv_t ingress,
tmv_t origin);
-
+enum servo_state clock_synchronize_corr(struct clock *c, tmv_t ingress, tmv_t origin, tmv_t c2, tmv_t c1);
/**
* Inform a slaved clock about the master's sync interval.
* @param c The clock instance.
diff --git a/port.c b/port.c
index f2b666c..f919127 100644
--- a/port.c
+++ b/port.c
@@ -1263,7 +1263,7 @@ static void port_synchronize(struct port *p,
}
last_state = clock_servo_state(p->clock);
- state = clock_synchronize(p->clock, t2, t1c);
+ state = clock_synchronize_corr(p->clock, t2, t1c, c2, c1);
switch (state) {
case SERVO_UNLOCKED:
port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0);
@@ -2074,7 +2074,7 @@ void process_delay_resp(struct port *p, struct ptp_message *m)
monitor_delay(p->slave_event_monitor, clock_parent_identity(p->clock),
m->header.sequenceId, t3, c3, t4);
- clock_path_delay(p->clock, t3, t4c);
+ clock_path_delay_corr(p->clock, t3, t4c, c3);
TAILQ_REMOVE(&p->delay_req, req, list);
msg_put(req);
diff --git a/tsproc.c b/tsproc.c
index a871049..8af3bdd 100644
--- a/tsproc.c
+++ b/tsproc.c
@@ -39,6 +39,11 @@ struct tsproc {
tmv_t t3;
tmv_t t4;
+ /* Corrections */
+ tmv_t c1; // sync correction
+ tmv_t c2; // follow-up correction
+ tmv_t c3; // delay responce correction
+
/* Current filtered delay */
tmv_t filtered_delay;
int filtered_delay_valid;
@@ -104,12 +109,29 @@ void tsproc_down_ts(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts)
tsp->t2 = local_ts;
}
+void tsproc_down_ts_corr(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts, tmv_t c1, tmv_t c2)
+{
+ tsp->t1 = remote_ts;
+ tsp->t2 = local_ts;
+ tsp->c1 = c1;
+ tsp->c2 = c2;
+ pr_debug("t2 - t1 = %+10" PRId64 " c1 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t2, tsp->t1)), tmv_to_nanoseconds(c1));
+}
+
void tsproc_up_ts(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts)
{
tsp->t3 = local_ts;
tsp->t4 = remote_ts;
}
+void tsproc_up_ts_corr(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts, tmv_t c3)
+{
+ tsp->t3 = local_ts;
+ tsp->t4 = remote_ts;
+ tsp->c3 = c3;
+ pr_debug("t4 - t3 = %+10" PRId64 " c3 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t4, tsp->t3)), tmv_to_nanoseconds(c3));
+}
+
void tsproc_set_clock_rate_ratio(struct tsproc *tsp, double clock_rate_ratio)
{
tsp->clock_rate_ratio = clock_rate_ratio;
@@ -145,6 +167,22 @@ tmv_t get_raw_delay(struct tsproc *tsp)
return delay;
}
+void tsproc_dump_state(struct tsproc *tsp)
+{
+ pr_info("t1 = %+10" PRId64 " t2 = %+10" PRId64, tmv_to_nanoseconds(tsp->t1), tmv_to_nanoseconds(tsp->t2));
+ pr_info("t3 = %+10" PRId64 " t4 = %+10" PRId64, tmv_to_nanoseconds(tsp->t3), tmv_to_nanoseconds(tsp->t4));
+ pr_info("c1 = %+10" PRId64 " c2 = %+10" PRId64, tmv_to_nanoseconds(tsp->c1), tmv_to_nanoseconds(tsp->c2));
+ pr_info("c3 = %+10" PRId64, tmv_to_nanoseconds(tsp->c3));
+
+ pr_info("path delay = (t2 - t3) * rr + (t4 - t1)");
+ pr_info("t2 - t3 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t2, tsp->t3)));
+ pr_info("t4 - t1 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t4, tsp->t1)));
+
+ pr_info("t4 - t3 = %+10" PRId64 " c3 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t4, tsp->t3)), tmv_to_nanoseconds(tsp->c3));
+ pr_info("t2 - t1 = %+10" PRId64 " c1 = %+10" PRId64, tmv_to_nanoseconds(tmv_sub(tsp->t2, tsp->t1)), tmv_to_nanoseconds(tsp->c1));
+
+}
+
int tsproc_update_delay(struct tsproc *tsp, tmv_t *delay)
{
tmv_t raw_delay;
diff --git a/tsproc.h b/tsproc.h
index fdb35a8..8baa0d9 100644
--- a/tsproc.h
+++ b/tsproc.h
@@ -58,6 +58,7 @@ void tsproc_destroy(struct tsproc *tsp);
* @param local_ts The local reception time.
*/
void tsproc_down_ts(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts);
+void tsproc_down_ts_corr(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts, tmv_t c1, tmv_t c2);
/**
* Feed an upstream measurement into a time stamp processor.
@@ -66,6 +67,7 @@ void tsproc_down_ts(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts);
* @param remote_ts The remote reception time.
*/
void tsproc_up_ts(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts);
+void tsproc_up_ts_corr(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts, tmv_t c3);
/**
* Set ratio between remote and local clock frequencies.
@@ -106,5 +108,5 @@ int tsproc_update_offset(struct tsproc *tsp, tmv_t *offset, double *weight);
* 1 to reset everything (e.g. when remote clock changed).
*/
void tsproc_reset(struct tsproc *tsp, int full);
-
+void tsproc_dump_state(struct tsproc *tsp);
#endif