ts2phc: split slave poll from servo loop

Since it has been argued that:

- a ts2phc slave deals with extts events
- a clock deals with synchronization via a servo loop

then the code for synchronization should not be part of the
implementation of a ts2phc slave. Move it to the main ts2phc.c.

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
master
Vladimir Oltean 2020-08-01 17:45:00 +03:00
parent 1441562149
commit a6c19b7154
3 changed files with 191 additions and 108 deletions

View File

@ -20,6 +20,9 @@
#include "ts2phc.h" #include "ts2phc.h"
#include "version.h" #include "version.h"
#define NS_PER_SEC 1000000000LL
#define SAMPLE_WEIGHT 1.0
struct interface { struct interface {
STAILQ_ENTRY(interface) list; STAILQ_ENTRY(interface) list;
}; };
@ -153,6 +156,30 @@ struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock)
return servo; return servo;
} }
void clock_add_tstamp(struct clock *clock, tmv_t t)
{
struct timespec ts = tmv_to_timespec(t);
pr_debug("adding tstamp %ld.%09ld to clock %s",
ts.tv_sec, ts.tv_nsec, clock->name);
clock->last_ts = t;
clock->is_ts_available = 1;
}
static int clock_get_tstamp(struct clock *clock, tmv_t *ts)
{
if (!clock->is_ts_available)
return 0;
clock->is_ts_available = 0;
*ts = clock->last_ts;
return 1;
}
static void clock_flush_tstamp(struct clock *clock)
{
clock->is_ts_available = 0;
}
struct clock *clock_add(struct ts2phc_private *priv, const char *device) struct clock *clock_add(struct ts2phc_private *priv, const char *device)
{ {
clockid_t clkid = CLOCK_INVALID; clockid_t clkid = CLOCK_INVALID;
@ -302,6 +329,64 @@ static int auto_init_ports(struct ts2phc_private *priv)
return 0; return 0;
} }
static void ts2phc_synchronize_clocks(struct ts2phc_private *priv)
{
struct timespec source_ts;
tmv_t source_tmv;
struct clock *c;
int valid, err;
err = ts2phc_master_getppstime(priv->master, &source_ts);
if (err < 0) {
pr_err("source ts not valid");
return;
}
if (source_ts.tv_nsec > NS_PER_SEC / 2)
source_ts.tv_sec++;
source_ts.tv_nsec = 0;
source_tmv = timespec_to_tmv(source_ts);
LIST_FOREACH(c, &priv->clocks, list) {
int64_t offset;
double adj;
tmv_t ts;
valid = clock_get_tstamp(c, &ts);
if (!valid) {
pr_debug("%s timestamp not valid, skipping", c->name);
continue;
}
offset = tmv_to_nanoseconds(tmv_sub(ts, source_tmv));
if (c->no_adj) {
pr_info("%s offset %10" PRId64, c->name,
offset);
continue;
}
adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ts),
SAMPLE_WEIGHT, &c->servo_state);
pr_info("%s offset %10" PRId64 " s%d freq %+7.0f",
c->name, offset, c->servo_state, adj);
switch (c->servo_state) {
case SERVO_UNLOCKED:
break;
case SERVO_JUMP:
clockadj_set_freq(c->clkid, -adj);
clockadj_step(c->clkid, -offset);
break;
case SERVO_LOCKED:
case SERVO_LOCKED_STABLE:
clockadj_set_freq(c->clkid, -adj);
break;
}
}
}
static void usage(char *progname) static void usage(char *progname)
{ {
fprintf(stderr, fprintf(stderr,
@ -495,11 +580,18 @@ int main(int argc, char *argv[])
} }
while (is_running()) { while (is_running()) {
struct clock *c;
LIST_FOREACH(c, &priv.clocks, list)
clock_flush_tstamp(c);
err = ts2phc_slave_poll(&priv); err = ts2phc_slave_poll(&priv);
if (err) { if (err < 0) {
pr_err("poll failed"); pr_err("poll failed");
break; break;
} }
if (err > 0)
ts2phc_synchronize_clocks(&priv);
} }
ts2phc_cleanup(&priv); ts2phc_cleanup(&priv);

View File

@ -28,6 +28,8 @@ struct clock {
char *name; char *name;
int no_adj; int no_adj;
int is_destination; int is_destination;
int is_ts_available;
tmv_t last_ts;
}; };
struct port { struct port {
@ -52,7 +54,7 @@ struct ts2phc_private {
struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock); struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock);
struct clock *clock_add(struct ts2phc_private *priv, const char *device); struct clock *clock_add(struct ts2phc_private *priv, const char *device);
void clock_add_tstamp(struct clock *clock, struct timespec ts); void clock_add_tstamp(struct clock *clock, tmv_t ts);
void clock_destroy(struct clock *clock); void clock_destroy(struct clock *clock);
#include "ts2phc_master.h" #include "ts2phc_master.h"

View File

@ -24,42 +24,29 @@
#include "ts2phc.h" #include "ts2phc.h"
#include "util.h" #include "util.h"
#define NS_PER_SEC 1000000000LL
#define SAMPLE_WEIGHT 1.0
struct ts2phc_slave { struct ts2phc_slave {
char *name; char *name;
STAILQ_ENTRY(ts2phc_slave) list; STAILQ_ENTRY(ts2phc_slave) list;
struct ptp_pin_desc pin_desc; struct ptp_pin_desc pin_desc;
unsigned int polarity; unsigned int polarity;
int32_t correction; tmv_t correction;
uint32_t ignore_lower; uint32_t ignore_lower;
uint32_t ignore_upper; uint32_t ignore_upper;
struct clock *clock; struct clock *clock;
int no_adj;
}; };
struct ts2phc_slave_array { struct ts2phc_slave_array {
struct ts2phc_slave **slave; struct ts2phc_slave **slave;
int *collected_events;
struct pollfd *pfd; struct pollfd *pfd;
}; };
struct ts2phc_source_timestamp {
struct timespec ts;
bool valid;
};
enum extts_result { enum extts_result {
EXTTS_ERROR = -1, EXTTS_ERROR = -1,
EXTTS_OK = 0, EXTTS_OK = 0,
EXTTS_IGNORE = 1, EXTTS_IGNORE = 1,
}; };
static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
struct ts2phc_source_timestamp ts,
int64_t *offset,
uint64_t *local_ts);
static int ts2phc_slave_array_create(struct ts2phc_private *priv) static int ts2phc_slave_array_create(struct ts2phc_private *priv)
{ {
struct ts2phc_slave_array *polling_array; struct ts2phc_slave_array *polling_array;
@ -86,6 +73,15 @@ static int ts2phc_slave_array_create(struct ts2phc_private *priv)
polling_array->slave = NULL; polling_array->slave = NULL;
return -1; return -1;
} }
polling_array->collected_events = malloc(priv->n_slaves * sizeof(int));
if (!polling_array->collected_events) {
pr_err("low memory");
free(polling_array->slave);
free(polling_array->pfd);
polling_array->pfd = NULL;
polling_array->slave = NULL;
return -1;
}
i = 0; i = 0;
STAILQ_FOREACH(slave, &priv->slaves, list) { STAILQ_FOREACH(slave, &priv->slaves, list) {
polling_array->slave[i] = slave; polling_array->slave[i] = slave;
@ -107,8 +103,12 @@ static void ts2phc_slave_array_destroy(struct ts2phc_private *priv)
{ {
struct ts2phc_slave_array *polling_array = priv->polling_array; struct ts2phc_slave_array *polling_array = priv->polling_array;
if (!polling_array)
return;
free(polling_array->slave); free(polling_array->slave);
free(polling_array->pfd); free(polling_array->pfd);
free(polling_array->collected_events);
free(polling_array); free(polling_array);
priv->polling_array = NULL; priv->polling_array = NULL;
} }
@ -152,6 +152,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
struct ptp_extts_request extts; struct ptp_extts_request extts;
struct ts2phc_slave *slave; struct ts2phc_slave *slave;
int err, pulsewidth; int err, pulsewidth;
int32_t correction;
slave = calloc(1, sizeof(*slave)); slave = calloc(1, sizeof(*slave));
if (!slave) { if (!slave) {
@ -171,8 +172,9 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
"ts2phc.channel"); "ts2phc.channel");
slave->polarity = config_get_int(priv->cfg, device, slave->polarity = config_get_int(priv->cfg, device,
"ts2phc.extts_polarity"); "ts2phc.extts_polarity");
slave->correction = config_get_int(priv->cfg, device, correction = config_get_int(priv->cfg, device,
"ts2phc.extts_correction"); "ts2phc.extts_correction");
slave->correction = nanoseconds_to_tmv(correction);
pulsewidth = config_get_int(priv->cfg, device, pulsewidth = config_get_int(priv->cfg, device,
"ts2phc.pulsewidth"); "ts2phc.pulsewidth");
@ -238,71 +240,32 @@ static void ts2phc_slave_destroy(struct ts2phc_slave *slave)
free(slave); free(slave);
} }
static int ts2phc_slave_event(struct ts2phc_slave *slave, static enum extts_result ts2phc_slave_event(struct ts2phc_private *priv,
struct ts2phc_source_timestamp source_ts) struct ts2phc_slave *slave)
{ {
enum extts_result result; enum extts_result result = EXTTS_OK;
uint64_t extts_ts;
int64_t offset;
double adj;
result = ts2phc_slave_offset(slave, source_ts, &offset, &extts_ts);
switch (result) {
case EXTTS_ERROR:
return -1;
case EXTTS_OK:
break;
case EXTTS_IGNORE:
return 0;
}
if (slave->no_adj) {
pr_info("%s master offset %10" PRId64, slave->name, offset);
return 0;
}
adj = servo_sample(slave->clock->servo, offset, extts_ts,
SAMPLE_WEIGHT, &slave->clock->servo_state);
pr_info("%s master offset %10" PRId64 " s%d freq %+7.0f",
slave->name, offset, slave->clock->servo_state, adj);
switch (slave->clock->servo_state) {
case SERVO_UNLOCKED:
break;
case SERVO_JUMP:
clockadj_set_freq(slave->clock->clkid, -adj);
clockadj_step(slave->clock->clkid, -offset);
break;
case SERVO_LOCKED:
case SERVO_LOCKED_STABLE:
clockadj_set_freq(slave->clock->clkid, -adj);
break;
}
return 0;
}
static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
struct ts2phc_source_timestamp src,
int64_t *offset,
uint64_t *local_ts)
{
struct timespec source_ts = src.ts;
struct ptp_extts_event event; struct ptp_extts_event event;
uint64_t event_ns, source_ns; struct timespec source_ts;
int cnt; int err, cnt;
tmv_t ts;
cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event)); cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event));
if (cnt != sizeof(event)) { if (cnt != sizeof(event)) {
pr_err("read extts event failed: %m"); pr_err("read extts event failed: %m");
return EXTTS_ERROR; result = EXTTS_ERROR;
goto out;
} }
if (event.index != slave->pin_desc.chan) { if (event.index != slave->pin_desc.chan) {
pr_err("extts on unexpected channel"); pr_err("extts on unexpected channel");
return EXTTS_ERROR; result = EXTTS_ERROR;
goto out;
}
err = ts2phc_master_getppstime(priv->master, &source_ts);
if (err < 0) {
pr_debug("source ts not valid");
return 0;
} }
event_ns = event.t.sec * NS_PER_SEC;
event_ns += event.t.nsec;
if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) && if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
source_ts.tv_nsec > slave->ignore_lower && source_ts.tv_nsec > slave->ignore_lower &&
@ -312,20 +275,17 @@ static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
slave->name, event.index, event.t.sec, event.t.nsec, slave->name, event.index, event.t.sec, event.t.nsec,
(int64_t) source_ts.tv_sec, source_ts.tv_nsec); (int64_t) source_ts.tv_sec, source_ts.tv_nsec);
return EXTTS_IGNORE; result = EXTTS_IGNORE;
goto out;
} }
if (source_ts.tv_nsec > 500000000) {
source_ts.tv_sec++;
}
source_ns = source_ts.tv_sec * NS_PER_SEC;
*offset = event_ns + slave->correction - source_ns;
*local_ts = event_ns + slave->correction;
pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64 out:
".%ld diff %" PRId64, if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
slave->name, event.index, event.t.sec, event.t.nsec, return result;
slave->correction,
(int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset); ts = pct_to_tmv(event.t);
ts = tmv_add(ts, slave->correction);
clock_add_tstamp(slave->clock, ts);
return EXTTS_OK; return EXTTS_OK;
} }
@ -401,9 +361,16 @@ void ts2phc_slave_cleanup(struct ts2phc_private *priv)
int ts2phc_slave_poll(struct ts2phc_private *priv) int ts2phc_slave_poll(struct ts2phc_private *priv)
{ {
struct ts2phc_slave_array *polling_array = priv->polling_array; struct ts2phc_slave_array *polling_array = priv->polling_array;
struct ts2phc_source_timestamp source_ts; int all_slaves_have_events = 0;
int ignore_any = 0;
unsigned int i; unsigned int i;
int cnt, err; int cnt;
for (i = 0; i < priv->n_slaves; i++)
polling_array->collected_events[i] = 0;
while (!all_slaves_have_events) {
struct ts2phc_slave *slave;
cnt = poll(polling_array->pfd, priv->n_slaves, 2000); cnt = poll(polling_array->pfd, priv->n_slaves, 2000);
if (cnt < 0) { if (cnt < 0) {
@ -418,18 +385,40 @@ int ts2phc_slave_poll(struct ts2phc_private *priv)
return 0; return 0;
} }
err = ts2phc_master_getppstime(priv->master, &source_ts.ts);
source_ts.valid = err ? false : true;
if (!source_ts.valid) {
pr_debug("ignoring invalid master time stamp");
return 0;
}
for (i = 0; i < priv->n_slaves; i++) { for (i = 0; i < priv->n_slaves; i++) {
if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) { if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
ts2phc_slave_event(polling_array->slave[i], source_ts); enum extts_result result;
slave = polling_array->slave[i];
result = ts2phc_slave_event(priv, slave);
if (result == EXTTS_ERROR)
return -EIO;
if (result == EXTTS_IGNORE)
ignore_any = 1;
/*
* Collect the events anyway, even if we'll
* ignore this master edge anyway. We don't
* want slave events from different edges
* to pile up and mix.
*/
polling_array->collected_events[i]++;
} }
} }
all_slaves_have_events = true;
for (i = 0; i < priv->n_slaves; i++) {
if (!polling_array->collected_events[i]) {
all_slaves_have_events = false;
break;
}
}
}
if (ignore_any)
return 0; return 0;
return 1;
} }