diff --git a/clock.c b/clock.c index b841e81..f5349b9 100644 --- a/clock.c +++ b/clock.c @@ -106,8 +106,6 @@ struct clock { struct freq_estimator fest; struct time_status_np status; double nrr; - tmv_t c1; - tmv_t c2; tmv_t t1; tmv_t t2; struct clock_description desc; @@ -549,16 +547,9 @@ static enum servo_state clock_no_adjust(struct clock *c) { double fui; double ratio, freq; - tmv_t origin2; struct freq_estimator *f = &c->fest; enum servo_state state = SERVO_UNLOCKED; /* - * We have clock.t1 as the origin time stamp, and clock.t2 as - * the ingress. According to the master's clock, the time at - * which the sync arrived is: - * - * origin = origin_ts + path_delay + correction - * * The ratio of the local clock freqency to the master clock * is estimated by: * @@ -571,7 +562,7 @@ static enum servo_state clock_no_adjust(struct clock *c) */ if (!f->ingress1) { f->ingress1 = c->t2; - f->origin1 = tmv_add(c->t1, tmv_add(c->c1, c->c2)); + f->origin1 = c->t1; return state; } f->count++; @@ -582,12 +573,8 @@ static enum servo_state clock_no_adjust(struct clock *c) pr_warning("bad timestamps in rate ratio calculation"); return state; } - /* - * origin2 = c->t1 (+c->path_delay) + c->c1 + c->c2; - */ - origin2 = tmv_add(c->t1, tmv_add(c->c1, c->c2)); - ratio = tmv_dbl(tmv_sub(origin2, f->origin1)) / + ratio = tmv_dbl(tmv_sub(c->t1, f->origin1)) / tmv_dbl(tmv_sub(c->t2, f->ingress1)); freq = (1.0 - ratio) * 1e9; @@ -611,7 +598,7 @@ static enum servo_state clock_no_adjust(struct clock *c) pr_debug("diff %+.9f", ratio - (fui + c->nrr - 1.0)); f->ingress1 = c->t2; - f->origin1 = origin2; + f->origin1 = c->t1; f->count = 0; return state; @@ -1289,27 +1276,22 @@ int clock_poll(struct clock *c) return 0; } -void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, - Integer64 correction) +void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx) { - tmv_t c1, c2, c3, pd, t1, t2, t3, t4; + tmv_t pd, t1, t2, t3, t4; double rr; if (tmv_is_zero(c->t1)) return; - c1 = c->c1; - c2 = c->c2; - c3 = correction_to_tmv(correction); t1 = c->t1; t2 = c->t2; - t3 = timespec_to_tmv(req); - t4 = timestamp_to_tmv(rx); + t3 = req; + t4 = rx; rr = clock_rate_ratio(c); /* * c->path_delay = (t2 - t3) * rr + (t4 - t1); - * c->path_delay -= c_sync + c_fup + c_delay_resp; * c->path_delay /= 2.0; */ @@ -1317,18 +1299,14 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, if (rr != 1.0) pd = dbl_tmv(tmv_dbl(pd) * rr); pd = tmv_add(pd, tmv_sub(t4, t1)); - pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3))); 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) - (c1 + c2 + c3)"); + 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); - pr_debug("c1 %10" PRId64, c1); - pr_debug("c2 %10" PRId64, c2); - pr_debug("c3 %10" PRId64, c3); } c->path_delay = filter_sample(c->delay_filter, pd); @@ -1395,30 +1373,18 @@ int clock_switch_phc(struct clock *c, int phc_index) return 0; } -enum servo_state clock_synchronize(struct clock *c, - struct timespec ingress_ts, - struct timestamp origin_ts, - Integer64 correction1, - Integer64 correction2) +enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin) { double adj; - tmv_t ingress, origin; enum servo_state state = SERVO_UNLOCKED; - ingress = timespec_to_tmv(ingress_ts); - origin = timestamp_to_tmv(origin_ts); - c->t1 = origin; c->t2 = ingress; - c->c1 = correction_to_tmv(correction1); - c->c2 = correction_to_tmv(correction2); - /* - * c->master_offset = ingress - origin - c->path_delay - c->c1 - c->c2; + * c->master_offset = ingress - origin - c->path_delay; */ - c->master_offset = tmv_sub(ingress, - tmv_add(origin, tmv_add(c->path_delay, tmv_add(c->c1, c->c2)))); + c->master_offset = tmv_sub(ingress, tmv_add(origin, c->path_delay)); if (!c->path_delay) return state; diff --git a/clock.h b/clock.h index 4834464..b6df7a9 100644 --- a/clock.h +++ b/clock.h @@ -169,11 +169,10 @@ struct PortIdentity clock_parent_identity(struct clock *c); * @param c The clock instance. * @param req The transmission time of the delay request message. * @param rx The reception time of the delay request message, - * as reported in the delay response message. - * @param correction The correction field from the delay response message. + * as reported in the delay response message, including + * correction. */ -void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx, - Integer64 correction); +void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx); /** * Provide the estimated peer delay from a slave port. @@ -215,18 +214,16 @@ int clock_switch_phc(struct clock *c, int phc_index); /** * Provide a data point to synchronize the clock. * @param c The clock instance to synchronize. - * @param ingress_ts The ingress time stamp on the sync message. - * @param origin_ts The reported transmission time of the sync message. + * @param ingress The ingress time stamp on the sync message. + * @param origin The reported transmission time of the sync message, + including any corrections. * @param correction1 The correction field of the sync message. * @param correction2 The correction field of the follow up message. * Pass zero in the case of one step operation. * @return The state of the clock's servo. */ -enum servo_state clock_synchronize(struct clock *c, - struct timespec ingress_ts, - struct timestamp origin_ts, - Integer64 correction1, - Integer64 correction2); +enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, + tmv_t origin); /** * Inform a slaved clock about the master's sync interval. diff --git a/port.c b/port.c index 18a956d..0765746 100644 --- a/port.c +++ b/port.c @@ -887,9 +887,8 @@ static int port_management_set(struct port *target, return respond ? 1 : 0; } -static void port_nrate_calculate(struct port *p, tmv_t t3, tmv_t t4, tmv_t c) +static void port_nrate_calculate(struct port *p, tmv_t origin, tmv_t ingress) { - tmv_t origin2; struct nrate_estimator *n = &p->nrate; /* @@ -899,24 +898,23 @@ static void port_nrate_calculate(struct port *p, tmv_t t3, tmv_t t4, tmv_t c) p->pdr_missing = 0; if (!n->ingress1) { - n->ingress1 = t4; - n->origin1 = tmv_add(t3, c); + n->ingress1 = ingress; + n->origin1 = origin; return; } n->count++; if (n->count < n->max_count) { return; } - origin2 = tmv_add(t3, c); - if (tmv_eq(t4, n->ingress1)) { + if (tmv_eq(ingress, n->ingress1)) { pr_warning("bad timestamps in nrate calculation"); return; } n->ratio = - tmv_dbl(tmv_sub(origin2, n->origin1)) / - tmv_dbl(tmv_sub(t4, n->ingress1)); - n->ingress1 = t4; - n->origin1 = origin2; + tmv_dbl(tmv_sub(origin, n->origin1)) / + tmv_dbl(tmv_sub(ingress, n->ingress1)); + n->ingress1 = ingress; + n->origin1 = origin; n->count = 0; n->ratio_valid = 1; } @@ -1012,11 +1010,17 @@ static void port_synchronize(struct port *p, Integer64 correction1, Integer64 correction2) { enum servo_state state; + tmv_t t1, t1c, t2, c1, c2; port_set_sync_rx_tmo(p); - state = clock_synchronize(p->clock, ingress_ts, origin_ts, - correction1, correction2); + t1 = timestamp_to_tmv(origin_ts); + t2 = timespec_to_tmv(ingress_ts); + c1 = correction_to_tmv(correction1); + c2 = correction_to_tmv(correction2); + t1c = tmv_add(t1, tmv_add(c1, c2)); + + state = clock_synchronize(p->clock, t2, t1c); switch (state) { case SERVO_UNLOCKED: port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0); @@ -1623,6 +1627,7 @@ static void process_delay_resp(struct port *p, struct ptp_message *m) struct delay_req_msg *req; struct delay_resp_msg *rsp = &m->delay_resp; struct PortIdentity master; + tmv_t c3, t3, t4, t4c; if (!p->delay_req) return; @@ -1639,8 +1644,12 @@ static void process_delay_resp(struct port *p, struct ptp_message *m) if (!pid_eq(&master, &m->header.sourcePortIdentity)) return; - clock_path_delay(p->clock, p->delay_req->hwts.ts, m->ts.pdu, - m->header.correction); + c3 = correction_to_tmv(m->header.correction); + t3 = timespec_to_tmv(p->delay_req->hwts.ts); + t4 = timestamp_to_tmv(m->ts.pdu); + t4c = tmv_sub(t4, c3); + + clock_path_delay(p->clock, t3, t4c); if (p->logMinDelayReqInterval != rsp->hdr.logMessageInterval) { // TODO - validate the input. @@ -1789,7 +1798,7 @@ out: static void port_peer_delay(struct port *p) { - tmv_t c1, c2, t1, t2, t3, t4, pd; + tmv_t c1, c2, t1, t2, t3, t3c, t4, pd; struct ptp_message *req = p->peer_delay_req; struct ptp_message *rsp = p->peer_delay_resp; struct ptp_message *fup = p->peer_delay_fup; @@ -1837,11 +1846,10 @@ static void port_peer_delay(struct port *p) t3 = timestamp_to_tmv(fup->ts.pdu); 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(t3, t2)); - pd = tmv_sub(pd, c1); - pd = tmv_sub(pd, c2); + 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); @@ -1851,7 +1859,7 @@ calc: pr_debug("pdelay %hu %10" PRId64 "%10" PRId64, portnum(p), p->peer_delay, pd); if (p->pod.follow_up_info) - port_nrate_calculate(p, t3, t4, tmv_add(c1, c2)); + 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);