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 <mlichvar@redhat.com>master
parent
6813fd7c3d
commit
c452e862dd
100
clock.c
100
clock.c
|
@ -38,6 +38,7 @@
|
||||||
#include "stats.h"
|
#include "stats.h"
|
||||||
#include "print.h"
|
#include "print.h"
|
||||||
#include "tlv.h"
|
#include "tlv.h"
|
||||||
|
#include "tsproc.h"
|
||||||
#include "uds.h"
|
#include "uds.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
|
@ -102,12 +103,11 @@ struct clock {
|
||||||
enum servo_state servo_state;
|
enum servo_state servo_state;
|
||||||
tmv_t master_offset;
|
tmv_t master_offset;
|
||||||
tmv_t path_delay;
|
tmv_t path_delay;
|
||||||
struct filter *delay_filter;
|
tmv_t ingress_ts;
|
||||||
|
struct tsproc *tsproc;
|
||||||
struct freq_estimator fest;
|
struct freq_estimator fest;
|
||||||
struct time_status_np status;
|
struct time_status_np status;
|
||||||
double nrr;
|
double nrr;
|
||||||
tmv_t t1;
|
|
||||||
tmv_t t2;
|
|
||||||
struct clock_description desc;
|
struct clock_description desc;
|
||||||
struct clock_stats stats;
|
struct clock_stats stats;
|
||||||
int stats_interval;
|
int stats_interval;
|
||||||
|
@ -271,7 +271,7 @@ void clock_destroy(struct clock *c)
|
||||||
phc_close(c->clkid);
|
phc_close(c->clkid);
|
||||||
}
|
}
|
||||||
servo_destroy(c->servo);
|
servo_destroy(c->servo);
|
||||||
filter_destroy(c->delay_filter);
|
tsproc_destroy(c->tsproc);
|
||||||
stats_destroy(c->stats.offset);
|
stats_destroy(c->stats.offset);
|
||||||
stats_destroy(c->stats.freq);
|
stats_destroy(c->stats.freq);
|
||||||
stats_destroy(c->stats.delay);
|
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:
|
case TLV_TIME_STATUS_NP:
|
||||||
tsn = (struct time_status_np *) tlv->data;
|
tsn = (struct time_status_np *) tlv->data;
|
||||||
tsn->master_offset = c->master_offset;
|
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 =
|
tsn->cumulativeScaledRateOffset =
|
||||||
(Integer32) (c->status.cumulativeScaledRateOffset +
|
(Integer32) (c->status.cumulativeScaledRateOffset +
|
||||||
c->nrr * POW2_41 - POW2_41);
|
c->nrr * POW2_41 - POW2_41);
|
||||||
|
@ -543,7 +543,8 @@ static void clock_stats_update(struct clock_stats *s,
|
||||||
stats_reset(s->delay);
|
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 fui;
|
||||||
double ratio, freq;
|
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.
|
* error caused by our imperfect path delay measurement.
|
||||||
*/
|
*/
|
||||||
if (!f->ingress1) {
|
if (!f->ingress1) {
|
||||||
f->ingress1 = c->t2;
|
f->ingress1 = ingress;
|
||||||
f->origin1 = c->t1;
|
f->origin1 = origin;
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
f->count++;
|
f->count++;
|
||||||
if (f->count < f->max_count) {
|
if (f->count < f->max_count) {
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
if (tmv_eq(c->t2, f->ingress1)) {
|
if (tmv_eq(ingress, f->ingress1)) {
|
||||||
pr_warning("bad timestamps in rate ratio calculation");
|
pr_warning("bad timestamps in rate ratio calculation");
|
||||||
return state;
|
return state;
|
||||||
}
|
}
|
||||||
|
|
||||||
ratio = tmv_dbl(tmv_sub(c->t1, f->origin1)) /
|
ratio = tmv_dbl(tmv_sub(origin, f->origin1)) /
|
||||||
tmv_dbl(tmv_sub(c->t2, f->ingress1));
|
tmv_dbl(tmv_sub(ingress, f->ingress1));
|
||||||
freq = (1.0 - ratio) * 1e9;
|
freq = (1.0 - ratio) * 1e9;
|
||||||
|
|
||||||
if (c->stats.max_count > 1) {
|
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("master/local %.9f", ratio);
|
||||||
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 = ingress;
|
||||||
f->origin1 = c->t1;
|
f->origin1 = origin;
|
||||||
f->count = 0;
|
f->count = 0;
|
||||||
|
|
||||||
return state;
|
return state;
|
||||||
|
@ -851,10 +852,9 @@ struct clock *clock_create(int phc_index, struct interfaces_head *ifaces,
|
||||||
}
|
}
|
||||||
c->servo_state = SERVO_UNLOCKED;
|
c->servo_state = SERVO_UNLOCKED;
|
||||||
c->servo_type = servo;
|
c->servo_type = servo;
|
||||||
c->delay_filter = filter_create(dds->delay_filter,
|
c->tsproc = tsproc_create(dds->delay_filter, dds->delay_filter_length);
|
||||||
dds->delay_filter_length);
|
if (!c->tsproc) {
|
||||||
if (!c->delay_filter) {
|
pr_err("Failed to create time stamp processor");
|
||||||
pr_err("Failed to create delay filter");
|
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
c->nrr = 1.0;
|
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)
|
void clock_path_delay(struct clock *c, tmv_t req, tmv_t rx)
|
||||||
{
|
{
|
||||||
tmv_t pd, t1, t2, t3, t4;
|
tsproc_up_ts(c->tsproc, req, rx);
|
||||||
double rr;
|
|
||||||
|
|
||||||
if (tmv_is_zero(c->t1))
|
if (tsproc_update_delay(c->tsproc, &c->path_delay))
|
||||||
return;
|
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);
|
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)
|
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->path_delay = ppd;
|
||||||
c->nrr = nrr;
|
c->nrr = nrr;
|
||||||
|
|
||||||
|
tsproc_set_delay(c->tsproc, ppd);
|
||||||
|
tsproc_up_ts(c->tsproc, req, rx);
|
||||||
|
|
||||||
if (c->stats.delay)
|
if (c->stats.delay)
|
||||||
stats_add_value(c->stats.delay, tmv_to_nanoseconds(ppd));
|
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;
|
double adj;
|
||||||
enum servo_state state = SERVO_UNLOCKED;
|
enum servo_state state = SERVO_UNLOCKED;
|
||||||
|
|
||||||
c->t1 = origin;
|
c->ingress_ts = ingress;
|
||||||
c->t2 = ingress;
|
|
||||||
|
|
||||||
/*
|
tsproc_down_ts(c->tsproc, origin, ingress);
|
||||||
* c->master_offset = ingress - origin - c->path_delay;
|
|
||||||
*/
|
|
||||||
c->master_offset = tmv_sub(ingress, tmv_add(origin, c->path_delay));
|
|
||||||
|
|
||||||
if (!c->path_delay)
|
if (tsproc_update_offset(c->tsproc, &c->master_offset))
|
||||||
return state;
|
return state;
|
||||||
|
|
||||||
if (clock_utc_correct(c, ingress))
|
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);
|
c->cur.offsetFromMaster = tmv_to_TimeInterval(c->master_offset);
|
||||||
|
|
||||||
if (c->free_running)
|
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),
|
adj = servo_sample(c->servo, tmv_to_nanoseconds(c->master_offset),
|
||||||
tmv_to_nanoseconds(ingress), &state);
|
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));
|
tmv_to_nanoseconds(c->path_delay));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
tsproc_set_clock_rate_ratio(c->tsproc, clock_rate_ratio(c));
|
||||||
|
|
||||||
switch (state) {
|
switch (state) {
|
||||||
case SERVO_UNLOCKED:
|
case SERVO_UNLOCKED:
|
||||||
break;
|
break;
|
||||||
case SERVO_JUMP:
|
case SERVO_JUMP:
|
||||||
clockadj_set_freq(c->clkid, -adj);
|
clockadj_set_freq(c->clkid, -adj);
|
||||||
clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset));
|
clockadj_step(c->clkid, -tmv_to_nanoseconds(c->master_offset));
|
||||||
c->t1 = tmv_zero();
|
c->ingress_ts = tmv_zero();
|
||||||
c->t2 = tmv_zero();
|
|
||||||
if (c->sanity_check) {
|
if (c->sanity_check) {
|
||||||
clockcheck_set_freq(c->sanity_check, -adj);
|
clockcheck_set_freq(c->sanity_check, -adj);
|
||||||
clockcheck_step(c->sanity_check,
|
clockcheck_step(c->sanity_check,
|
||||||
-tmv_to_nanoseconds(c->master_offset));
|
-tmv_to_nanoseconds(c->master_offset));
|
||||||
}
|
}
|
||||||
|
tsproc_reset(c->tsproc, 0);
|
||||||
break;
|
break;
|
||||||
case SERVO_LOCKED:
|
case SERVO_LOCKED:
|
||||||
clockadj_set_freq(c->clkid, -adj);
|
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)) {
|
if (!cid_eq(&best_id, &c->best_id)) {
|
||||||
clock_freq_est_reset(c);
|
clock_freq_est_reset(c);
|
||||||
filter_reset(c->delay_filter);
|
tsproc_reset(c->tsproc, 1);
|
||||||
c->t1 = tmv_zero();
|
c->ingress_ts = tmv_zero();
|
||||||
c->t2 = tmv_zero();
|
|
||||||
c->path_delay = 0;
|
c->path_delay = 0;
|
||||||
c->nrr = 1.0;
|
c->nrr = 1.0;
|
||||||
fresh_best = 1;
|
fresh_best = 1;
|
||||||
|
|
5
clock.h
5
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.
|
* Provide the estimated peer delay from a slave port.
|
||||||
* @param c The clock instance.
|
* @param c The clock instance.
|
||||||
* @param ppd The peer delay as measured on a slave port.
|
* @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.
|
* @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.
|
* Poll for events and dispatch them.
|
||||||
|
|
2
makefile
2
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 \
|
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 \
|
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 \
|
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 \
|
OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
|
||||||
sysoff.o timemaster.o
|
sysoff.o timemaster.o
|
||||||
|
|
39
port.c
39
port.c
|
@ -35,6 +35,7 @@
|
||||||
#include "sk.h"
|
#include "sk.h"
|
||||||
#include "tlv.h"
|
#include "tlv.h"
|
||||||
#include "tmv.h"
|
#include "tmv.h"
|
||||||
|
#include "tsproc.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
#define ALLOWED_LOST_RESPONSES 3
|
#define ALLOWED_LOST_RESPONSES 3
|
||||||
|
@ -86,7 +87,7 @@ struct port {
|
||||||
UInteger16 sync;
|
UInteger16 sync;
|
||||||
} seqnum;
|
} seqnum;
|
||||||
tmv_t peer_delay;
|
tmv_t peer_delay;
|
||||||
struct filter *delay_filter;
|
struct tsproc *tsproc;
|
||||||
int log_sync_interval;
|
int log_sync_interval;
|
||||||
struct nrate_estimator nrate;
|
struct nrate_estimator nrate;
|
||||||
unsigned int pdr_missing;
|
unsigned int pdr_missing;
|
||||||
|
@ -1798,11 +1799,10 @@ out:
|
||||||
|
|
||||||
static void port_peer_delay(struct port *p)
|
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 *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;
|
||||||
double adj_t41;
|
|
||||||
|
|
||||||
/* Check for response, validate port and sequence number. */
|
/* 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);
|
c2 = correction_to_tmv(fup->header.correction);
|
||||||
calc:
|
calc:
|
||||||
t3c = tmv_add(t3, tmv_add(c1, c2));
|
t3c = tmv_add(t3, tmv_add(c1, c2));
|
||||||
adj_t41 = p->nrate.ratio * clock_rate_ratio(p->clock) *
|
tsproc_set_clock_rate_ratio(p->tsproc, p->nrate.ratio *
|
||||||
tmv_dbl(tmv_sub(t4, t1));
|
clock_rate_ratio(p->clock));
|
||||||
pd = tmv_sub(dbl_tmv(adj_t41), tmv_sub(t3c, t2));
|
tsproc_up_ts(p->tsproc, t1, t2);
|
||||||
pd = tmv_div(pd, 2);
|
tsproc_down_ts(p->tsproc, t3c, t4);
|
||||||
|
if (tsproc_update_delay(p->tsproc, &p->peer_delay))
|
||||||
p->peer_delay = filter_sample(p->delay_filter, pd);
|
return;
|
||||||
|
|
||||||
p->peerMeanPathDelay = tmv_to_TimeInterval(p->peer_delay);
|
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)
|
if (p->pod.follow_up_info)
|
||||||
port_nrate_calculate(p, t3c, t4);
|
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, t1, t2,
|
||||||
|
p->nrate.ratio);
|
||||||
}
|
}
|
||||||
|
|
||||||
msg_put(p->peer_delay_req);
|
msg_put(p->peer_delay_req);
|
||||||
|
@ -1981,7 +1980,7 @@ void port_close(struct port *p)
|
||||||
port_disable(p);
|
port_disable(p);
|
||||||
}
|
}
|
||||||
transport_destroy(p->trp);
|
transport_destroy(p->trp);
|
||||||
filter_destroy(p->delay_filter);
|
tsproc_destroy(p->tsproc);
|
||||||
if (p->fault_fd >= 0)
|
if (p->fault_fd >= 0)
|
||||||
close(p->fault_fd);
|
close(p->fault_fd);
|
||||||
free(p);
|
free(p);
|
||||||
|
@ -2535,10 +2534,10 @@ struct port *port_open(int phc_index,
|
||||||
p->delayMechanism = interface->dm;
|
p->delayMechanism = interface->dm;
|
||||||
p->versionNumber = PTP_VERSION;
|
p->versionNumber = PTP_VERSION;
|
||||||
|
|
||||||
p->delay_filter = filter_create(interface->delay_filter,
|
p->tsproc = tsproc_create(interface->delay_filter,
|
||||||
interface->delay_filter_length);
|
interface->delay_filter_length);
|
||||||
if (!p->delay_filter) {
|
if (!p->tsproc) {
|
||||||
pr_err("Failed to create delay filter");
|
pr_err("Failed to create time stamp processor");
|
||||||
goto err_transport;
|
goto err_transport;
|
||||||
}
|
}
|
||||||
p->nrate.ratio = 1.0;
|
p->nrate.ratio = 1.0;
|
||||||
|
@ -2549,13 +2548,13 @@ struct port *port_open(int phc_index,
|
||||||
p->fault_fd = timerfd_create(CLOCK_MONOTONIC, 0);
|
p->fault_fd = timerfd_create(CLOCK_MONOTONIC, 0);
|
||||||
if (p->fault_fd < 0) {
|
if (p->fault_fd < 0) {
|
||||||
pr_err("timerfd_create failed: %m");
|
pr_err("timerfd_create failed: %m");
|
||||||
goto err_filter;
|
goto err_tsproc;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return p;
|
return p;
|
||||||
|
|
||||||
err_filter:
|
err_tsproc:
|
||||||
filter_destroy(p->delay_filter);
|
tsproc_destroy(p->tsproc);
|
||||||
err_transport:
|
err_transport:
|
||||||
transport_destroy(p->trp);
|
transport_destroy(p->trp);
|
||||||
err_port:
|
err_port:
|
||||||
|
|
|
@ -0,0 +1,163 @@
|
||||||
|
/**
|
||||||
|
* @file tsproc.c
|
||||||
|
* @note Copyright (C) 2015 Miroslav Lichvar <mlichvar@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);
|
||||||
|
}
|
||||||
|
}
|
|
@ -0,0 +1,97 @@
|
||||||
|
/**
|
||||||
|
* @file tsproc.h
|
||||||
|
* @brief Time stamp processor.
|
||||||
|
* @note Copyright (C) 2015 Miroslav Lichvar <mlichvar@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
|
Loading…
Reference in New Issue