Monitor the port state change events from ptp4l, and use that information to determine the "reference" clock.
Then synchronize all other clocks in our list to that source, by feeding into their respective servo loop an offset equal to the delta between their timestamp and the timestamp of the reference clock. All timestamps are representative of the same event, which is the most recent perout pulse of the PPS source. Signed-off-by: Vladimir Oltean <olte...@gmail.com> --- v5->v6: - None. v4->v5: - Rename source clock to reference clock, destination clock to target clock - Rebase on top of earlier data structure renaming (master -> PPS source, slave -> PPS sink) v3->v4: - Use bool for boolean types. v2->v3: - None. ts2phc.c | 130 ++++++++++++++++++++++++++++++++++++---- ts2phc.h | 3 + ts2phc_phc_pps_source.c | 1 + ts2phc_pps_sink.c | 1 + 4 files changed, 123 insertions(+), 12 deletions(-) diff --git a/ts2phc.c b/ts2phc.c index 5aaa2702c787..4d23a5dcdf5a 100644 --- a/ts2phc.c +++ b/ts2phc.c @@ -113,6 +113,7 @@ static int ts2phc_recv_subscribed(void *context, struct ptp_message *msg, state = ts2phc_clock_compute_state(priv, clock); if (clock->state != state || clock->new_state) { clock->new_state = state; + priv->state_changed = true; } } return 1; @@ -317,33 +318,126 @@ static int ts2phc_auto_init_ports(struct ts2phc_private *priv) LIST_FOREACH(clock, &priv->clocks, list) { clock->new_state = ts2phc_clock_compute_state(priv, clock); } + priv->state_changed = true; return 0; } -static void ts2phc_synchronize_clocks(struct ts2phc_private *priv) +static void ts2phc_reconfigure(struct ts2phc_private *priv) +{ + struct ts2phc_clock *c, *ref_clk = NULL, *last = NULL; + int num_ref_clocks = 0, num_target_clocks = 0; + + pr_info("reconfiguring after port state change"); + priv->state_changed = false; + + LIST_FOREACH(c, &priv->clocks, list) { + if (c->new_state) { + c->state = c->new_state; + c->new_state = 0; + } + + switch (c->state) { + case PS_FAULTY: + case PS_DISABLED: + case PS_LISTENING: + case PS_PRE_MASTER: + case PS_MASTER: + case PS_PASSIVE: + if (!c->is_target) { + pr_info("selecting %s for synchronization", + c->name); + c->is_target = true; + } + num_target_clocks++; + break; + case PS_UNCALIBRATED: + num_ref_clocks++; + break; + case PS_SLAVE: + ref_clk = c; + num_ref_clocks++; + break; + default: + break; + } + last = c; + } + if (num_target_clocks >= 1 && !ref_clk) { + priv->ref_clock = last; + priv->ref_clock->is_target = false; + /* Reset to original state in next reconfiguration. */ + priv->ref_clock->new_state = priv->ref_clock->state; + priv->ref_clock->state = PS_SLAVE; + pr_info("no reference clock, selecting %s by default", + last->name); + return; + } + if (num_ref_clocks > 1) { + pr_info("multiple reference clocks available, postponing sync..."); + priv->ref_clock = NULL; + return; + } + if (num_ref_clocks > 0 && !ref_clk) { + pr_info("reference clock not ready, waiting..."); + priv->ref_clock = NULL; + return; + } + if (!num_ref_clocks && !num_target_clocks) { + pr_info("no PHC ready, waiting..."); + priv->ref_clock = NULL; + return; + } + if (!num_ref_clocks) { + pr_info("nothing to synchronize"); + priv->ref_clock = NULL; + return; + } + ref_clk->is_target = false; + priv->ref_clock = ref_clk; + pr_info("selecting %s as the reference clock", ref_clk->name); +} + +static void ts2phc_synchronize_clocks(struct ts2phc_private *priv, int autocfg) { - 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; + if (autocfg) { + if (!priv->ref_clock) { + pr_debug("no reference clock, skipping"); + return; + } + valid = ts2phc_clock_get_tstamp(priv->ref_clock, &source_tmv); + if (!valid) { + pr_err("reference clock (%s) timestamp not valid, skipping", + priv->ref_clock->name); + return; + } + } else { + struct timespec source_ts; + + 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); + source_tmv = timespec_to_tmv(source_ts); + } LIST_FOREACH(c, &priv->clocks, list) { int64_t offset; double adj; tmv_t ts; + if (!c->is_target) + continue; + valid = ts2phc_clock_get_tstamp(c, &ts); if (!valid) { pr_debug("%s timestamp not valid, skipping", c->name); @@ -579,6 +673,18 @@ int main(int argc, char *argv[]) while (is_running()) { struct ts2phc_clock *c; + if (autocfg) { + /* Collect updates from ptp4l */ + err = pmc_agent_update(priv.agent); + if (err < 0) { + pr_err("pmc_agent_update returned %d", err); + break; + } + + if (priv.state_changed) + ts2phc_reconfigure(&priv); + } + LIST_FOREACH(c, &priv.clocks, list) ts2phc_clock_flush_tstamp(c); @@ -588,7 +694,7 @@ int main(int argc, char *argv[]) break; } if (err > 0) - ts2phc_synchronize_clocks(&priv); + ts2phc_synchronize_clocks(&priv, autocfg); } ts2phc_cleanup(&priv); diff --git a/ts2phc.h b/ts2phc.h index ae95034b7113..7bdd65bdf572 100644 --- a/ts2phc.h +++ b/ts2phc.h @@ -31,6 +31,7 @@ struct ts2phc_clock { enum servo_state servo_state; char *name; bool no_adj; + bool is_target; bool is_ts_available; tmv_t last_ts; }; @@ -49,6 +50,8 @@ struct ts2phc_private { struct ts2phc_sink_array *polling_array; struct config *cfg; struct pmc_agent *agent; + struct ts2phc_clock *ref_clock; + bool state_changed; LIST_HEAD(port_head, ts2phc_port) ports; LIST_HEAD(clock_head, ts2phc_clock) clocks; }; diff --git a/ts2phc_phc_pps_source.c b/ts2phc_phc_pps_source.c index f82ed37a7926..92273f4fc14d 100644 --- a/ts2phc_phc_pps_source.c +++ b/ts2phc_phc_pps_source.c @@ -99,6 +99,7 @@ struct ts2phc_pps_source *ts2phc_phc_pps_source_create(struct ts2phc_private *pr free(s); return NULL; } + s->clock->is_target = false; pr_debug("PHC PPS source %s has ptp index %d", dev, s->clock->phc_index); diff --git a/ts2phc_pps_sink.c b/ts2phc_pps_sink.c index 22c103f8bea4..b0ac6da375ae 100644 --- a/ts2phc_pps_sink.c +++ b/ts2phc_pps_sink.c @@ -184,6 +184,7 @@ static struct ts2phc_pps_sink *ts2phc_pps_sink_create(struct ts2phc_private *pri pr_err("failed to open clock"); goto no_posix_clock; } + sink->clock->is_target = true; pr_debug("PPS sink %s has ptp index %d", device, sink->clock->phc_index); -- 2.34.1 _______________________________________________ Linuxptp-devel mailing list Linuxptp-devel@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/linuxptp-devel