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