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
parent
1441562149
commit
a6c19b7154
94
ts2phc.c
94
ts2phc.c
|
@ -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);
|
||||||
|
|
4
ts2phc.h
4
ts2phc.h
|
@ -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"
|
||||||
|
|
177
ts2phc_slave.c
177
ts2phc_slave.c
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue