diff --git a/ts2phc.c b/ts2phc.c index 75a4010..ac420bf 100644 --- a/ts2phc.c +++ b/ts2phc.c @@ -477,6 +477,42 @@ static void ts2phc_synchronize_clocks(struct ts2phc_private *priv, int autocfg) } } +static int ts2phc_collect_master_tstamp(struct ts2phc_private *priv) +{ + struct clock *master_clock; + struct timespec master_ts; + int err; + + master_clock = ts2phc_master_get_clock(priv->master); + /* + * Master isn't a PHC (it may be a generic or a GPS master), + * don't error out, just don't do anything. If it doesn't have a PHC, + * there is nothing to synchronize, which is the only point of + * collecting its perout timestamp in the first place. + */ + if (!master_clock) + return 0; + + err = ts2phc_master_getppstime(priv->master, &master_ts); + if (err < 0) { + pr_err("source ts not valid"); + return err; + } + + /* + * As long as the kernel doesn't support a proper API for reporting + * a precise perout timestamp, we'll have to use this crude + * approximation. + */ + if (master_ts.tv_nsec > NS_PER_SEC / 2) + master_ts.tv_sec++; + master_ts.tv_nsec = 0; + + clock_add_tstamp(master_clock, timespec_to_tmv(master_ts)); + + return 0; +} + static void usage(char *progname) { fprintf(stderr, @@ -695,8 +731,15 @@ int main(int argc, char *argv[]) pr_err("poll failed"); break; } - if (err > 0) + if (err > 0) { + err = ts2phc_collect_master_tstamp(&priv); + if (err) { + pr_err("failed to collect master tstamp"); + break; + } + ts2phc_synchronize_clocks(&priv, autocfg); + } } ts2phc_cleanup(&priv); diff --git a/ts2phc_master.c b/ts2phc_master.c index 4617c4a..0dc02a1 100644 --- a/ts2phc_master.c +++ b/ts2phc_master.c @@ -38,3 +38,11 @@ int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts) { return master->getppstime(master, ts); } + +struct clock *ts2phc_master_get_clock(struct ts2phc_master *m) +{ + if (m->get_clock) + return m->get_clock(m); + + return NULL; +} diff --git a/ts2phc_master.h b/ts2phc_master.h index a7e7186..6edf8c0 100644 --- a/ts2phc_master.h +++ b/ts2phc_master.h @@ -51,4 +51,6 @@ void ts2phc_master_destroy(struct ts2phc_master *master); */ int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts); +struct clock *ts2phc_master_get_clock(struct ts2phc_master *m); + #endif diff --git a/ts2phc_master_private.h b/ts2phc_master_private.h index 463a1f0..deef1b5 100644 --- a/ts2phc_master_private.h +++ b/ts2phc_master_private.h @@ -15,6 +15,7 @@ struct ts2phc_master { void (*destroy)(struct ts2phc_master *ts2phc_master); int (*getppstime)(struct ts2phc_master *master, struct timespec *ts); + struct clock *(*get_clock)(struct ts2phc_master *m); }; #endif diff --git a/ts2phc_phc_master.c b/ts2phc_phc_master.c index 6fda39f..8626626 100644 --- a/ts2phc_phc_master.c +++ b/ts2phc_phc_master.c @@ -83,6 +83,14 @@ static int ts2phc_phc_master_getppstime(struct ts2phc_master *m, return clock_gettime(master->clock->clkid, ts); } +struct clock *ts2phc_phc_master_get_clock(struct ts2phc_master *m) +{ + struct ts2phc_phc_master *master = + container_of(m, struct ts2phc_phc_master, master); + + return master->clock; +} + struct ts2phc_master *ts2phc_phc_master_create(struct ts2phc_private *priv, const char *dev) { @@ -94,6 +102,7 @@ struct ts2phc_master *ts2phc_phc_master_create(struct ts2phc_private *priv, } master->master.destroy = ts2phc_phc_master_destroy; master->master.getppstime = ts2phc_phc_master_getppstime; + master->master.get_clock = ts2phc_phc_master_get_clock; master->clock = clock_add(priv, dev); if (!master->clock) {