diff --git a/ts2phc.c b/ts2phc.c index 2342858..4d5f065 100644 --- a/ts2phc.c +++ b/ts2phc.c @@ -11,23 +11,20 @@ #include "config.h" #include "interface.h" #include "print.h" -#include "ts2phc_master.h" -#include "ts2phc_slave.h" +#include "ts2phc.h" #include "version.h" struct interface { STAILQ_ENTRY(interface) list; }; -static void ts2phc_cleanup(struct config *cfg, struct ts2phc_master *master) +static void ts2phc_cleanup(struct ts2phc_private *priv) { - ts2phc_slave_cleanup(); - if (master) { - ts2phc_master_destroy(master); - } - if (cfg) { - config_destroy(cfg); - } + ts2phc_slave_cleanup(priv); + if (priv->master) + ts2phc_master_destroy(priv->master); + if (priv->cfg) + config_destroy(priv->cfg); } static void usage(char *progname) @@ -56,8 +53,8 @@ static void usage(char *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; + struct ts2phc_private priv = {0}; char *config = NULL, *progname; const char *pps_source = NULL; struct config *cfg = NULL; @@ -68,7 +65,7 @@ int main(int argc, char *argv[]) cfg = config_create(); if (!cfg) { - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } @@ -81,14 +78,14 @@ int main(int argc, char *argv[]) switch (c) { case 0: if (config_parse_option(cfg, opts[index].name, optarg)) { - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } break; case 'c': if (!config_create_interface(optarg, cfg)) { fprintf(stderr, "failed to add slave\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } have_slave = 1; @@ -99,7 +96,7 @@ int main(int argc, char *argv[]) case 'l': if (get_arg_val_i(c, optarg, &print_level, PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) { - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } config_set_int(cfg, "logging_level", print_level); @@ -116,29 +113,29 @@ int main(int argc, char *argv[]) case 's': if (pps_source) { fprintf(stderr, "too many PPS sources\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } pps_source = optarg; break; case 'v': - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); version_show(stdout); return 0; case 'h': - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); usage(progname); return -1; case '?': default: - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); usage(progname); return -1; } } if (config && (c = config_read(config, cfg))) { fprintf(stderr, "failed to read config\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } print_set_progname(progname); @@ -147,18 +144,21 @@ int main(int argc, char *argv[]) print_set_syslog(config_get_int(cfg, NULL, "use_syslog")); print_set_level(config_get_int(cfg, NULL, "logging_level")); + STAILQ_INIT(&priv.slaves); + priv.cfg = cfg; + 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); + ts2phc_cleanup(&priv); return -1; } pps_source = interface_name(iface); } else { - if (ts2phc_slave_add(cfg, interface_name(iface))) { + if (ts2phc_slave_add(&priv, interface_name(iface))) { fprintf(stderr, "failed to add slave\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } have_slave = 1; @@ -166,19 +166,19 @@ int main(int argc, char *argv[]) } if (!have_slave) { fprintf(stderr, "no slave clocks specified\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); usage(progname); return -1; } if (!pps_source) { fprintf(stderr, "no PPS source specified\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); usage(progname); return -1; } - if (ts2phc_slave_arm()) { - fprintf(stderr, "failed to arm slaves\n"); - ts2phc_cleanup(cfg, master); + if (ts2phc_slaves_init(&priv)) { + fprintf(stderr, "failed to initialize slaves\n"); + ts2phc_cleanup(&priv); return -1; } @@ -189,21 +189,21 @@ int main(int argc, char *argv[]) } else { pps_type = TS2PHC_MASTER_PHC; } - master = ts2phc_master_create(cfg, pps_source, pps_type); - if (!master) { + priv.master = ts2phc_master_create(cfg, pps_source, pps_type); + if (!priv.master) { fprintf(stderr, "failed to create master\n"); - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return -1; } while (is_running()) { - err = ts2phc_slave_poll(master); + err = ts2phc_slave_poll(&priv); if (err) { pr_err("poll failed"); break; } } - ts2phc_cleanup(cfg, master); + ts2phc_cleanup(&priv); return err; } diff --git a/ts2phc.h b/ts2phc.h new file mode 100644 index 0000000..e47ea47 --- /dev/null +++ b/ts2phc.h @@ -0,0 +1,23 @@ +/** + * @file ts2phc.h + * @brief Structure definitions for ts2phc + * @note Copyright 2020 Vladimir Oltean + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_TS2PHC_H +#define HAVE_TS2PHC_H + +struct ts2phc_slave_array; + +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; +}; + +#include "ts2phc_master.h" +#include "ts2phc_slave.h" + +#endif diff --git a/ts2phc_slave.c b/ts2phc_slave.c index 749efe5..7b4fbfa 100644 --- a/ts2phc_slave.c +++ b/ts2phc_slave.c @@ -21,8 +21,7 @@ #include "phc.h" #include "print.h" #include "servo.h" -#include "ts2phc_master.h" -#include "ts2phc_slave.h" +#include "ts2phc.h" #include "util.h" #define NS_PER_SEC 1000000000LL @@ -47,7 +46,7 @@ struct ts2phc_slave { struct ts2phc_slave_array { struct ts2phc_slave **slave; struct pollfd *pfd; -} polling_array; +}; struct ts2phc_source_timestamp { struct timespec ts; @@ -65,49 +64,55 @@ static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave, 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) +static int ts2phc_slave_array_create(struct ts2phc_private *priv) { + struct ts2phc_slave_array *polling_array; 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) { + polling_array = malloc(sizeof(*polling_array)); + if (!polling_array) { pr_err("low memory"); return -1; } - polling_array.pfd = malloc(ts2phc_n_slaves * sizeof(*polling_array.pfd)); - if (!polling_array.pfd) { + + polling_array->slave = malloc(priv->n_slaves * + sizeof(*polling_array->slave)); + if (!polling_array->slave) { pr_err("low memory"); - free(polling_array.slave); - polling_array.slave = NULL; + return -1; + } + polling_array->pfd = malloc(priv->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; + STAILQ_FOREACH(slave, &priv->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; + for (i = 0; i < priv->n_slaves; i++) { + polling_array->pfd[i].events = POLLIN | POLLPRI; + polling_array->pfd[i].fd = polling_array->slave[i]->fd; } + + priv->polling_array = polling_array; + return 0; } -static void ts2phc_slave_array_destroy(void) +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; + struct ts2phc_slave_array *polling_array = priv->polling_array; + + 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) @@ -143,9 +148,10 @@ static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave) return 0; } -static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *device) +static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv, + const char *device) { - enum servo_type servo = config_get_int(cfg, NULL, "clock_servo"); + 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; @@ -161,13 +167,18 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char * free(slave); return NULL; } - slave->pin_desc.index = config_get_int(cfg, device, "ts2phc.pin_index"); + slave->pin_desc.index = config_get_int(priv->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"); + slave->pin_desc.chan = config_get_int(priv->cfg, device, + "ts2phc.channel"); + slave->polarity = config_get_int(priv->cfg, device, + "ts2phc.extts_polarity"); + slave->correction = config_get_int(priv->cfg, device, + "ts2phc.extts_correction"); - pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth"); + pulsewidth = config_get_int(priv->cfg, device, + "ts2phc.pulsewidth"); pulsewidth /= 2; slave->ignore_upper = 1000000000 - pulsewidth; slave->ignore_lower = pulsewidth; @@ -177,7 +188,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char * pr_err("failed to open clock"); goto no_posix_clock; } - slave->no_adj = config_get_int(cfg, NULL, "free_running"); + 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); @@ -190,7 +201,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char * max_adj = phc_max_adj(slave->clk); - slave->servo = servo_create(cfg, servo, -fadj, max_adj, 0); + slave->servo = servo_create(priv->cfg, servo, -fadj, max_adj, 0); if (!slave->servo) { pr_err("failed to create servo"); goto no_servo; @@ -346,28 +357,28 @@ static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave, /* public methods */ -int ts2phc_slave_add(struct config *cfg, const char *name) +int ts2phc_slave_add(struct ts2phc_private *priv, const char *name) { struct ts2phc_slave *slave; /* Create each interface only once. */ - STAILQ_FOREACH(slave, &ts2phc_slaves, list) { + STAILQ_FOREACH(slave, &priv->slaves, list) { if (0 == strcmp(name, slave->name)) { return 0; } } - slave = ts2phc_slave_create(cfg, name); + slave = ts2phc_slave_create(priv, name); if (!slave) { pr_err("failed to create slave"); return -1; } - STAILQ_INSERT_TAIL(&ts2phc_slaves, slave, list); - ts2phc_n_slaves++; + STAILQ_INSERT_TAIL(&priv->slaves, slave, list); + priv->n_slaves++; return 0; } -int ts2phc_slave_arm(void) +int ts2phc_slave_arm(struct ts2phc_private *priv) { struct ptp_extts_request extts; struct ts2phc_slave *slave; @@ -375,7 +386,7 @@ int ts2phc_slave_arm(void) memset(&extts, 0, sizeof(extts)); - STAILQ_FOREACH(slave, &ts2phc_slaves, list) { + 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); @@ -387,29 +398,38 @@ int ts2phc_slave_arm(void) return 0; } -void ts2phc_slave_cleanup(void) +int ts2phc_slaves_init(struct ts2phc_private *priv) +{ + int err; + + err = ts2phc_slave_array_create(priv); + if (err) + return err; + + return ts2phc_slave_arm(priv); +} + +void ts2phc_slave_cleanup(struct ts2phc_private *priv) { struct ts2phc_slave *slave; - ts2phc_slave_array_destroy(); + ts2phc_slave_array_destroy(priv); - while ((slave = STAILQ_FIRST(&ts2phc_slaves))) { - STAILQ_REMOVE_HEAD(&ts2phc_slaves, list); + while ((slave = STAILQ_FIRST(&priv->slaves))) { + STAILQ_REMOVE_HEAD(&priv->slaves, list); ts2phc_slave_destroy(slave); - ts2phc_n_slaves--; + priv->n_slaves--; } } -int ts2phc_slave_poll(struct ts2phc_master *master) +int ts2phc_slave_poll(struct ts2phc_private *priv) { + struct ts2phc_slave_array *polling_array = priv->polling_array; struct ts2phc_source_timestamp source_ts; unsigned int i; int cnt, err; - if (ts2phc_slave_array_create()) { - return -1; - } - cnt = poll(polling_array.pfd, ts2phc_n_slaves, 2000); + cnt = poll(polling_array->pfd, priv->n_slaves, 2000); if (cnt < 0) { if (EINTR == errno) { return 0; @@ -422,12 +442,12 @@ int ts2phc_slave_poll(struct ts2phc_master *master) return 0; } - err = ts2phc_master_getppstime(master, &source_ts.ts); + err = ts2phc_master_getppstime(priv->master, &source_ts.ts); source_ts.valid = err ? false : true; - 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); + 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); } } return 0; diff --git a/ts2phc_slave.h b/ts2phc_slave.h index 2de5ab7..1bad9d2 100644 --- a/ts2phc_slave.h +++ b/ts2phc_slave.h @@ -7,14 +7,14 @@ #ifndef HAVE_TS2PHC_SLAVE_H #define HAVE_TS2PHC_SLAVE_H -#include "ts2phc_master.h" +#include "ts2phc.h" -int ts2phc_slave_add(struct config *cfg, const char *name); +int ts2phc_slave_add(struct ts2phc_private *priv, const char *name); -int ts2phc_slave_arm(void); +int ts2phc_slaves_init(struct ts2phc_private *priv); -void ts2phc_slave_cleanup(void); +void ts2phc_slave_cleanup(struct ts2phc_private *priv); -int ts2phc_slave_poll(struct ts2phc_master *master); +int ts2phc_slave_poll(struct ts2phc_private *priv); #endif