The previous patches have established that:
- a PPS sink deals with extts events
- a clock deals with synchronization via a servo loop
Therefore, the code for synchronization should not be part of the
implementation of a PPS sink. Move it to the main ts2phc.c.
This allows it to be used by a PPS source as well (the PHC kind).
Signed-off-by: Vladimir Oltean <olte...@gmail.com>
---
v4->v5:
- Rebase on top of the variable renaming.
v3->v4:
- Use bool for boolean types.
v2->v3:
- None.
ts2phc.c | 94 +++++++++++++++++++++-
ts2phc.h | 3 +
ts2phc_pps_sink.c | 199 ++++++++++++++++++++++------------------------
3 files changed, 190 insertions(+), 106 deletions(-)
diff --git a/ts2phc.c b/ts2phc.c
index 1c577af7768e..f84950947712 100644
--- a/ts2phc.c
+++ b/ts2phc.c
@@ -21,6 +21,9 @@
#include "ts2phc.h"
#include "version.h"
+#define NS_PER_SEC 1000000000LL
+#define SAMPLE_WEIGHT 1.0
+
struct interface {
STAILQ_ENTRY(interface) list;
};
@@ -154,6 +157,30 @@ static struct servo *ts2phc_servo_create(struct
ts2phc_private *priv,
return servo;
}
+void ts2phc_clock_add_tstamp(struct ts2phc_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 = true;
+}
+
+static int ts2phc_clock_get_tstamp(struct ts2phc_clock *clock, tmv_t *ts)
+{
+ if (!clock->is_ts_available)
+ return 0;
+ clock->is_ts_available = false;
+ *ts = clock->last_ts;
+ return 1;
+}
+
+static void ts2phc_clock_flush_tstamp(struct ts2phc_clock *clock)
+{
+ clock->is_ts_available = false;
+}
+
struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
const char *device)
{
@@ -303,6 +330,64 @@ static int auto_init_ports(struct ts2phc_private *priv)
return 0;
}
+static void ts2phc_synchronize_clocks(struct ts2phc_private *priv)
+{
+ struct timespec source_ts;
+ tmv_t source_tmv;
+ struct ts2phc_clock *c;
+ int valid, err;
+
+ err = ts2phc_pps_source_getppstime(priv->src, &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 = ts2phc_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)
{
fprintf(stderr,
@@ -501,11 +586,18 @@ int main(int argc, char *argv[])
}
while (is_running()) {
+ struct ts2phc_clock *c;
+
+ LIST_FOREACH(c, &priv.clocks, list)
+ ts2phc_clock_flush_tstamp(c);
+
err = ts2phc_pps_sink_poll(&priv);
- if (err) {
+ if (err < 0) {
pr_err("poll failed");
break;
}
+ if (err > 0)
+ ts2phc_synchronize_clocks(&priv);
}
ts2phc_cleanup(&priv);
diff --git a/ts2phc.h b/ts2phc.h
index 1fab09dbf130..f52a2f3d3cae 100644
--- a/ts2phc.h
+++ b/ts2phc.h
@@ -27,6 +27,8 @@ struct ts2phc_clock {
enum servo_state servo_state;
char *name;
bool no_adj;
+ bool is_ts_available;
+ tmv_t last_ts;
};
struct ts2phc_port {
@@ -50,6 +52,7 @@ struct ts2phc_private {
struct ts2phc_clock *ts2phc_clock_add(struct ts2phc_private *priv,
const char *device);
void ts2phc_clock_destroy(struct ts2phc_clock *clock);
+void ts2phc_clock_add_tstamp(struct ts2phc_clock *clock, tmv_t ts);
#include "ts2phc_pps_source.h"
#include "ts2phc_pps_sink.h"
diff --git a/ts2phc_pps_sink.c b/ts2phc_pps_sink.c
index 69cc97179e36..92bb8dd47107 100644
--- a/ts2phc_pps_sink.c
+++ b/ts2phc_pps_sink.c
@@ -24,42 +24,29 @@
#include "ts2phc.h"
#include "util.h"
-#define NS_PER_SEC 1000000000LL
-#define SAMPLE_WEIGHT 1.0
-
struct ts2phc_pps_sink {
char *name;
STAILQ_ENTRY(ts2phc_pps_sink) list;
struct ptp_pin_desc pin_desc;
unsigned int polarity;
- int32_t correction;
+ tmv_t correction;
uint32_t ignore_lower;
uint32_t ignore_upper;
struct ts2phc_clock *clock;
- int no_adj;
};
struct ts2phc_sink_array {
struct ts2phc_pps_sink **sink;
+ int *collected_events;
struct pollfd *pfd;
};
-struct ts2phc_source_timestamp {
- struct timespec ts;
- bool valid;
-};
-
enum extts_result {
EXTTS_ERROR = -1,
EXTTS_OK = 0,
EXTTS_IGNORE = 1,
};
-static enum extts_result
-ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
- struct ts2phc_source_timestamp ts,
- int64_t *offset, uint64_t *local_ts);
-
static int ts2phc_pps_sink_array_create(struct ts2phc_private *priv)
{
struct ts2phc_sink_array *polling_array;
@@ -86,6 +73,15 @@ static int ts2phc_pps_sink_array_create(struct
ts2phc_private *priv)
polling_array->sink = NULL;
return -1;
}
+ polling_array->collected_events = malloc(priv->n_sinks * sizeof(int));
+ if (!polling_array->collected_events) {
+ pr_err("low memory");
+ free(polling_array->sink);
+ free(polling_array->pfd);
+ polling_array->pfd = NULL;
+ polling_array->sink = NULL;
+ return -1;
+ }
i = 0;
STAILQ_FOREACH(sink, &priv->sinks, list) {
polling_array->sink[i] = sink;
@@ -107,8 +103,12 @@ static void ts2phc_pps_sink_array_destroy(struct
ts2phc_private *priv)
{
struct ts2phc_sink_array *polling_array = priv->polling_array;
+ if (!polling_array)
+ return;
+
free(polling_array->sink);
free(polling_array->pfd);
+ free(polling_array->collected_events);
free(polling_array);
priv->polling_array = NULL;
}
@@ -153,6 +153,7 @@ static struct ts2phc_pps_sink
*ts2phc_pps_sink_create(struct ts2phc_private *pri
struct ptp_extts_request extts;
struct ts2phc_pps_sink *sink;
int err, pulsewidth;
+ int32_t correction;
sink = calloc(1, sizeof(*sink));
if (!sink) {
@@ -169,7 +170,8 @@ static struct ts2phc_pps_sink
*ts2phc_pps_sink_create(struct ts2phc_private *pri
sink->pin_desc.func = PTP_PF_EXTTS;
sink->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel");
sink->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity");
- sink->correction = config_get_int(cfg, device,
"ts2phc.extts_correction");
+ correction = config_get_int(cfg, device, "ts2phc.extts_correction");
+ sink->correction = nanoseconds_to_tmv(correction);
pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth");
pulsewidth /= 2;
@@ -234,71 +236,32 @@ static void ts2phc_pps_sink_destroy(struct
ts2phc_pps_sink *sink)
free(sink);
}
-static int ts2phc_pps_sink_event(struct ts2phc_pps_sink *sink,
- struct ts2phc_source_timestamp source_ts)
+static enum extts_result ts2phc_pps_sink_event(struct ts2phc_private *priv,
+ struct ts2phc_pps_sink *sink)
{
- enum extts_result result;
- uint64_t extts_ts;
- int64_t offset;
- double adj;
-
- result = ts2phc_pps_sink_offset(sink, source_ts, &offset, &extts_ts);
- switch (result) {
- case EXTTS_ERROR:
- return -1;
- case EXTTS_OK:
- break;
- case EXTTS_IGNORE:
- return 0;
- }
-
- if (sink->no_adj) {
- pr_info("%s source offset %10" PRId64, sink->name, offset);
- return 0;
- }
-
- adj = servo_sample(sink->clock->servo, offset, extts_ts,
- SAMPLE_WEIGHT, &sink->clock->servo_state);
-
- pr_info("%s source offset %10" PRId64 " s%d freq %+7.0f",
- sink->name, offset, sink->clock->servo_state, adj);
-
- switch (sink->clock->servo_state) {
- case SERVO_UNLOCKED:
- break;
- case SERVO_JUMP:
- clockadj_set_freq(sink->clock->clkid, -adj);
- clockadj_step(sink->clock->clkid, -offset);
- break;
- case SERVO_LOCKED:
- case SERVO_LOCKED_STABLE:
- clockadj_set_freq(sink->clock->clkid, -adj);
- break;
- }
- return 0;
-}
-
-static enum extts_result
-ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
- struct ts2phc_source_timestamp src,
- int64_t *offset, uint64_t *local_ts)
-{
- struct timespec source_ts = src.ts;
+ enum extts_result result = EXTTS_OK;
struct ptp_extts_event event;
- uint64_t event_ns, source_ns;
- int cnt;
+ struct timespec source_ts;
+ int err, cnt;
+ tmv_t ts;
cnt = read(CLOCKID_TO_FD(sink->clock->clkid), &event, sizeof(event));
if (cnt != sizeof(event)) {
pr_err("read extts event failed: %m");
- return EXTTS_ERROR;
+ result = EXTTS_ERROR;
+ goto out;
}
if (event.index != sink->pin_desc.chan) {
pr_err("extts on unexpected channel");
- return EXTTS_ERROR;
+ result = EXTTS_ERROR;
+ goto out;
+ }
+
+ err = ts2phc_pps_source_getppstime(priv->src, &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 (sink->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
source_ts.tv_nsec > sink->ignore_lower &&
@@ -308,20 +271,17 @@ ts2phc_pps_sink_offset(struct ts2phc_pps_sink *sink,
sink->name, event.index, event.t.sec, event.t.nsec,
(int64_t) source_ts.tv_sec, source_ts.tv_nsec);
- return EXTTS_IGNORE;
- }
- if (source_ts.tv_nsec > 500000000) {
- source_ts.tv_sec++;
+ result = EXTTS_IGNORE;
+ goto out;
}
- source_ns = source_ts.tv_sec * NS_PER_SEC;
- *offset = event_ns + sink->correction - source_ns;
- *local_ts = event_ns + sink->correction;
- pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64
- ".%ld diff %" PRId64,
- sink->name, event.index, event.t.sec, event.t.nsec,
- sink->correction,
- (int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset);
+out:
+ if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
+ return result;
+
+ ts = pct_to_tmv(event.t);
+ ts = tmv_add(ts, sink->correction);
+ ts2phc_clock_add_tstamp(sink->clock, ts);
return EXTTS_OK;
}
@@ -397,35 +357,64 @@ void ts2phc_pps_sink_cleanup(struct ts2phc_private *priv)
int ts2phc_pps_sink_poll(struct ts2phc_private *priv)
{
struct ts2phc_sink_array *polling_array = priv->polling_array;
- struct ts2phc_source_timestamp source_ts;
+ bool all_sinks_have_events = false;
+ bool ignore_any = false;
unsigned int i;
- int cnt, err;
+ int cnt;
+
+ for (i = 0; i < priv->n_sinks; i++)
+ polling_array->collected_events[i] = 0;
+
+ while (!all_sinks_have_events) {
+ struct ts2phc_pps_sink *sink;
- cnt = poll(polling_array->pfd, priv->n_sinks, 2000);
- if (cnt < 0) {
- if (EINTR == errno) {
+ cnt = poll(polling_array->pfd, priv->n_sinks, 2000);
+ if (cnt < 0) {
+ if (errno == -EINTR) {
+ return 0;
+ } else {
+ pr_emerg("poll failed");
+ return -1;
+ }
+ } else if (!cnt) {
+ pr_debug("poll returns zero, no events");
return 0;
- } else {
- pr_emerg("poll failed");
- return -1;
}
- } else if (!cnt) {
- pr_debug("poll returns zero, no events");
- return 0;
- }
- err = ts2phc_pps_source_getppstime(priv->src, &source_ts.ts);
- source_ts.valid = err ? false : true;
+ for (i = 0; i < priv->n_sinks; i++) {
+ if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
+ enum extts_result result;
+
+ sink = polling_array->sink[i];
+
+ result = ts2phc_pps_sink_event(priv, sink);
+ if (result == EXTTS_ERROR)
+ return -EIO;
+ if (result == EXTTS_IGNORE)
+ ignore_any = true;
+
+ /*
+ * Collect the events anyway, even if we'll
+ * ignore this source edge anyway. We don't
+ * want sink events from different edges
+ * to pile up and mix.
+ */
+ polling_array->collected_events[i]++;
+ }
+ }
- if (!source_ts.valid) {
- pr_debug("ignoring invalid source time stamp");
- return 0;
- }
+ all_sinks_have_events = true;
- for (i = 0; i < priv->n_sinks; i++) {
- if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
- ts2phc_pps_sink_event(polling_array->sink[i],
source_ts);
+ for (i = 0; i < priv->n_sinks; i++) {
+ if (!polling_array->collected_events[i]) {
+ all_sinks_have_events = false;
+ break;
+ }
}
}
- return 0;
+
+ if (ignore_any)
+ return 0;
+
+ return 1;
}
--
2.25.1
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
