linuxptp-devel

[Linuxptp-devel] [PATCHv3 4/6] servo: add support for weighted samples.

[Miroslav L. <mli...@re...>]

Add weight parameter to the sample function. Samples with smaller weight
are less reliable, they can be ignored by the servo or the adjustments
of the clock can be smaller.

Signed-off-by: Miroslav Lichvar <mli...@re...>
---
 clock.c         | 6 +++---
 linreg.c        | 1 +
 ntpshm.c        | 1 +
 phc2sys.c       | 2 +-
 pi.c            | 1 +
 servo.c         | 3 ++-
 servo.h         | 3 +++
 servo_private.h | 2 +-
 8 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/clock.c b/clock.c
index 71fda50..3350b3d 100644
--- a/clock.c
+++ b/clock.c
@@ -1351,14 +1351,14 @@ int clock_switch_phc(struct clock *c, int phc_index)
 
 enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 {
-	double adj;
+	double adj, weight;
 	enum servo_state state = SERVO_UNLOCKED;
 
 	c->ingress_ts = ingress;
 
 	tsproc_down_ts(c->tsproc, origin, ingress);
 
-	if (tsproc_update_offset(c->tsproc, &c->master_offset, NULL))
+	if (tsproc_update_offset(c->tsproc, &c->master_offset, &weight))
 		return state;
 
 	if (clock_utc_correct(c, ingress))
@@ -1370,7 +1370,7 @@ enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
 		return clock_no_adjust(c, ingress, origin);
 
 	adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset),
-			   tmv_to_nanoseconds(ingress), &state);
+			   tmv_to_nanoseconds(ingress), weight, &state);
 	c->servo_state = state;
 
 	if (c->stats.max_count > 1) {
diff --git a/linreg.c b/linreg.c
index fde604d..3f7fe9a 100644
--- a/linreg.c
+++ b/linreg.c
@@ -209,6 +209,7 @@ static int get_best_size(struct linreg_servo *s)
 static double linreg_sample(struct servo *servo,
 			    int64_t offset,
 			    uint64_t local_ts,
+			    double weight,
 			    enum servo_state *state)
 {
 	struct linreg_servo *s = container_of(servo, struct linreg_servo, servo);
diff --git a/ntpshm.c b/ntpshm.c
index 21a11cf..8b18e2d 100644
--- a/ntpshm.c
+++ b/ntpshm.c
@@ -80,6 +80,7 @@ static void ntpshm_destroy(struct servo *servo)
 static double ntpshm_sample(struct servo *servo,
 			    int64_t offset,
 			    uint64_t local_ts,
+			    double weight,
 			    enum servo_state *state)
 {
 	struct ntpshm_servo *s = container_of(servo, struct ntpshm_servo, servo);
diff --git a/phc2sys.c b/phc2sys.c
index 23993ac..9ff5bf9 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -469,7 +469,7 @@ static void update_clock(struct node *node, struct clock *clock,
 	if (clock->sanity_check && clockcheck_sample(clock->sanity_check, ts))
 		servo_reset(clock->servo);
 
-	ppb = servo_sample(clock->servo, offset, ts, &state);
+	ppb = servo_sample(clock->servo, offset, ts, 1.0, &state);
 	clock->servo_state = state;
 
 	switch (state) {
diff --git a/pi.c b/pi.c
index c8b8587..9c7b148 100644
--- a/pi.c
+++ b/pi.c
@@ -64,6 +64,7 @@ static void pi_destroy(struct servo *servo)
 static double pi_sample(struct servo *servo,
 			int64_t offset,
 			uint64_t local_ts,
+			double weight,
 			enum servo_state *state)
 {
 	struct pi_servo *s = container_of(servo, struct pi_servo, servo);
diff --git a/servo.c b/servo.c
index f200f75..2739ebd 100644
--- a/servo.c
+++ b/servo.c
@@ -78,11 +78,12 @@ void servo_destroy(struct servo *servo)
 double servo_sample(struct servo *servo,
 		    int64_t offset,
 		    uint64_t local_ts,
+		    double weight,
 		    enum servo_state *state)
 {
 	double r;
 
-	r = servo->sample(servo, offset, local_ts, state);
+	r = servo->sample(servo, offset, local_ts, weight, state);
 
 	if (*state != SERVO_UNLOCKED)
 		servo->first_update = 0;
diff --git a/servo.h b/servo.h
index e054501..4be8c23 100644
--- a/servo.h
+++ b/servo.h
@@ -104,12 +104,15 @@ void servo_destroy(struct servo *servo);
  * @param servo     Pointer to a servo obtained via @ref servo_create().
  * @param offset    The estimated clock offset in nanoseconds.
  * @param local_ts  The local time stamp of the sample in nanoseconds.
+ * @param weight    The weight of the sample, larger if more reliable,
+ *                  1.0 is the maximum value.
  * @param state     Returns the servo's state.
  * @return The clock adjustment in parts per billion.
  */
 double servo_sample(struct servo *servo,
 		    int64_t offset,
 		    uint64_t local_ts,
+		    double weight,
 		    enum servo_state *state);
 
 /**
diff --git a/servo_private.h b/servo_private.h
index 9a1a459..b8c3c98 100644
--- a/servo_private.h
+++ b/servo_private.h
@@ -30,7 +30,7 @@ struct servo {
 	void (*destroy)(struct servo *servo);
 
 	double (*sample)(struct servo *servo,
-			 int64_t offset, uint64_t local_ts,
+			 int64_t offset, uint64_t local_ts, double weight,
 			 enum servo_state *state);
 
 	void (*sync_interval)(struct servo *servo, double interval);
-- 
2.1.0

[Linuxptp-devel] [PATCHv3 2/6] Refactor time stamp processing.

[Miroslav L. <mli...@re...>]

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

Signed-off-by: Miroslav Lichvar <mli...@re...>
---
 clock.c  | 100 ++++++++++++++-------------------------
 clock.h  |   5 +-
 makefile |   2 +-
 port.c   |  39 ++++++++-------
 tsproc.c | 163 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tsproc.h |  97 +++++++++++++++++++++++++++++++++++++
 6 files changed, 320 insertions(+), 86 deletions(-)
 create mode 100644 tsproc.c
 create mode 100644 tsproc.h

diff --git a/clock.c b/clock.c
index f5349b9..9735fbd 100644
--- a/clock.c
+++ b/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->origin1 = c->t1;
+		f->ingress1 = ingress;
+		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)) /
-		tmv_dbl(tmv_sub(c->t2, f->ingress1));
+	ratio = tmv_dbl(tmv_sub(origin, f->origin1)) /
+		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->origin1 = c->t1;
+	f->ingress1 = ingress;
+	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,
-					dds->delay_filter_length);
-	if (!c->delay_filter) {
-		pr_err("Failed to create delay filter");
+	c->tsproc = tsproc_create(dds->delay_filter, dds->delay_filter_length);
+	if (!c->tsproc) {
+		pr_err("Failed to create time stamp processor");
 		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;
-	double rr;
+	tsproc_up_ts(c->tsproc, req, rx);
 
-	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->t2 = ingress;
+	c->ingress_ts = ingress;
 
-	/*
-	 * c->master_offset = ingress - origin - c->path_delay;
-	 */
-	c->master_offset = tmv_sub(ingress, tmv_add(origin, c->path_delay));
+	tsproc_down_ts(c->tsproc, origin, ingress);
 
-	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->t2 = tmv_zero();
+		c->ingress_ts = 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);
-		c->t1 = tmv_zero();
-		c->t2 = tmv_zero();
+		tsproc_reset(c->tsproc, 1);
+		c->ingress_ts = tmv_zero();
 		c->path_delay = 0;
 		c->nrr = 1.0;
 		fresh_best = 1;
diff --git a/clock.h b/clock.h
index b6df7a9..a8286dd 100644
--- a/clock.h
+++ b/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.
diff --git a/makefile b/makefile
index 5469301..ca82f5e 100644
--- a/makefile
+++ b/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
diff --git a/port.c b/port.c
index 0765746..7bbcffc 100644
--- a/port.c
+++ b/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) *
-			tmv_dbl(tmv_sub(t4, t1));
-	pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3c, t2));
-	pd = tmv_div(pd, 2);
-
-	p->peer_delay = filter_sample(p->delay_filter, pd);
+	tsproc_set_clock_rate_ratio(p->tsproc, p->nrate.ratio *
+				    clock_rate_ratio(p->clock));
+	tsproc_up_ts(p->tsproc, t1, t2);
+	tsproc_down_ts(p->tsproc, t3c, t4);
+	if (tsproc_update_delay(p->tsproc, &p->peer_delay))
+		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,
-					interface->delay_filter_length);
-	if (!p->delay_filter) {
-		pr_err("Failed to create delay filter");
+	p->tsproc = tsproc_create(interface->delay_filter,
+				  interface->delay_filter_length);
+	if (!p->tsproc) {
+		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:
-	filter_destroy(p->delay_filter);
+err_tsproc:
+	tsproc_destroy(p->tsproc);
 err_transport:
 	transport_destroy(p->trp);
 err_port:
diff --git a/tsproc.c b/tsproc.c
new file mode 100644
index 0000000..37be08a
--- /dev/null
+++ b/tsproc.c
@@ -0,0 +1,163 @@
+/**
+ * @file tsproc.c
+ * @note Copyright (C) 2015 Miroslav Lichvar <mli...@re...>
+ *
+ * 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);
+	}
+}
diff --git a/tsproc.h b/tsproc.h
new file mode 100644
index 0000000..6b0b1b5
--- /dev/null
+++ b/tsproc.h
@@ -0,0 +1,97 @@
+/**
+ * @file tsproc.h
+ * @brief Time stamp processor.
+ * @note Copyright (C) 2015 Miroslav Lichvar <mli...@re...>
+ *
+ * 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
-- 
2.1.0

[Linuxptp-devel] [PATCHv3 6/6] pi: use sample weight.

[Miroslav L. <mli...@re...>]

Signed-off-by: Miroslav Lichvar <mli...@re...>
---
 pi.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/pi.c b/pi.c
index 9c7b148..e0116fe 100644
--- a/pi.c
+++ b/pi.c
@@ -137,8 +137,8 @@ static double pi_sample(struct servo *servo,
 			break;
 		}
 
-		ki_term = s->ki * offset;
-		ppb = s->kp * offset + s->drift + ki_term;
+		ki_term = s->ki * offset * weight;
+		ppb = s->kp * offset * weight + s->drift + ki_term;
 		if (ppb < -servo->max_frequency) {
 			ppb = -servo->max_frequency;
 		} else if (ppb > servo->max_frequency) {
-- 
2.1.0

Re: [Linuxptp-devel] [PATCHv3 0/6] Improve accuracy with software timestamping

[Richard C. <ric...@gm...>]

On Thu, Mar 26, 2015 at 04:32:11PM +0100, Miroslav Lichvar wrote:
> Changes since v2:
> - fix swapped origin/ingress timestamps in port_nrate_calculate()
> - use one field per line in tsproc struct
> - use tmv_is_zero()
> - don't allow NULL offset in tsproc_update_offset()
> 
> Changes since v1:
> - include the changes to refactor time stamp processing
> - raw and weighting modes are configurable (disabled by default)
> 
> This adds new time stamp processing modes that are suitable with
> software time stamping.

Series applied.

Thanks,
Richard

Re: [Linuxptp-devel] [PATCHv3 0/6] Improve accuracy with software timestamping

[Miroslav L. <mli...@re...>]

On Mon, Mar 30, 2015 at 11:07:16AM +0200, Richard Cochran wrote:
> On Thu, Mar 26, 2015 at 04:32:11PM +0100, Miroslav Lichvar wrote:
> > This adds new time stamp processing modes that are suitable with
> > software time stamping.
> 
> Series applied.

Thanks!

-- 
Miroslav Lichvar