Introduce a time stamp processor for offset/delay calculations and use
it in the clock and port modules.
Signed-off-by: Miroslav Lichvar <***@redhat.com>
---
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 <***@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);
+ }
+}
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 <***@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
--
2.1.0