From c452e862dd929936131d3f1f7d15072674f305ca Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Thu, 26 Mar 2015 16:32:13 +0100 Subject: [PATCH] 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 --- 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 + * + * 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 +#include + +#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 + * + * 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