diff --git a/.gitignore b/.gitignore index 3dbcbfa..1e7d1ca 100644 --- a/.gitignore +++ b/.gitignore @@ -9,3 +9,4 @@ /phc_ctl /snmp4lptp /timemaster +/ts2phc diff --git a/config.c b/config.c index f20c5f7..b81149c 100644 --- a/config.c +++ b/config.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -169,6 +170,13 @@ static struct config_enum delay_mech_enu[] = { { NULL, 0 }, }; +static struct config_enum extts_polarity_enu[] = { + { "rising", PTP_RISING_EDGE }, + { "falling", PTP_FALLING_EDGE }, + { "both", PTP_RISING_EDGE | PTP_FALLING_EDGE }, + { NULL, 0 }, +}; + static struct config_enum hwts_filter_enu[] = { { "normal", HWTS_FILTER_NORMAL }, { "check", HWTS_FILTER_CHECK }, @@ -297,6 +305,12 @@ struct config_item config_tab[] = { GLOB_ITEM_INT("timeSource", INTERNAL_OSCILLATOR, 0x10, 0xfe), GLOB_ITEM_ENU("time_stamping", TS_HARDWARE, timestamping_enu), PORT_ITEM_INT("transportSpecific", 0, 0, 0x0F), + PORT_ITEM_INT("ts2phc.channel", 0, 0, INT_MAX), + PORT_ITEM_INT("ts2phc.extts_correction", 0, INT_MIN, INT_MAX), + PORT_ITEM_ENU("ts2phc.extts_polarity", PTP_RISING_EDGE, extts_polarity_enu), + PORT_ITEM_INT("ts2phc.master", 0, 0, 1), + PORT_ITEM_INT("ts2phc.pin_index", 0, 0, INT_MAX), + GLOB_ITEM_INT("ts2phc.pulsewidth", 500000000, 1000000, 999000000), PORT_ITEM_ENU("tsproc_mode", TSPROC_FILTER, tsproc_enu), GLOB_ITEM_INT("twoStepFlag", 1, 0, 1), GLOB_ITEM_INT("tx_timestamp_timeout", 1, 1, INT_MAX), diff --git a/configs/ts2phc-generic.cfg b/configs/ts2phc-generic.cfg new file mode 100644 index 0000000..69df2a9 --- /dev/null +++ b/configs/ts2phc-generic.cfg @@ -0,0 +1,18 @@ +# +# This example uses a PPS signal from a GPS receiver as an input to +# the SDP0 pin of an Intel i210 card. The pulse from the receiver has +# a width of 100 milliseconds. +# +# Important! The polarity is set to "both" because the i210 always +# time stamps both the rising and the falling edges of the input +# signal. +# +[global] +use_syslog 0 +verbose 1 +logging_level 6 +ts2phc.pulsewidth 100000000 +[eth6] +ts2phc.channel 0 +ts2phc.extts_polarity both +ts2phc.pin_index 0 diff --git a/makefile b/makefile index a23945a..9d8817a 100644 --- a/makefile +++ b/makefile @@ -22,10 +22,11 @@ CC = $(CROSS_COMPILE)gcc VER = -DVER=$(version) CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS) LDLIBS = -lm -lrt $(EXTRA_LDFLAGS) -PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster +PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster ts2phc FILTERS = filter.o mave.o mmedian.o SERVOS = linreg.o ntpshm.o nullf.o pi.o servo.o TRANSP = raw.o transport.o udp.o udp6.o uds.o +TS2PHC = ts2phc.o ts2phc_generic_master.o ts2phc_master.o ts2phc_slave.o OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \ e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o msg.o phc.o port.o \ port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) sk.o \ @@ -33,7 +34,7 @@ OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \ unicast_fsm.o unicast_service.o util.o version.o OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \ - sysoff.o timemaster.o + sysoff.o timemaster.o $(TS2PHC) SRC = $(OBJECTS:.o=.c) DEPEND = $(OBJECTS:.o=.d) srcdir := $(dir $(lastword $(MAKEFILE_LIST))) @@ -66,6 +67,9 @@ phc_ctl: phc_ctl.o phc.o sk.o util.o clockadj.o sysoff.o print.o version.o timemaster: phc.o print.o rtnl.o sk.o timemaster.o util.o version.o +ts2phc: config.o clockadj.o hash.o interface.o phc.o print.o $(SERVOS) sk.o \ + $(TS2PHC) util.o version.o + version.o: .version version.sh $(filter-out version.d,$(DEPEND)) .version: force diff --git a/ts2phc.c b/ts2phc.c new file mode 100644 index 0000000..abc54b6 --- /dev/null +++ b/ts2phc.c @@ -0,0 +1,200 @@ +/** + * @file ts2phc.c + * @brief Utility program to synchronize the PHC clock to external events + * @note Copyright (C) 2013 Balint Ferencz + * @note Based on the phc2sys utility + * @note Copyright (C) 2012 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#include + +#include "config.h" +#include "interface.h" +#include "print.h" +#include "ts2phc_master.h" +#include "ts2phc_slave.h" +#include "version.h" + +struct interface { + STAILQ_ENTRY(interface) list; +}; + +static void ts2phc_cleanup(struct config *cfg, struct ts2phc_master *master) +{ + ts2phc_slave_cleanup(); + if (master) { + ts2phc_master_destroy(master); + } + if (cfg) { + config_destroy(cfg); + } +} + +static void usage(char *progname) +{ + fprintf(stderr, + "\n" + "usage: %s [options]\n\n" + " -c [dev|name] phc slave clock (like /dev/ptp0 or eth0)\n" + " (may be specified multiple times)\n" + " -f [file] read configuration from 'file'\n" + " -h prints this message and exits\n" + " -l [num] set the logging level to 'num'\n" + " -m print messages to stdout\n" + " -q do not print messages to the syslog\n" + " -s [dev|name] source of the PPS signal\n" + " may take any of the following forms:\n" + " generic - an external 1-PPS without ToD information\n" + " -v prints the software version and exits\n" + "\n", + progname); +} + +int main(int argc, char *argv[]) +{ + int c, err = 0, have_slave = 0, index, print_level; + struct ts2phc_master *master = NULL; + enum ts2phc_master_type pps_type; + char *config = NULL, *progname; + const char *pps_source = NULL; + struct config *cfg = NULL; + struct interface *iface; + struct option *opts; + + handle_term_signals(); + + cfg = config_create(); + if (!cfg) { + ts2phc_cleanup(cfg, master); + return -1; + } + + opts = config_long_options(cfg); + + /* Process the command line arguments. */ + progname = strrchr(argv[0], '/'); + progname = progname ? 1 + progname : argv[0]; + while (EOF != (c = getopt_long(argc, argv, "c:f:hi:l:mqs:v", opts, &index))) { + switch (c) { + case 0: + if (config_parse_option(cfg, opts[index].name, optarg)) { + ts2phc_cleanup(cfg, master); + return -1; + } + break; + case 'c': + if (!config_create_interface(optarg, cfg)) { + fprintf(stderr, "failed to add slave\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + have_slave = 1; + break; + case 'f': + config = optarg; + break; + case 'l': + if (get_arg_val_i(c, optarg, &print_level, + PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) { + ts2phc_cleanup(cfg, master); + return -1; + } + config_set_int(cfg, "logging_level", print_level); + print_set_level(print_level); + break; + case 'm': + config_set_int(cfg, "verbose", 1); + print_set_verbose(1); + break; + case 'q': + config_set_int(cfg, "use_syslog", 0); + print_set_syslog(0); + break; + case 's': + if (pps_source) { + fprintf(stderr, "too many PPS sources\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + pps_source = optarg; + break; + case 'v': + ts2phc_cleanup(cfg, master); + version_show(stdout); + return 0; + case 'h': + ts2phc_cleanup(cfg, master); + usage(progname); + return -1; + case '?': + default: + ts2phc_cleanup(cfg, master); + usage(progname); + return -1; + } + } + if (config && (c = config_read(config, cfg))) { + fprintf(stderr, "failed to read config\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + print_set_progname(progname); + print_set_tag(config_get_string(cfg, NULL, "message_tag")); + print_set_verbose(config_get_int(cfg, NULL, "verbose")); + print_set_syslog(config_get_int(cfg, NULL, "use_syslog")); + print_set_level(config_get_int(cfg, NULL, "logging_level")); + + STAILQ_FOREACH(iface, &cfg->interfaces, list) { + if (1 == config_get_int(cfg, interface_name(iface), "ts2phc.master")) { + if (pps_source) { + fprintf(stderr, "too many PPS sources\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + pps_source = interface_name(iface); + } else { + if (ts2phc_slave_add(cfg, interface_name(iface))) { + fprintf(stderr, "failed to add slave\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + have_slave = 1; + } + } + if (!have_slave) { + fprintf(stderr, "no slave clocks specified\n"); + ts2phc_cleanup(cfg, master); + usage(progname); + return -1; + } + if (!pps_source) { + fprintf(stderr, "no PPS source specified\n"); + ts2phc_cleanup(cfg, master); + usage(progname); + return -1; + } + if (ts2phc_slave_arm()) { + fprintf(stderr, "failed to arm slaves\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + + pps_type = TS2PHC_MASTER_GENERIC; + master = ts2phc_master_create(cfg, pps_source, pps_type); + if (!master) { + fprintf(stderr, "failed to create master\n"); + ts2phc_cleanup(cfg, master); + return -1; + } + + while (is_running()) { + err = ts2phc_slave_poll(master); + if (err) { + pr_err("poll failed"); + break; + } + } + + ts2phc_cleanup(cfg, master); + return err; +} diff --git a/ts2phc_generic_master.c b/ts2phc_generic_master.c new file mode 100644 index 0000000..ad4f7f1 --- /dev/null +++ b/ts2phc_generic_master.c @@ -0,0 +1,63 @@ +/** + * @file ts2phc_generic_master.c + * @note Copyright (C) 2019 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#include +#include + +#include "missing.h" +#include "print.h" +#include "ts2phc_generic_master.h" +#include "ts2phc_master_private.h" +#include "util.h" + +struct ts2phc_generic_master { + struct ts2phc_master master; +}; + +static void ts2phc_generic_master_destroy(struct ts2phc_master *master) +{ + struct ts2phc_generic_master *s = + container_of(master, struct ts2phc_generic_master, master); + free(s); +} + +/* + * Returns the time on the PPS source device at which the most recent + * PPS event was generated. This implementation assumes that the + * system time is approximately correct. + */ +static int ts2phc_generic_master_getppstime(struct ts2phc_master *m, + struct timespec *ts) +{ + struct timex ntx; + int code; + + memset(&ntx, 0, sizeof(ntx)); + ntx.modes = ADJ_NANO; + code = adjtimex(&ntx); + if (code == -1) { + pr_err("adjtimex failed: %m"); + return -1; + } + ts->tv_sec = ntx.time.tv_sec + ntx.tai; + ts->tv_nsec = ntx.time.tv_usec; + + return 0; +} + +struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg, + const char *dev) +{ + struct ts2phc_generic_master *master; + + master = calloc(1, sizeof(*master)); + if (!master) { + return NULL; + } + master->master.destroy = ts2phc_generic_master_destroy; + master->master.getppstime = ts2phc_generic_master_getppstime; + + return &master->master; +} diff --git a/ts2phc_generic_master.h b/ts2phc_generic_master.h new file mode 100644 index 0000000..ac0ce4f --- /dev/null +++ b/ts2phc_generic_master.h @@ -0,0 +1,14 @@ +/** + * @file ts2phc_generic_master.h + * @note Copyright (C) 2019 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_TS2PHC_GENERIC_MASTER_H +#define HAVE_TS2PHC_GENERIC_MASTER_H + +#include "ts2phc_master.h" + +struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg, + const char *dev); + +#endif diff --git a/ts2phc_master.c b/ts2phc_master.c new file mode 100644 index 0000000..a1004f4 --- /dev/null +++ b/ts2phc_master.c @@ -0,0 +1,34 @@ +/** + * @file ts2phc_master.c + * @note Copyright (C) 2019 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#include "ts2phc_generic_master.h" +#include "ts2phc_master_private.h" + +struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev, + enum ts2phc_master_type type) +{ + struct ts2phc_master *master = NULL; + + switch (type) { + case TS2PHC_MASTER_GENERIC: + master = ts2phc_generic_master_create(cfg, dev); + break; + case TS2PHC_MASTER_NMEA: + break; + case TS2PHC_MASTER_PHC: + break; + } + return master; +} + +void ts2phc_master_destroy(struct ts2phc_master *master) +{ + master->destroy(master); +} + +int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts) +{ + return master->getppstime(master, ts); +} diff --git a/ts2phc_master.h b/ts2phc_master.h new file mode 100644 index 0000000..79765b6 --- /dev/null +++ b/ts2phc_master.h @@ -0,0 +1,52 @@ +/** + * @file ts2phc_master.h + * @note Copyright (C) 2019 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_TS2PHC_MASTER_H +#define HAVE_TS2PHC_MASTER_H + +#include + +struct config; + +/** + * Opaque type + */ +struct ts2phc_master; + +/** + * Defines the available PPS master clocks. + */ +enum ts2phc_master_type { + TS2PHC_MASTER_GENERIC, + TS2PHC_MASTER_NMEA, + TS2PHC_MASTER_PHC, +}; + +/** + * Create a new instance of a PPS master clock. + * @param cfg Pointer to a valid configuration. + * @param dev Name of the master clock or NULL. + * @param type The type of the clock to create. + * @return A pointer to a new PPS master clock on success, NULL otherwise. + */ +struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev, + enum ts2phc_master_type type); + +/** + * Destroy an instance of a PPS master clock. + * @param master Pointer to a master obtained via @ref ts2phc_master_create(). + */ +void ts2phc_master_destroy(struct ts2phc_master *master); + +/** + * Returns the time on the PPS source device at which the most recent + * PPS event was generated. + * @param master Pointer to a master obtained via @ref ts2phc_master_create(). + * @param ts Buffer to hold the time of the last PPS event. + * @return Zero if the reported time is valid, non-zero otherwise. + */ +int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts); + +#endif diff --git a/ts2phc_master_private.h b/ts2phc_master_private.h new file mode 100644 index 0000000..463a1f0 --- /dev/null +++ b/ts2phc_master_private.h @@ -0,0 +1,20 @@ +/** + * @file ts2phc_master_private.h + * @note Copyright (C) 2019 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_TS2PHC_MASTER_PRIVATE_H +#define HAVE_TS2PHC_MASTER_PRIVATE_H + +#include +#include + +#include "contain.h" +#include "ts2phc_master.h" + +struct ts2phc_master { + void (*destroy)(struct ts2phc_master *ts2phc_master); + int (*getppstime)(struct ts2phc_master *master, struct timespec *ts); +}; + +#endif diff --git a/ts2phc_slave.c b/ts2phc_slave.c new file mode 100644 index 0000000..82848d0 --- /dev/null +++ b/ts2phc_slave.c @@ -0,0 +1,426 @@ +/** + * @file ts2phc_slave.c + * @brief Utility program to synchronize the PHC clock to external events + * @note Copyright (C) 2019 Balint Ferencz + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "config.h" +#include "clockadj.h" +#include "missing.h" +#include "phc.h" +#include "print.h" +#include "servo.h" +#include "ts2phc_master.h" +#include "ts2phc_slave.h" +#include "util.h" + +#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; + int no_adj; + int fd; +}; + +struct ts2phc_slave_array { + struct ts2phc_slave **slave; + struct pollfd *pfd; +} polling_array; + +struct ts2phc_source_timestamp { + struct timespec ts; + int valid; +}; + +enum extts_result { + EXTTS_ERROR = -1, + EXTTS_OK = 0, + EXTTS_IGNORE = 1, +}; + +static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave, + struct ts2phc_source_timestamp ts, + int64_t *offset, + uint64_t *local_ts); + +static STAILQ_HEAD(slave_ifaces_head, ts2phc_slave) ts2phc_slaves = + STAILQ_HEAD_INITIALIZER(ts2phc_slaves); + +static unsigned int ts2phc_n_slaves; + +static int ts2phc_slave_array_create(void) +{ + struct ts2phc_slave *slave; + unsigned int i; + + if (polling_array.slave) { + return 0; + } + polling_array.slave = malloc(ts2phc_n_slaves * sizeof(*polling_array.slave)); + if (!polling_array.slave) { + pr_err("low memory"); + return -1; + } + polling_array.pfd = malloc(ts2phc_n_slaves * sizeof(*polling_array.pfd)); + if (!polling_array.pfd) { + pr_err("low memory"); + free(polling_array.slave); + polling_array.slave = NULL; + return -1; + } + i = 0; + STAILQ_FOREACH(slave, &ts2phc_slaves, list) { + polling_array.slave[i] = slave; + i++; + } + for (i = 0; i < ts2phc_n_slaves; i++) { + polling_array.pfd[i].events = POLLIN | POLLPRI; + polling_array.pfd[i].fd = polling_array.slave[i]->fd; + } + return 0; +} + +static void ts2phc_slave_array_destroy(void) +{ + free(polling_array.slave); + free(polling_array.pfd); + polling_array.slave = NULL; + polling_array.pfd = NULL; +} + +static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave) +{ + struct pollfd pfd = { + .events = POLLIN | POLLPRI, + .fd = slave->fd, + }; + struct ptp_extts_event event; + int cnt, size; + + while (1) { + cnt = poll(&pfd, 1, 0); + if (cnt < 0) { + if (EINTR == errno) { + continue; + } else { + pr_emerg("poll failed"); + return -1; + } + } else if (!cnt) { + break; + } + size = read(pfd.fd, &event, sizeof(event)); + if (size != sizeof(event)) { + pr_err("read failed"); + return -1; + } + pr_debug("%s SKIP extts index %u at %lld.%09u", + slave->name, event.index, event.t.sec, event.t.nsec); + } + + return 0; +} + +static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *device) +{ + enum servo_type servo = config_get_int(cfg, NULL, "clock_servo"); + int err, fadj, junk, max_adj, pulsewidth; + struct ptp_extts_request extts; + struct ts2phc_slave *slave; + + slave = calloc(1, sizeof(*slave)); + if (!slave) { + pr_err("low memory"); + return NULL; + } + slave->name = strdup(device); + if (!slave->name) { + pr_err("low memory"); + free(slave); + return NULL; + } + slave->pin_desc.index = config_get_int(cfg, device, "ts2phc.pin_index"); + slave->pin_desc.func = PTP_PF_EXTTS; + slave->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel"); + slave->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity"); + slave->correction = config_get_int(cfg, device, "ts2phc.extts_correction"); + + pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth"); + pulsewidth /= 2; + slave->ignore_upper = 1000000000 - pulsewidth; + slave->ignore_lower = pulsewidth; + + slave->clk = posix_clock_open(device, &junk); + if (slave->clk == CLOCK_INVALID) { + pr_err("failed to open clock"); + goto no_posix_clock; + } + slave->no_adj = config_get_int(cfg, NULL, "free_running"); + slave->fd = CLOCKID_TO_FD(slave->clk); + + pr_debug("PHC slave %s has ptp index %d", device, junk); + + 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(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 (err < 0) { + pr_err("PTP_PIN_SETFUNC request failed"); + goto no_pin_func; + } + } + + /* + * Disable external time stamping, and then read out any stale + * time stamps. + */ + memset(&extts, 0, sizeof(extts)); + extts.index = slave->pin_desc.chan; + extts.flags = 0; + if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) { + pr_err(PTP_EXTTS_REQUEST_FAILED); + } + if (ts2phc_slave_clear_fifo(slave)) { + goto no_ext_ts; + } + + return slave; +no_ext_ts: +no_pin_func: + servo_destroy(slave->servo); +no_servo: + close(slave->fd); +no_posix_clock: + free(slave->name); + free(slave); + return NULL; +} + +static void ts2phc_slave_destroy(struct ts2phc_slave *slave) +{ + struct ptp_extts_request extts; + + memset(&extts, 0, sizeof(extts)); + extts.index = slave->pin_desc.chan; + extts.flags = 0; + if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) { + pr_err(PTP_EXTTS_REQUEST_FAILED); + } + posix_clock_close(slave->clk); + free(slave->name); + free(slave); +} + +static int ts2phc_slave_event(struct ts2phc_slave *slave, + struct ts2phc_source_timestamp source_ts) +{ + enum extts_result result; + uint64_t extts_ts; + int64_t offset; + double adj; + + result = ts2phc_slave_offset(slave, source_ts, &offset, &extts_ts); + switch (result) { + case EXTTS_ERROR: + return -1; + case EXTTS_OK: + break; + case EXTTS_IGNORE: + return 0; + } + + if (slave->no_adj) { + pr_info("%s master offset %10" PRId64, slave->name, offset); + return 0; + } + + adj = servo_sample(slave->servo, offset, extts_ts, + SAMPLE_WEIGHT, &slave->state); + + pr_debug("%s master offset %10" PRId64 " s%d freq %+7.0f", + slave->name, offset, slave->state, adj); + + switch (slave->state) { + case SERVO_UNLOCKED: + break; + case SERVO_JUMP: + clockadj_set_freq(slave->clk, -adj); + clockadj_step(slave->clk, -offset); + break; + case SERVO_LOCKED: + case SERVO_LOCKED_STABLE: + clockadj_set_freq(slave->clk, -adj); + break; + } + return 0; +} + +static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave, + struct ts2phc_source_timestamp src, + int64_t *offset, + uint64_t *local_ts) +{ + struct timespec source_ts = src.ts; + struct ptp_extts_event event; + uint64_t event_ns, source_ns; + int cnt; + + cnt = read(slave->fd, &event, sizeof(event)); + if (cnt != sizeof(event)) { + pr_err("read extts event failed: %m"); + return EXTTS_ERROR; + } + if (event.index != slave->pin_desc.chan) { + pr_err("extts on unexpected channel"); + return EXTTS_ERROR; + } + event_ns = event.t.sec * NS_PER_SEC; + event_ns += event.t.nsec; + + if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) && + source_ts.tv_nsec > slave->ignore_lower && + source_ts.tv_nsec < slave->ignore_upper) { + + pr_debug("%s SKIP extts index %u at %lld.%09u src %" PRIi64 ".%ld", + slave->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++; + } + source_ns = source_ts.tv_sec * NS_PER_SEC; + *offset = event_ns + slave->correction - source_ns; + *local_ts = event_ns + slave->correction; + + pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64 + ".%ld diff %" PRId64, + slave->name, event.index, event.t.sec, event.t.nsec, + slave->correction, + (int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset); + + return EXTTS_OK; +} + +/* public methods */ + +int ts2phc_slave_add(struct config *cfg, const char *name) +{ + struct ts2phc_slave *slave; + + /* Create each interface only once. */ + STAILQ_FOREACH(slave, &ts2phc_slaves, list) { + if (0 == strcmp(name, slave->name)) { + return 0; + } + } + slave = ts2phc_slave_create(cfg, name); + if (!slave) { + pr_err("failed to create slave"); + return -1; + } + STAILQ_INSERT_TAIL(&ts2phc_slaves, slave, list); + ts2phc_n_slaves++; + + return 0; +} + +int ts2phc_slave_arm(void) +{ + struct ptp_extts_request extts; + struct ts2phc_slave *slave; + int err; + + memset(&extts, 0, sizeof(extts)); + + STAILQ_FOREACH(slave, &ts2phc_slaves, list) { + extts.index = slave->pin_desc.chan; + extts.flags = slave->polarity | PTP_ENABLE_FEATURE; + err = ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts); + if (err < 0) { + pr_err(PTP_EXTTS_REQUEST_FAILED); + return -1; + } + } + return 0; +} + +void ts2phc_slave_cleanup(void) +{ + struct ts2phc_slave *slave; + + ts2phc_slave_array_destroy(); + + while ((slave = STAILQ_FIRST(&ts2phc_slaves))) { + STAILQ_REMOVE_HEAD(&ts2phc_slaves, list); + ts2phc_slave_destroy(slave); + ts2phc_n_slaves--; + } +} + +int ts2phc_slave_poll(struct ts2phc_master *master) +{ + struct ts2phc_source_timestamp source_ts; + unsigned int i; + int cnt; + + if (ts2phc_slave_array_create()) { + return -1; + } + cnt = poll(polling_array.pfd, ts2phc_n_slaves, 2000); + if (cnt < 0) { + if (EINTR == errno) { + return 0; + } else { + pr_emerg("poll failed"); + return -1; + } + } else if (!cnt) { + pr_debug("poll returns zero, no events"); + return 0; + } + + source_ts.valid = ts2phc_master_getppstime(master, &source_ts.ts); + + for (i = 0; i < ts2phc_n_slaves; i++) { + if (polling_array.pfd[i].revents & (POLLIN|POLLPRI)) { + ts2phc_slave_event(polling_array.slave[i], source_ts); + } + } + return 0; +} diff --git a/ts2phc_slave.h b/ts2phc_slave.h new file mode 100644 index 0000000..2de5ab7 --- /dev/null +++ b/ts2phc_slave.h @@ -0,0 +1,20 @@ +/** + * @file ts2phc_slave.h + * @brief Utility program to synchronize the PHC clock to external events + * @note Copyright (C) 2019 Balint Ferencz + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_TS2PHC_SLAVE_H +#define HAVE_TS2PHC_SLAVE_H + +#include "ts2phc_master.h" + +int ts2phc_slave_add(struct config *cfg, const char *name); + +int ts2phc_slave_arm(void); + +void ts2phc_slave_cleanup(void); + +int ts2phc_slave_poll(struct ts2phc_master *master); + +#endif