Refactor time stamp processing.

Introduce a time stamp processor for offset/delay calculations and use
it in the clock and port modules.

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

clock.c

@@ -38,6 +38,7 @@
 #include "stats.h"
 #include "print.h"
 #include "tlv.h"
+#include "tsproc.h"
 #include "uds.h"
 #include "util.h"
 
@@ -102,12 +103,11 @@ struct clock {
 	enum servo_state servo_state;
 	tmv_t master_offset;
 	tmv_t path_delay;
-	struct filter *delay_filter;
+	tmv_t ingress_ts;
+	struct tsproc *tsproc;
 	struct freq_estimator fest;
 	struct time_status_np status;
 	double nrr;
-	tmv_t t1;
-	tmv_t t2;
 	struct clock_description desc;
 	struct clock_stats stats;
 	int stats_interval;
@@ -271,7 +271,7 @@ void clock_destroy(struct clock *c)
 		phc_close(c->clkid);
 	}
 	servo_destroy(c->servo);
-	filter_destroy(c->delay_filter);
+	tsproc_destroy(c->tsproc);
 	stats_destroy(c->stats.offset);
 	stats_destroy(c->stats.freq);
 	stats_destroy(c->stats.delay);
@@ -413,7 +413,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 	case TLV_TIME_STATUS_NP:
 		tsn = (struct time_status_np *) tlv->data;
 		tsn->master_offset = c->master_offset;
-		tsn->ingress_time = tmv_to_nanoseconds(c->t2);
+		tsn->ingress_time = tmv_to_nanoseconds(c->ingress_ts);
 		tsn->cumulativeScaledRateOffset =
 			(Integer32) (c->status.cumulativeScaledRateOffset +
 				      c->nrr * POW2_41 - POW2_41);
@@ -543,7 +543,8 @@ static void clock_stats_update(struct clock_stats *s,
 	stats_reset(s->delay);
 }
 
-static enum servo_state clock_no_adjust(struct clock *c)
+static enum servo_state clock_no_adjust(struct clock *c, tmv_t ingress,
+					tmv_t origin)
 {
 	double fui;
 	double ratio, freq;
@@ -561,21 +562,21 @@ static enum servo_state clock_no_adjust(struct clock *c)
 	 * error caused by our imperfect path delay measurement.
 	 */
 	if (!f->ingress1) {
-		f->ingress1 = c->t2;
+		f->ingress1 = ingress;
-		f->origin1 = c->t1;
+		f->origin1 = origin;
 		return state;
 	}
 	f->count++;
 	if (f->count < f->max_count) {
 		return state;
 	}
-	if (tmv_eq(c->t2, f->ingress1)) {
+	if (tmv_eq(ingress, f->ingress1)) {
 		pr_warning("bad timestamps in rate ratio calculation");
 		return state;
 	}
 
-	ratio = tmv_dbl(tmv_sub(c->t1, f->origin1)) /
+	ratio = tmv_dbl(tmv_sub(origin, f->origin1)) /
-		tmv_dbl(tmv_sub(c->t2, f->ingress1));
+		tmv_dbl(tmv_sub(ingress, f->ingress1));
 	freq = (1.0 - ratio) * 1e9;
 
 	if (c->stats.max_count > 1) {
@@ -597,8 +598,8 @@ static enum servo_state clock_no_adjust(struct clock *c)
 	pr_debug("master/local  %.9f", ratio);
 	pr_debug("diff         %+.9f", ratio - (fui + c->nrr - 1.0));
 
-	f->ingress1 = c->t2;
+	f->ingress1 = ingress;
-	f->origin1 = c->t1;
+	f->origin1 = origin;
 	f->count = 0;
 
 	return state;
@@ -851,10 +852,9 @@ struct clock *clock_create(int phc_index, struct interfaces_head *ifaces,
 	}
 	c->servo_state = SERVO_UNLOCKED;
 	c->servo_type = servo;
-	c->delay_filter = filter_create(dds->delay_filter,
+	c->tsproc = tsproc_create(dds->delay_filter, dds->delay_filter_length);
-					dds->delay_filter_length);
+	if (!c->tsproc) {
-	if (!c->delay_filter) {
+		pr_err("Failed to create time stamp processor");
-		pr_err("Failed to create delay filter");
 		return NULL;
 	}
 	c->nrr = 1.0;
@@ -1278,52 +1278,27 @@ int clock_poll(struct clock *c)
 
 void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx)
 {
-	tmv_t pd, t1, t2, t3, t4;
+	tsproc_up_ts(c->tsproc, req, rx);
-	double rr;
 
-	if (tmv_is_zero(c->t1))
+	if (tsproc_update_delay(c->tsproc, &c->path_delay))
 		return;
 
-	t1 = c->t1;
-	t2 = c->t2;
-	t3 = req;
-	t4 = rx;
-	rr = clock_rate_ratio(c);
-
-	/*
-	 * c->path_delay = (t2 - t3) * rr + (t4 - t1);
-	 * c->path_delay /= 2.0;
-	 */
-
-	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_div(pd, 2);
-
-	if (pd < 0) {
-		pr_debug("negative path delay %10" PRId64, pd);
-		pr_debug("path_delay = (t2 - t3) * rr + (t4 - t1)");
-		pr_debug("t2 - t3 = %+10" PRId64, t2 - t3);
-		pr_debug("t4 - t1 = %+10" PRId64, t4 - t1);
-		pr_debug("rr = %.9f", rr);
-	}
-
-	c->path_delay = filter_sample(c->delay_filter, pd);
-
 	c->cur.meanPathDelay = tmv_to_TimeInterval(c->path_delay);
 
-	pr_debug("path delay    %10" PRId64 " %10" PRId64, c->path_delay, pd);
-
 	if (c->stats.delay)
-		stats_add_value(c->stats.delay, tmv_to_nanoseconds(pd));
+		stats_add_value(c->stats.delay,
+				tmv_to_nanoseconds(c->path_delay));
 }
 
-void clock_peer_delay(struct clock *c, tmv_t ppd, double nrr)
+void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx,
+		      double nrr)
 {
 	c->path_delay = ppd;
 	c->nrr = nrr;
 
+	tsproc_set_delay(c->tsproc, ppd);
+	tsproc_up_ts(c->tsproc, req, rx);
+
 	if (c->stats.delay)
 		stats_add_value(c->stats.delay, tmv_to_nanoseconds(ppd));
 }
@@ -1378,15 +1353,11 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 	double adj;
 	enum servo_state state = SERVO_UNLOCKED;
 
-	c->t1 = origin;
+	c->ingress_ts = ingress;
-	c->t2 = ingress;
 
-	/*
+	tsproc_down_ts(c->tsproc, origin, ingress);
-	 * c->master_offset = ingress - origin - c->path_delay;
-	 */
-	c->master_offset = tmv_sub(ingress, tmv_add(origin, c->path_delay));
 
-	if (!c->path_delay)
+	if (tsproc_update_offset(c->tsproc, &c->master_offset))
 		return state;
 
 	if (clock_utc_correct(c, ingress))
@@ -1395,7 +1366,7 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 	c->cur.offsetFromMaster = tmv_to_TimeInterval(c->master_offset);
 
 	if (c->free_running)
-		return clock_no_adjust(c);
+		return clock_no_adjust(c, ingress, origin);
 
 	adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset),
 			   tmv_to_nanoseconds(ingress), &state);
@@ -1411,19 +1382,21 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 			tmv_to_nanoseconds(c->path_delay));
 	}
 
+	tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c));
+
 	switch (state) {
 	case SERVO_UNLOCKED:
 		break;
 	case SERVO_JUMP:
 		clockadj_set_freq(c->clkid, -adj);
 		clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset));
-		c->t1 = tmv_zero();
+		c->ingress_ts = tmv_zero();
-		c->t2 = tmv_zero();
 		if (c->sanity_check) {
 			clockcheck_set_freq(c->sanity_check, -adj);
 			clockcheck_step(c->sanity_check,
 					-tmv_to_nanoseconds(c->master_offset));
 		}
+		tsproc_reset(c->tsproc, 0);
 		break;
 	case SERVO_LOCKED:
 		clockadj_set_freq(c->clkid, -adj);
@@ -1497,9 +1470,8 @@ static void handle_state_decision_event(struct clock *c)
 
 	if (!cid_eq(&best_id, &c->best_id)) {
 		clock_freq_est_reset(c);
-		filter_reset(c->delay_filter);
+		tsproc_reset(c->tsproc, 1);
-		c->t1 = tmv_zero();
+		c->ingress_ts = tmv_zero();
-		c->t2 = tmv_zero();
 		c->path_delay = 0;
 		c->nrr = 1.0;
 		fresh_best = 1;

clock.h

@@ -178,9 +178,12 @@ void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx);
  * Provide the estimated peer delay from a slave port.
  * @param c           The clock instance.
  * @param ppd         The peer delay as measured on a slave port.
+ * @param req         The transmission time of the pdelay request message.
+ * @param rx          The reception time of the pdelay request message.
  * @param nrr         The neighbor rate ratio as measured on a slave port.
  */
-void clock_peer_delay(struct clock *c, tmv_t ppd, double nrr);
+void clock_peer_delay(struct clock *c, tmv_t ppd, tmv_t req, tmv_t rx,
+		      double nrr);
 
 /**
  * Poll for events and dispatch them.

makefile

@@ -26,7 +26,7 @@ PRG	= ptp4l pmc phc2sys hwstamp_ctl phc_ctl timemaster
 OBJ     = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
  filter.o fsm.o linreg.o mave.o mmedian.o msg.o ntpshm.o phc.o \
  pi.o port.o print.o ptp4l.o raw.o servo.o sk.o stats.o tlv.o \
- transport.o udp.o udp6.o uds.o util.o version.o
+ transport.o tsproc.o udp.o udp6.o uds.o util.o version.o
 
 OBJECTS	= $(OBJ) hwstamp_ctl.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
  sysoff.o timemaster.o

port.c

@@ -35,6 +35,7 @@
 #include "sk.h"
 #include "tlv.h"
 #include "tmv.h"
+#include "tsproc.h"
 #include "util.h"
 
 #define ALLOWED_LOST_RESPONSES 3
@@ -86,7 +87,7 @@ struct port {
 		UInteger16 sync;
 	} seqnum;
 	tmv_t peer_delay;
-	struct filter *delay_filter;
+	struct tsproc *tsproc;
 	int log_sync_interval;
 	struct nrate_estimator nrate;
 	unsigned int pdr_missing;
@@ -1798,11 +1799,10 @@ out:
 
 static void port_peer_delay(struct port *p)
 {
-	tmv_t c1, c2, t1, t2, t3, t3c, t4, pd;
+	tmv_t c1, c2, t1, t2, t3, t3c, t4;
 	struct ptp_message *req = p->peer_delay_req;
 	struct ptp_message *rsp = p->peer_delay_resp;
 	struct ptp_message *fup = p->peer_delay_fup;
-	double adj_t41;
 
 	/* Check for response, validate port and sequence number. */
 
@@ -1847,22 +1847,21 @@ static void port_peer_delay(struct port *p)
 	c2 = correction_to_tmv(fup->header.correction);
 calc:
 	t3c = tmv_add(t3, tmv_add(c1, c2));
-	adj_t41 = p->nrate.ratio * clock_rate_ratio(p->clock) *
+	tsproc_set_clock_rate_ratio(p->tsproc, p->nrate.ratio *
-			tmv_dbl(tmv_sub(t4, t1));
+				    clock_rate_ratio(p->clock));
-	pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3c, t2));
+	tsproc_up_ts(p->tsproc, t1, t2);
-	pd = tmv_div(pd, 2);
+	tsproc_down_ts(p->tsproc, t3c, t4);
-
+	if (tsproc_update_delay(p->tsproc, &p->peer_delay))
-	p->peer_delay = filter_sample(p->delay_filter, pd);
+		return;
 
 	p->peerMeanPathDelay = tmv_to_TimeInterval(p->peer_delay);
 
-	pr_debug("pdelay %hu   %10" PRId64 "%10" PRId64, portnum(p), p->peer_delay, pd);
-
 	if (p->pod.follow_up_info)
 		port_nrate_calculate(p, t3c, t4);
 
 	if (p->state == PS_UNCALIBRATED || p->state == PS_SLAVE) {
-		clock_peer_delay(p->clock, p->peer_delay, p->nrate.ratio);
+		clock_peer_delay(p->clock, p->peer_delay, t1, t2,
+				 p->nrate.ratio);
 	}
 
 	msg_put(p->peer_delay_req);
@@ -1981,7 +1980,7 @@ void port_close(struct port *p)
 		port_disable(p);
 	}
 	transport_destroy(p->trp);
-	filter_destroy(p->delay_filter);
+	tsproc_destroy(p->tsproc);
 	if (p->fault_fd >= 0)
 		close(p->fault_fd);
 	free(p);
@@ -2535,10 +2534,10 @@ struct port *port_open(int phc_index,
 	p->delayMechanism = interface->dm;
 	p->versionNumber = PTP_VERSION;
 
-	p->delay_filter = filter_create(interface->delay_filter,
+	p->tsproc = tsproc_create(interface->delay_filter,
-					interface->delay_filter_length);
+				  interface->delay_filter_length);
-	if (!p->delay_filter) {
+	if (!p->tsproc) {
-		pr_err("Failed to create delay filter");
+		pr_err("Failed to create time stamp processor");
 		goto err_transport;
 	}
 	p->nrate.ratio = 1.0;
@@ -2549,13 +2548,13 @@ struct port *port_open(int phc_index,
 		p->fault_fd = timerfd_create(CLOCK_MONOTONIC, 0);
 		if (p->fault_fd < 0) {
 			pr_err("timerfd_create failed: %m");
-			goto err_filter;
+			goto err_tsproc;
 		}
 	}
 	return p;
 
-err_filter:
+err_tsproc:
-	filter_destroy(p->delay_filter);
+	tsproc_destroy(p->tsproc);
 err_transport:
 	transport_destroy(p->trp);
 err_port:

tsproc.c

@@ -0,0 +1,163 @@
+/**
+ * @file tsproc.c
+ * @note Copyright (C) 2015 Miroslav Lichvar <mlichvar@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <stdlib.h>
+#include <inttypes.h>
+
+#include "tsproc.h"
+#include "filter.h"
+#include "print.h"
+
+struct tsproc {
+	/* Current ratio between remote and local clock frequency */
+	double clock_rate_ratio;
+
+	/* Latest down measurement */
+	tmv_t t1;
+	tmv_t t2;
+
+	/* Latest up measurement */
+	tmv_t t3;
+	tmv_t t4;
+
+	/* Current filtered delay */
+	tmv_t filtered_delay;
+
+	/* Delay filter */
+	struct filter *delay_filter;
+};
+
+struct tsproc *tsproc_create(enum filter_type delay_filter, int filter_length)
+{
+	struct tsproc *tsp;
+
+	tsp = calloc(1, sizeof(*tsp));
+	if (!tsp)
+		return NULL;
+
+	tsp->delay_filter = filter_create(delay_filter, filter_length);
+	if (!tsp->delay_filter) {
+		free(tsp);
+		return NULL;
+	}
+
+	tsp->clock_rate_ratio = 1.0;
+
+	return tsp;
+}
+
+void tsproc_destroy(struct tsproc *tsp)
+{
+	filter_destroy(tsp->delay_filter);
+	free(tsp);
+}
+
+void tsproc_down_ts(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts)
+{
+	tsp->t1 = remote_ts;
+	tsp->t2 = local_ts;
+}
+
+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_set_clock_rate_ratio(struct tsproc *tsp, double clock_rate_ratio)
+{
+	tsp->clock_rate_ratio = clock_rate_ratio;
+}
+
+void tsproc_set_delay(struct tsproc *tsp, tmv_t delay)
+{
+	tsp->filtered_delay = delay;
+}
+
+tmv_t get_raw_delay(struct tsproc *tsp)
+{
+	tmv_t t23, t41, delay;
+
+	/* delay = ((t2 - t3) * rr + (t4 - t1)) / 2 */
+
+	t23 = tmv_sub(tsp->t2, tsp->t3);
+	if (tsp->clock_rate_ratio != 1.0)
+		t23 = dbl_tmv(tmv_dbl(t23) * tsp->clock_rate_ratio);
+	t41 = tmv_sub(tsp->t4, tsp->t1);
+	delay = tmv_div(tmv_add(t23, t41), 2);
+
+	if (delay < 0) {
+		pr_debug("negative delay %10" PRId64, delay);
+		pr_debug("delay = (t2 - t3) * rr + (t4 - t1)");
+		pr_debug("t2 - t3 = %+10" PRId64, t23);
+		pr_debug("t4 - t1 = %+10" PRId64, t41);
+		pr_debug("rr = %.9f", tsp->clock_rate_ratio);
+	}
+
+	return delay;
+}
+
+int tsproc_update_delay(struct tsproc *tsp, tmv_t *delay)
+{
+	tmv_t raw_delay;
+
+	if (tmv_is_zero(tsp->t1) || tmv_is_zero(tsp->t2) ||
+	    tmv_is_zero(tsp->t3) || tmv_is_zero(tsp->t4))
+		return -1;
+
+	raw_delay = get_raw_delay(tsp);
+	tsp->filtered_delay = filter_sample(tsp->delay_filter, raw_delay);
+
+	pr_debug("delay   filtered %10" PRId64 "   raw %10" PRId64,
+		 tsp->filtered_delay, raw_delay);
+
+	if (delay)
+		*delay = tsp->filtered_delay;
+
+	return 0;
+}
+
+int tsproc_update_offset(struct tsproc *tsp, tmv_t *offset)
+{
+	tmv_t delay;
+
+	if (tmv_is_zero(tsp->t1) || tmv_is_zero(tsp->t2) ||
+	    tmv_is_zero(tsp->t3) || tmv_is_zero(tsp->t4))
+		return -1;
+
+	delay = tsp->filtered_delay;
+
+	/* offset = t2 - t1 - delay */
+	*offset = tmv_sub(tmv_sub(tsp->t2, tsp->t1), delay);
+
+	return 0;
+}
+
+void tsproc_reset(struct tsproc *tsp, int full)
+{
+	tsp->t1 = tmv_zero();
+	tsp->t2 = tmv_zero();
+	tsp->t3 = tmv_zero();
+	tsp->t4 = tmv_zero();
+
+	if (full) {
+		tsp->clock_rate_ratio = 1.0;
+		filter_reset(tsp->delay_filter);
+	}
+}

tsproc.h

@@ -0,0 +1,97 @@
+/**
+ * @file tsproc.h
+ * @brief Time stamp processor.
+ * @note Copyright (C) 2015 Miroslav Lichvar <mlichvar@redhat.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef HAVE_TSPROC_H
+#define HAVE_TSPROC_H
+
+#include "filter.h"
+
+/** Opaque type */
+struct tsproc;
+
+/**
+ * Create a new instance of the time stamp processor.
+ * @param delay_filter   Type of the filter that will be applied to delay.
+ * @param filter_length  Length of the filter.
+ * @return               A pointer to a new tsproc on success, NULL otherwise.
+ */
+struct tsproc *tsproc_create(enum filter_type delay_filter, int filter_length);
+
+/**
+ * Destroy a time stamp processor.
+ * @param tsp       Pointer obtained via @ref tsproc_create().
+ */
+void tsproc_destroy(struct tsproc *tsp);
+
+/**
+ * Feed a downstream measurement into a time stamp processor.
+ * @param tsp       Pointer obtained via @ref tsproc_create().
+ * @param remote_ts The remote transmission time.
+ * @param local_ts  The local reception time.
+ */
+void tsproc_down_ts(struct tsproc *tsp, tmv_t remote_ts, tmv_t local_ts);
+
+/**
+ * Feed an upstream measurement into a time stamp processor.
+ * @param tsp       Pointer obtained via @ref tsproc_create().
+ * @param local_ts  The local transmission time.
+ * @param remote_ts The remote reception time.
+ */
+void tsproc_up_ts(struct tsproc *tsp, tmv_t local_ts, tmv_t remote_ts);
+
+/**
+ * Set ratio between remote and local clock frequencies.
+ * @param tsp               Pointer obtained via @ref tsproc_create().
+ * @param clock_rate_ratio  The ratio between frequencies.
+ */
+void tsproc_set_clock_rate_ratio(struct tsproc *tsp, double clock_rate_ratio);
+
+/**
+ * Set delay in a time stamp processor. This can be used to override the last
+ * calculated value.
+ * @param tsp    Pointer obtained via @ref tsproc_create().
+ * @param delay  The new delay.
+ */
+void tsproc_set_delay(struct tsproc *tsp, tmv_t delay);
+
+/**
+ * Update delay in a time stamp processor using new measurements.
+ * @param tsp    Pointer obtained via @ref tsproc_create().
+ * @param delay  A pointer to store the new delay, may be NULL.
+ * @return       0 on success, -1 when missing a measurement.
+ */
+int tsproc_update_delay(struct tsproc *tsp, tmv_t *delay);
+
+/**
+ * Update offset in a time stamp processor using new measurements.
+ * @param tsp    Pointer obtained via @ref tsproc_create().
+ * @param offset A pointer to store the new offset.
+ * @return       0 on success, -1 when missing a measurement.
+ */
+int tsproc_update_offset(struct tsproc *tsp, tmv_t *offset);
+
+/**
+ * Reset a time stamp processor.
+ * @param tsp    Pointer obtained via @ref tsproc_create().
+ * @param full   0 to reset stored measurements (e.g. after clock was stepped),
+ *               1 to reset everything (e.g. when remote clock changed).
+ */
+void tsproc_reset(struct tsproc *tsp, int full);
+
+#endif