Convert and correct time stamps early.
Convert time stamps to tmv_t and apply all corrections before passing them to clock/port functions to reduce the number of parameters. Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>master
parent
09e5e6e934
commit
6813fd7c3d
56
clock.c
56
clock.c
|
@ -106,8 +106,6 @@ struct clock {
|
||||||
struct freq_estimator fest;
|
struct freq_estimator fest;
|
||||||
struct time_status_np status;
|
struct time_status_np status;
|
||||||
double nrr;
|
double nrr;
|
||||||
tmv_t c1;
|
|
||||||
tmv_t c2;
|
|
||||||
tmv_t t1;
|
tmv_t t1;
|
||||||
tmv_t t2;
|
tmv_t t2;
|
||||||
struct clock_description desc;
|
struct clock_description desc;
|
||||||
|
@ -549,16 +547,9 @@ static enum servo_state clock_no_adjust(struct clock *c)
|
||||||
{
|
{
|
||||||
double fui;
|
double fui;
|
||||||
double ratio, freq;
|
double ratio, freq;
|
||||||
tmv_t origin2;
|
|
||||||
struct freq_estimator *f = &c->fest;
|
struct freq_estimator *f = &c->fest;
|
||||||
enum servo_state state = SERVO_UNLOCKED;
|
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
|
* The ratio of the local clock freqency to the master clock
|
||||||
* is estimated by:
|
* is estimated by:
|
||||||
*
|
*
|
||||||
|
@ -571,7 +562,7 @@ static enum servo_state clock_no_adjust(struct clock *c)
|
||||||
*/
|
*/
|
||||||
if (!f->ingress1) {
|
if (!f->ingress1) {
|
||||||
f->ingress1 = c->t2;
|
f->ingress1 = c->t2;
|
||||||
f->origin1 = tmv_add(c->t1, tmv_add(c->c1, c->c2));
|
f->origin1 = c->t1;
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
f->count++;
|
f->count++;
|
||||||
|
@ -582,12 +573,8 @@ static enum servo_state clock_no_adjust(struct clock *c)
|
||||||
pr_warning("bad timestamps in rate ratio calculation");
|
pr_warning("bad timestamps in rate ratio calculation");
|
||||||
return state;
|
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));
|
tmv_dbl(tmv_sub(c->t2, f->ingress1));
|
||||||
freq = (1.0 - ratio) * 1e9;
|
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));
|
pr_debug("diff %+.9f", ratio - (fui + c->nrr - 1.0));
|
||||||
|
|
||||||
f->ingress1 = c->t2;
|
f->ingress1 = c->t2;
|
||||||
f->origin1 = origin2;
|
f->origin1 = c->t1;
|
||||||
f->count = 0;
|
f->count = 0;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
|
@ -1289,27 +1276,22 @@ int clock_poll(struct clock *c)
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
|
void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx)
|
||||||
Integer64 correction)
|
|
||||||
{
|
{
|
||||||
tmv_t c1, c2, c3, pd, t1, t2, t3, t4;
|
tmv_t pd, t1, t2, t3, t4;
|
||||||
double rr;
|
double rr;
|
||||||
|
|
||||||
if (tmv_is_zero(c->t1))
|
if (tmv_is_zero(c->t1))
|
||||||
return;
|
return;
|
||||||
|
|
||||||
c1 = c->c1;
|
|
||||||
c2 = c->c2;
|
|
||||||
c3 = correction_to_tmv(correction);
|
|
||||||
t1 = c->t1;
|
t1 = c->t1;
|
||||||
t2 = c->t2;
|
t2 = c->t2;
|
||||||
t3 = timespec_to_tmv(req);
|
t3 = req;
|
||||||
t4 = timestamp_to_tmv(rx);
|
t4 = rx;
|
||||||
rr = clock_rate_ratio(c);
|
rr = clock_rate_ratio(c);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* c->path_delay = (t2 - t3) * rr + (t4 - t1);
|
* c->path_delay = (t2 - t3) * rr + (t4 - t1);
|
||||||
* c->path_delay -= c_sync + c_fup + c_delay_resp;
|
|
||||||
* c->path_delay /= 2.0;
|
* 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)
|
if (rr != 1.0)
|
||||||
pd = dbl_tmv(tmv_dbl(pd) * rr);
|
pd = dbl_tmv(tmv_dbl(pd) * rr);
|
||||||
pd = tmv_add(pd, tmv_sub(t4, t1));
|
pd = tmv_add(pd, tmv_sub(t4, t1));
|
||||||
pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3)));
|
|
||||||
pd = tmv_div(pd, 2);
|
pd = tmv_div(pd, 2);
|
||||||
|
|
||||||
if (pd < 0) {
|
if (pd < 0) {
|
||||||
pr_debug("negative path delay %10" PRId64, pd);
|
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("t2 - t3 = %+10" PRId64, t2 - t3);
|
||||||
pr_debug("t4 - t1 = %+10" PRId64, t4 - t1);
|
pr_debug("t4 - t1 = %+10" PRId64, t4 - t1);
|
||||||
pr_debug("rr = %.9f", rr);
|
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);
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
enum servo_state clock_synchronize(struct clock *c,
|
enum servo_state clock_synchronize(struct clock *c, tmv_t ingress, tmv_t origin)
|
||||||
struct timespec ingress_ts,
|
|
||||||
struct timestamp origin_ts,
|
|
||||||
Integer64 correction1,
|
|
||||||
Integer64 correction2)
|
|
||||||
{
|
{
|
||||||
double adj;
|
double adj;
|
||||||
tmv_t ingress, origin;
|
|
||||||
enum servo_state state = SERVO_UNLOCKED;
|
enum servo_state state = SERVO_UNLOCKED;
|
||||||
|
|
||||||
ingress = timespec_to_tmv(ingress_ts);
|
|
||||||
origin = timestamp_to_tmv(origin_ts);
|
|
||||||
|
|
||||||
c->t1 = origin;
|
c->t1 = origin;
|
||||||
c->t2 = ingress;
|
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,
|
c->master_offset = tmv_sub(ingress, tmv_add(origin, c->path_delay));
|
||||||
tmv_add(origin, tmv_add(c->path_delay, tmv_add(c->c1, c->c2))));
|
|
||||||
|
|
||||||
if (!c->path_delay)
|
if (!c->path_delay)
|
||||||
return state;
|
return state;
|
||||||
|
|
19
clock.h
19
clock.h
|
@ -169,11 +169,10 @@ struct PortIdentity clock_parent_identity(struct clock *c);
|
||||||
* @param c The clock instance.
|
* @param c The clock instance.
|
||||||
* @param req The transmission time of the delay request message.
|
* @param req The transmission time of the delay request message.
|
||||||
* @param rx The reception time of the delay request message,
|
* @param rx The reception time of the delay request message,
|
||||||
* as reported in the delay response message.
|
* as reported in the delay response message, including
|
||||||
* @param correction The correction field from the delay response message.
|
* correction.
|
||||||
*/
|
*/
|
||||||
void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
|
void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx);
|
||||||
Integer64 correction);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Provide the estimated peer delay from a slave port.
|
* 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.
|
* Provide a data point to synchronize the clock.
|
||||||
* @param c The clock instance to synchronize.
|
* @param c The clock instance to synchronize.
|
||||||
* @param ingress_ts The ingress time stamp on the sync message.
|
* @param ingress The ingress time stamp on the sync message.
|
||||||
* @param origin_ts The reported transmission time of 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 correction1 The correction field of the sync message.
|
||||||
* @param correction2 The correction field of the follow up message.
|
* @param correction2 The correction field of the follow up message.
|
||||||
* Pass zero in the case of one step operation.
|
* Pass zero in the case of one step operation.
|
||||||
* @return The state of the clock's servo.
|
* @return The state of the clock's servo.
|
||||||
*/
|
*/
|
||||||
enum servo_state clock_synchronize(struct clock *c,
|
enum servo_state clock_synchronize(struct clock *c, tmv_t ingress,
|
||||||
struct timespec ingress_ts,
|
tmv_t origin);
|
||||||
struct timestamp origin_ts,
|
|
||||||
Integer64 correction1,
|
|
||||||
Integer64 correction2);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Inform a slaved clock about the master's sync interval.
|
* Inform a slaved clock about the master's sync interval.
|
||||||
|
|
46
port.c
46
port.c
|
@ -887,9 +887,8 @@ static int port_management_set(struct port *target,
|
||||||
return respond ? 1 : 0;
|
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;
|
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;
|
p->pdr_missing = 0;
|
||||||
|
|
||||||
if (!n->ingress1) {
|
if (!n->ingress1) {
|
||||||
n->ingress1 = t4;
|
n->ingress1 = ingress;
|
||||||
n->origin1 = tmv_add(t3, c);
|
n->origin1 = origin;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
n->count++;
|
n->count++;
|
||||||
if (n->count < n->max_count) {
|
if (n->count < n->max_count) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
origin2 = tmv_add(t3, c);
|
if (tmv_eq(ingress, n->ingress1)) {
|
||||||
if (tmv_eq(t4, n->ingress1)) {
|
|
||||||
pr_warning("bad timestamps in nrate calculation");
|
pr_warning("bad timestamps in nrate calculation");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
n->ratio =
|
n->ratio =
|
||||||
tmv_dbl(tmv_sub(origin2, n->origin1)) /
|
tmv_dbl(tmv_sub(origin, n->origin1)) /
|
||||||
tmv_dbl(tmv_sub(t4, n->ingress1));
|
tmv_dbl(tmv_sub(ingress, n->ingress1));
|
||||||
n->ingress1 = t4;
|
n->ingress1 = ingress;
|
||||||
n->origin1 = origin2;
|
n->origin1 = origin;
|
||||||
n->count = 0;
|
n->count = 0;
|
||||||
n->ratio_valid = 1;
|
n->ratio_valid = 1;
|
||||||
}
|
}
|
||||||
|
@ -1012,11 +1010,17 @@ static void port_synchronize(struct port *p,
|
||||||
Integer64 correction1, Integer64 correction2)
|
Integer64 correction1, Integer64 correction2)
|
||||||
{
|
{
|
||||||
enum servo_state state;
|
enum servo_state state;
|
||||||
|
tmv_t t1, t1c, t2, c1, c2;
|
||||||
|
|
||||||
port_set_sync_rx_tmo(p);
|
port_set_sync_rx_tmo(p);
|
||||||
|
|
||||||
state = clock_synchronize(p->clock, ingress_ts, origin_ts,
|
t1 = timestamp_to_tmv(origin_ts);
|
||||||
correction1, correction2);
|
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) {
|
switch (state) {
|
||||||
case SERVO_UNLOCKED:
|
case SERVO_UNLOCKED:
|
||||||
port_dispatch(p, EV_SYNCHRONIZATION_FAULT, 0);
|
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_req_msg *req;
|
||||||
struct delay_resp_msg *rsp = &m->delay_resp;
|
struct delay_resp_msg *rsp = &m->delay_resp;
|
||||||
struct PortIdentity master;
|
struct PortIdentity master;
|
||||||
|
tmv_t c3, t3, t4, t4c;
|
||||||
|
|
||||||
if (!p->delay_req)
|
if (!p->delay_req)
|
||||||
return;
|
return;
|
||||||
|
@ -1639,8 +1644,12 @@ static void process_delay_resp(struct port *p, struct ptp_message *m)
|
||||||
if (!pid_eq(&master, &m->header.sourcePortIdentity))
|
if (!pid_eq(&master, &m->header.sourcePortIdentity))
|
||||||
return;
|
return;
|
||||||
|
|
||||||
clock_path_delay(p->clock, p->delay_req->hwts.ts, m->ts.pdu,
|
c3 = correction_to_tmv(m->header.correction);
|
||||||
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) {
|
if (p->logMinDelayReqInterval != rsp->hdr.logMessageInterval) {
|
||||||
// TODO - validate the input.
|
// TODO - validate the input.
|
||||||
|
@ -1789,7 +1798,7 @@ out:
|
||||||
|
|
||||||
static void port_peer_delay(struct port *p)
|
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 *req = p->peer_delay_req;
|
||||||
struct ptp_message *rsp = p->peer_delay_resp;
|
struct ptp_message *rsp = p->peer_delay_resp;
|
||||||
struct ptp_message *fup = p->peer_delay_fup;
|
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);
|
t3 = timestamp_to_tmv(fup->ts.pdu);
|
||||||
c2 = correction_to_tmv(fup->header.correction);
|
c2 = correction_to_tmv(fup->header.correction);
|
||||||
calc:
|
calc:
|
||||||
|
t3c = tmv_add(t3, tmv_add(c1, c2));
|
||||||
adj_t41 = p->nrate.ratio * clock_rate_ratio(p->clock) *
|
adj_t41 = p->nrate.ratio * clock_rate_ratio(p->clock) *
|
||||||
tmv_dbl(tmv_sub(t4, t1));
|
tmv_dbl(tmv_sub(t4, t1));
|
||||||
pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3, t2));
|
pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3c, t2));
|
||||||
pd = tmv_sub(pd, c1);
|
|
||||||
pd = tmv_sub(pd, c2);
|
|
||||||
pd = tmv_div(pd, 2);
|
pd = tmv_div(pd, 2);
|
||||||
|
|
||||||
p->peer_delay = filter_sample(p->delay_filter, pd);
|
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);
|
pr_debug("pdelay %hu %10" PRId64 "%10" PRId64, portnum(p), p->peer_delay, pd);
|
||||||
|
|
||||||
if (p->pod.follow_up_info)
|
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) {
|
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, p->nrate.ratio);
|
||||||
|
|
Loading…
Reference in New Issue