diff --git a/ts2phc.c b/ts2phc.c index de1e7d0..75a4010 100644 --- a/ts2phc.c +++ b/ts2phc.c @@ -329,29 +329,119 @@ static int auto_init_ports(struct ts2phc_private *priv) return 0; } -static void ts2phc_synchronize_clocks(struct ts2phc_private *priv) +static void ts2phc_reconfigure(struct ts2phc_private *priv) +{ + struct clock *c, *src = NULL, *last = NULL; + int src_cnt = 0, dst_cnt = 0; + + pr_info("reconfiguring after port state change"); + priv->state_changed = 0; + + 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_destination) { + pr_info("selecting %s for synchronization", + c->name); + c->is_destination = 1; + } + dst_cnt++; + break; + case PS_UNCALIBRATED: + src_cnt++; + break; + case PS_SLAVE: + src = c; + src_cnt++; + break; + } + last = c; + } + if (dst_cnt >= 1 && !src) { + priv->source = last; + priv->source->is_destination = 0; + /* Reset to original state in next reconfiguration. */ + priv->source->new_state = priv->source->state; + priv->source->state = PS_SLAVE; + pr_info("no source, selecting %s as the default clock", + last->name); + return; + } + if (src_cnt > 1) { + pr_info("multiple source clocks available, postponing sync..."); + priv->source = NULL; + return; + } + if (src_cnt > 0 && !src) { + pr_info("source clock not ready, waiting..."); + priv->source = NULL; + return; + } + if (!src_cnt && !dst_cnt) { + pr_info("no PHC ready, waiting..."); + priv->source = NULL; + return; + } + if (!src_cnt) { + pr_info("nothing to synchronize"); + priv->source = NULL; + return; + } + src->is_destination = 0; + priv->source = src; + pr_info("selecting %s as the source clock", src->name); +} + +static void ts2phc_synchronize_clocks(struct ts2phc_private *priv, int autocfg) { - 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; + if (autocfg) { + if (!priv->source) { + pr_debug("no source, skipping"); + return; + } + valid = clock_get_tstamp(priv->source, &source_tmv); + if (!valid) { + pr_err("source clock (%s) timestamp not valid, skipping", + priv->source->name); + return; + } + } else { + struct timespec source_ts; - source_tmv = timespec_to_tmv(source_ts); + 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; + if (!c->is_destination) + continue; + valid = clock_get_tstamp(c, &ts); if (!valid) { pr_debug("%s timestamp not valid, skipping", c->name); @@ -582,6 +672,21 @@ int main(int argc, char *argv[]) while (is_running()) { struct clock *c; + if (autocfg) { + /* + * Make sure ptp4l sees us as alive and doesn't prune + * us from the list of subscribers + */ + err = update_pmc_node(&priv.node, 1); + if (err < 0) { + pr_err("update_pmc_node returned %d", err); + break; + } + run_pmc_events(&priv.node); + if (priv.state_changed) + ts2phc_reconfigure(&priv); + } + LIST_FOREACH(c, &priv.clocks, list) clock_flush_tstamp(c); @@ -591,7 +696,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_slave.c b/ts2phc_slave.c index f5c5dc0..031362a 100644 --- a/ts2phc_slave.c +++ b/ts2phc_slave.c @@ -187,6 +187,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, pr_err("failed to open clock"); goto no_posix_clock; } + slave->clock->is_destination = 1; pr_debug("PHC slave %s has ptp index %d", device, slave->clock->phc_index);