diff --git a/ts2phc.c b/ts2phc.c index 4d5f065..5c85f21 100644 --- a/ts2phc.c +++ b/ts2phc.c @@ -7,9 +7,14 @@ * @note SPDX-License-Identifier: GPL-2.0+ */ #include +#include +#include +#include +#include "clockadj.h" #include "config.h" #include "interface.h" +#include "phc.h" #include "print.h" #include "ts2phc.h" #include "version.h" @@ -27,6 +32,77 @@ static void ts2phc_cleanup(struct ts2phc_private *priv) config_destroy(priv->cfg); } +struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock) +{ + enum servo_type type = config_get_int(priv->cfg, NULL, "clock_servo"); + struct servo *servo; + int fadj, max_adj; + + fadj = (int) clockadj_get_freq(clock->clkid); + /* Due to a bug in older kernels, the reading may silently fail + and return 0. Set the frequency back to make sure fadj is + the actual frequency of the clock. */ + clockadj_set_freq(clock->clkid, fadj); + + max_adj = phc_max_adj(clock->clkid); + + servo = servo_create(priv->cfg, type, -fadj, max_adj, 0); + if (!servo) + return NULL; + + servo_sync_interval(servo, SERVO_SYNC_INTERVAL); + + return servo; +} + +struct clock *clock_add(struct ts2phc_private *priv, const char *device) +{ + clockid_t clkid = CLOCK_INVALID; + int phc_index = -1; + struct clock *c; + int err; + + clkid = posix_clock_open(device, &phc_index); + if (clkid == CLOCK_INVALID) + return NULL; + + LIST_FOREACH(c, &priv->clocks, list) { + if (c->phc_index == phc_index) { + /* Already have the clock, don't add it again */ + posix_clock_close(clkid); + return c; + } + } + + c = calloc(1, sizeof(*c)); + if (!c) { + pr_err("failed to allocate memory for a clock"); + return NULL; + } + c->clkid = clkid; + c->phc_index = phc_index; + c->servo_state = SERVO_UNLOCKED; + c->servo = servo_add(priv, c); + c->no_adj = config_get_int(priv->cfg, NULL, "free_running"); + err = asprintf(&c->name, "/dev/ptp%d", phc_index); + if (err < 0) { + free(c); + posix_clock_close(clkid); + return NULL; + } + + LIST_INSERT_HEAD(&priv->clocks, c, list); + return c; +} + +void clock_destroy(struct clock *c) +{ + servo_destroy(c->servo); + posix_clock_close(c->clkid); + free(c->name); + free(c); +} + static void usage(char *progname) { fprintf(stderr, diff --git a/ts2phc.h b/ts2phc.h index e47ea47..61712dc 100644 --- a/ts2phc.h +++ b/ts2phc.h @@ -7,16 +7,43 @@ #ifndef HAVE_TS2PHC_H #define HAVE_TS2PHC_H +#include +#include +#include "servo.h" + struct ts2phc_slave_array; +#define SERVO_SYNC_INTERVAL 1.0 + +struct clock { + LIST_ENTRY(clock) list; + LIST_ENTRY(clock) dst_list; + clockid_t clkid; + int phc_index; + int state; + int new_state; + struct servo *servo; + enum servo_state servo_state; + char *name; + int no_adj; + int is_destination; +}; + struct ts2phc_private { struct ts2phc_master *master; STAILQ_HEAD(slave_ifaces_head, ts2phc_slave) slaves; unsigned int n_slaves; struct ts2phc_slave_array *polling_array; struct config *cfg; + struct clock *source; + LIST_HEAD(clock_head, clock) clocks; }; +struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock); +struct clock *clock_add(struct ts2phc_private *priv, const char *device); +void clock_add_tstamp(struct clock *clock, struct timespec ts); +void clock_destroy(struct clock *clock); + #include "ts2phc_master.h" #include "ts2phc_slave.h" diff --git a/ts2phc_slave.c b/ts2phc_slave.c index 7b4fbfa..db44e29 100644 --- a/ts2phc_slave.c +++ b/ts2phc_slave.c @@ -26,21 +26,17 @@ #define NS_PER_SEC 1000000000LL #define SAMPLE_WEIGHT 1.0 -#define SERVO_SYNC_INTERVAL 1.0 struct ts2phc_slave { char *name; STAILQ_ENTRY(ts2phc_slave) list; struct ptp_pin_desc pin_desc; - enum servo_state state; unsigned int polarity; int32_t correction; uint32_t ignore_lower; uint32_t ignore_upper; - struct servo *servo; - clockid_t clk; + struct clock *clock; int no_adj; - int fd; }; struct ts2phc_slave_array { @@ -96,8 +92,10 @@ static int ts2phc_slave_array_create(struct ts2phc_private *priv) i++; } for (i = 0; i < priv->n_slaves; i++) { + struct ts2phc_slave *slave = polling_array->slave[i]; + polling_array->pfd[i].events = POLLIN | POLLPRI; - polling_array->pfd[i].fd = polling_array->slave[i]->fd; + polling_array->pfd[i].fd = CLOCKID_TO_FD(slave->clock->clkid); } priv->polling_array = polling_array; @@ -111,15 +109,15 @@ static void ts2phc_slave_array_destroy(struct ts2phc_private *priv) free(polling_array->slave); free(polling_array->pfd); - polling_array->slave = NULL; - polling_array->pfd = NULL; + free(polling_array); + priv->polling_array = NULL; } static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave) { struct pollfd pfd = { .events = POLLIN | POLLPRI, - .fd = slave->fd, + .fd = CLOCKID_TO_FD(slave->clock->clkid), }; struct ptp_extts_event event; int cnt, size; @@ -151,10 +149,9 @@ static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave) static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, const char *device) { - enum servo_type servo = config_get_int(priv->cfg, NULL, "clock_servo"); - int err, fadj, junk, max_adj, pulsewidth; struct ptp_extts_request extts; struct ts2phc_slave *slave; + int err, pulsewidth; slave = calloc(1, sizeof(*slave)); if (!slave) { @@ -183,33 +180,17 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, slave->ignore_upper = 1000000000 - pulsewidth; slave->ignore_lower = pulsewidth; - slave->clk = posix_clock_open(device, &junk); - if (slave->clk == CLOCK_INVALID) { + slave->clock = clock_add(priv, device); + if (!slave->clock) { pr_err("failed to open clock"); goto no_posix_clock; } - slave->no_adj = config_get_int(priv->cfg, NULL, "free_running"); - slave->fd = CLOCKID_TO_FD(slave->clk); - pr_debug("PHC slave %s has ptp index %d", device, junk); + pr_debug("PHC slave %s has ptp index %d", device, + slave->clock->phc_index); - fadj = (int) clockadj_get_freq(slave->clk); - /* Due to a bug in older kernels, the reading may silently fail - and return 0. Set the frequency back to make sure fadj is - the actual frequency of the clock. */ - clockadj_set_freq(slave->clk, fadj); - - max_adj = phc_max_adj(slave->clk); - - slave->servo = servo_create(priv->cfg, servo, -fadj, max_adj, 0); - if (!slave->servo) { - pr_err("failed to create servo"); - goto no_servo; - } - servo_sync_interval(slave->servo, SERVO_SYNC_INTERVAL); - - if (phc_number_pins(slave->clk) > 0) { - err = phc_pin_setfunc(slave->clk, &slave->pin_desc); + if (phc_number_pins(slave->clock->clkid) > 0) { + err = phc_pin_setfunc(slave->clock->clkid, &slave->pin_desc); if (err < 0) { pr_err("PTP_PIN_SETFUNC request failed"); goto no_pin_func; @@ -223,7 +204,8 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, memset(&extts, 0, sizeof(extts)); extts.index = slave->pin_desc.chan; extts.flags = 0; - if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) { + if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2, + &extts)) { pr_err(PTP_EXTTS_REQUEST_FAILED); } if (ts2phc_slave_clear_fifo(slave)) { @@ -233,9 +215,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, return slave; no_ext_ts: no_pin_func: - servo_destroy(slave->servo); -no_servo: - posix_clock_close(slave->clk); + clock_destroy(slave->clock); no_posix_clock: free(slave->name); free(slave); @@ -249,11 +229,11 @@ static void ts2phc_slave_destroy(struct ts2phc_slave *slave) memset(&extts, 0, sizeof(extts)); extts.index = slave->pin_desc.chan; extts.flags = 0; - if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) { + if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2, + &extts)) { pr_err(PTP_EXTTS_REQUEST_FAILED); } - servo_destroy(slave->servo); - posix_clock_close(slave->clk); + clock_destroy(slave->clock); free(slave->name); free(slave); } @@ -281,27 +261,22 @@ static int ts2phc_slave_event(struct ts2phc_slave *slave, return 0; } - if (!source_ts.valid) { - pr_debug("%s ignoring invalid master time stamp", slave->name); - return 0; - } - - adj = servo_sample(slave->servo, offset, extts_ts, - SAMPLE_WEIGHT, &slave->state); + adj = servo_sample(slave->clock->servo, offset, extts_ts, + SAMPLE_WEIGHT, &slave->clock->servo_state); pr_debug("%s master offset %10" PRId64 " s%d freq %+7.0f", - slave->name, offset, slave->state, adj); + slave->name, offset, slave->clock->servo_state, adj); - switch (slave->state) { + switch (slave->clock->servo_state) { case SERVO_UNLOCKED: break; case SERVO_JUMP: - clockadj_set_freq(slave->clk, -adj); - clockadj_step(slave->clk, -offset); + 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->clk, -adj); + clockadj_set_freq(slave->clock->clkid, -adj); break; } return 0; @@ -317,7 +292,7 @@ static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave, uint64_t event_ns, source_ns; int cnt; - cnt = read(slave->fd, &event, sizeof(event)); + cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event)); if (cnt != sizeof(event)) { pr_err("read extts event failed: %m"); return EXTTS_ERROR; @@ -389,7 +364,8 @@ int ts2phc_slave_arm(struct ts2phc_private *priv) STAILQ_FOREACH(slave, &priv->slaves, list) { extts.index = slave->pin_desc.chan; extts.flags = slave->polarity | PTP_ENABLE_FEATURE; - err = ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts); + err = ioctl(CLOCKID_TO_FD(slave->clock->clkid), + PTP_EXTTS_REQUEST2, &extts); if (err < 0) { pr_err(PTP_EXTTS_REQUEST_FAILED); return -1; @@ -445,6 +421,11 @@ int ts2phc_slave_poll(struct ts2phc_private *priv) 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++) { if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) { ts2phc_slave_event(polling_array->slave[i], source_ts);