From 0c5c39a8cd3675d91e872a75d75854907950957d Mon Sep 17 00:00:00 2001 From: Andre Mauricio Zelak Date: Mon, 12 Jun 2023 13:47:47 -0300 Subject: [PATCH 11/56] phc2sys: extract PMC functionality into a smaller struct pmc_node This creates a smaller structure within phc2sys_private, which embeds all properties related to the PMC. This structure is called "pmc_node", which is somewhat reminiscent of the old name of phc2sys_private (struct node). But the advantage is that struct pmc_node can be reused by other modules. The phc2sys code that is executed upon a subscription update, recv_subscribed, is now refactored into a function pointer callback. It is imaginable that other programs might to do other things in it. Note that putting this function pointer in struct pmc_node is, long term, maybe not the best of choices. It is only needed from the run_pmc_events() code path, and could be therefore passed as a more local callback to that function only. However, for that, further refactoring is needed inside the common run_pmc() function, so that is being left for another time. Signed-off-by: Vladimir Oltean [commit 1ca1419ad7e6cc04cf893f5a9ca449a90f39f4e0 upstream] Signed-off-by: Andre Mauricio Zelak --- phc2sys.c | 228 ++++++++++++++++++++++++++++++------------------------ 1 file changed, 125 insertions(+), 103 deletions(-) diff --git a/phc2sys.c b/phc2sys.c index 7959015..86b9822 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -39,6 +39,7 @@ #include "clockadj.h" #include "clockcheck.h" +#include "contain.h" #include "ds.h" #include "fsm.h" #include "missing.h" @@ -99,23 +100,34 @@ struct port { struct clock *clock; }; +struct pmc_node; + +typedef int pmc_node_recv_subscribed_t(struct pmc_node *node, + struct ptp_message *msg, + int excluded); + +struct pmc_node { + struct pmc *pmc; + int pmc_ds_requested; + uint64_t pmc_last_update; + int sync_offset; + int leap; + int utc_offset_traceable; + int clock_identity_set; + struct ClockIdentity clock_identity; + pmc_node_recv_subscribed_t *recv_subscribed; +}; + struct phc2sys_private { unsigned int stats_max_count; int sanity_freq_limit; enum servo_type servo_type; int phc_readings; double phc_interval; - int sync_offset; int forced_sync_offset; - int utc_offset_traceable; - int leap; int kernel_leap; - struct pmc *pmc; - int pmc_ds_requested; - uint64_t pmc_last_update; int state_changed; - int clock_identity_set; - struct ClockIdentity clock_identity; + struct pmc_node node; LIST_HEAD(port_head, port) ports; LIST_HEAD(clock_head, clock) clocks; LIST_HEAD(dst_clock_head, clock) dst_clocks; @@ -125,16 +137,16 @@ struct phc2sys_private { static struct config *phc2sys_config; -static int update_pmc(struct phc2sys_private *priv, int subscribe); +static int update_pmc_node(struct pmc_node *node, int subscribe); static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock, int64_t offset, uint64_t ts); -static int run_pmc_get_utc_offset(struct phc2sys_private *priv, +static int run_pmc_get_utc_offset(struct pmc_node *node, int timeout); -static void run_pmc_events(struct phc2sys_private *priv); +static void run_pmc_events(struct pmc_node *node); static int normalize_state(int state); -static int run_pmc_port_properties(struct phc2sys_private *priv, +static int run_pmc_port_properties(struct pmc_node *node, int timeout, unsigned int port, int *state, int *tstamping, char *iface); @@ -325,7 +337,7 @@ static void clock_reinit(struct phc2sys_private *priv, struct clock *clock, LIST_FOREACH(p, &priv->ports, list) { if (p->clock == clock) { - ret = run_pmc_port_properties(priv, 1000, p->number, + ret = run_pmc_port_properties(&priv->node, 1000, p->number, &state, ×tamping, iface); if (ret > 0) @@ -660,7 +672,7 @@ static int do_pps_loop(struct phc2sys_private *priv, struct clock *clock, if (src == CLOCK_INVALID) { /* The sync offset can't be applied with PPS alone. */ - priv->sync_offset = 0; + priv->node.sync_offset = 0; } else { enable_pps_output(priv->master->clkid); } @@ -691,7 +703,7 @@ static int do_pps_loop(struct phc2sys_private *priv, struct clock *clock, pps_offset = pps_ts - phc_ts; } - if (update_pmc(priv, 0) < 0) + if (update_pmc_node(&priv->node, 0) < 0) continue; update_clock(priv, clock, pps_offset, pps_ts, -1); } @@ -729,15 +741,15 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions) while (is_running()) { clock_nanosleep(CLOCK_MONOTONIC, 0, &interval, NULL); - if (update_pmc(priv, subscriptions) < 0) + if (update_pmc_node(&priv->node, subscriptions) < 0) continue; if (subscriptions) { - run_pmc_events(priv); + run_pmc_events(&priv->node); if (priv->state_changed) { /* force getting offset, as it may have * changed after the port state change */ - if (run_pmc_get_utc_offset(priv, 1000) <= 0) { + if (run_pmc_get_utc_offset(&priv->node, 1000) <= 0) { pr_err("failed to get UTC offset"); continue; } @@ -800,13 +812,12 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions) return 0; } -static int check_clock_identity(struct phc2sys_private *priv, - struct ptp_message *msg) +static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg) { - if (!priv->clock_identity_set) + if (!node->clock_identity_set) return 1; - return cid_eq(&priv->clock_identity, - &msg->header.sourcePortIdentity.clockIdentity); + return cid_eq(&node->clock_identity, + &msg->header.sourcePortIdentity.clockIdentity); } static int is_msg_mgt(struct ptp_message *msg) @@ -876,9 +887,13 @@ static int clock_compute_state(struct phc2sys_private *priv, return state; } -static int recv_subscribed(struct phc2sys_private *priv, - struct ptp_message *msg, int excluded) +#define node_to_phc2sys(node) \ + container_of(node, struct phc2sys_private, node) + +static int phc2sys_recv_subscribed(struct pmc_node *node, + struct ptp_message *msg, int excluded) { + struct phc2sys_private *priv = node_to_phc2sys(node); int mgt_id, state; struct portDS *pds; struct port *port; @@ -913,29 +928,28 @@ static int recv_subscribed(struct phc2sys_private *priv, return 0; } -static void send_subscription(struct phc2sys_private *priv) +static void send_subscription(struct pmc_node *node) { struct subscribe_events_np sen; memset(&sen, 0, sizeof(sen)); sen.duration = PMC_SUBSCRIBE_DURATION; sen.bitmask[0] = 1 << NOTIFY_PORT_STATE; - pmc_send_set_action(priv->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen)); + pmc_send_set_action(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen)); } -static int init_pmc(struct config *cfg, struct phc2sys_private *priv) +static int init_pmc_node(struct config *cfg, struct pmc_node *node, + const char *uds, + pmc_node_recv_subscribed_t *recv_subscribed) { - char uds_local[MAX_IFNAME_SIZE + 1]; - - snprintf(uds_local, sizeof(uds_local), "/var/run/phc2sys.%d", - getpid()); - priv->pmc = pmc_create(cfg, TRANS_UDS, uds_local, 0, + node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0, config_get_int(cfg, NULL, "domainNumber"), config_get_int(cfg, NULL, "transportSpecific") << 4, 1); - if (!priv->pmc) { + if (!node->pmc) { pr_err("failed to create pmc"); return -1; } + node->recv_subscribed = recv_subscribed; return 0; } @@ -946,7 +960,7 @@ static int init_pmc(struct config *cfg, struct phc2sys_private *priv) * -1: error reported by the other side * -2: local error, fatal */ -static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id, +static int run_pmc(struct pmc_node *node, int timeout, int ds_id, struct ptp_message **msg) { #define N_FD 1 @@ -954,9 +968,9 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id, int cnt, res; while (1) { - pollfd[0].fd = pmc_get_transport_fd(priv->pmc); + pollfd[0].fd = pmc_get_transport_fd(node->pmc); pollfd[0].events = POLLIN|POLLPRI; - if (!priv->pmc_ds_requested && ds_id >= 0) + if (!node->pmc_ds_requested && ds_id >= 0) pollfd[0].events |= POLLOUT; cnt = poll(pollfd, N_FD, timeout); @@ -966,7 +980,7 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id, } if (!cnt) { /* Request the data set again in the next run. */ - priv->pmc_ds_requested = 0; + node->pmc_ds_requested = 0; return 0; } @@ -975,24 +989,24 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id, !(pollfd[0].revents & (POLLIN|POLLPRI))) { switch (ds_id) { case TLV_SUBSCRIBE_EVENTS_NP: - send_subscription(priv); + send_subscription(node); break; default: - pmc_send_get_action(priv->pmc, ds_id); + pmc_send_get_action(node->pmc, ds_id); break; } - priv->pmc_ds_requested = 1; + node->pmc_ds_requested = 1; } if (!(pollfd[0].revents & (POLLIN|POLLPRI))) continue; - *msg = pmc_recv(priv->pmc); + *msg = pmc_recv(node->pmc); if (!*msg) continue; - if (!check_clock_identity(priv, *msg)) { + if (!check_clock_identity(node, *msg)) { msg_put(*msg); *msg = NULL; continue; @@ -1000,29 +1014,29 @@ static int run_pmc(struct phc2sys_private *priv, int timeout, int ds_id, res = is_msg_mgt(*msg); if (res < 0 && get_mgt_err_id(*msg) == ds_id) { - priv->pmc_ds_requested = 0; + node->pmc_ds_requested = 0; return -1; } - if (res <= 0 || recv_subscribed(priv, *msg, ds_id) || + if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) || get_mgt_id(*msg) != ds_id) { msg_put(*msg); *msg = NULL; continue; } - priv->pmc_ds_requested = 0; + node->pmc_ds_requested = 0; return 1; } } -static int run_pmc_wait_sync(struct phc2sys_private *priv, int timeout) +static int run_pmc_wait_sync(struct pmc_node *node, int timeout) { struct ptp_message *msg; - int res; - void *data; Enumeration8 portState; + void *data; + int res; while (1) { - res = run_pmc(priv, timeout, TLV_PORT_DATA_SET, &msg); + res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg); if (res <= 0) return res; @@ -1036,47 +1050,47 @@ static int run_pmc_wait_sync(struct phc2sys_private *priv, int timeout) return 1; } /* try to get more data sets (for other ports) */ - priv->pmc_ds_requested = 1; + node->pmc_ds_requested = 1; } } -static int run_pmc_get_utc_offset(struct phc2sys_private *priv, int timeout) +static int run_pmc_get_utc_offset(struct pmc_node *node, int timeout) { struct ptp_message *msg; int res; struct timePropertiesDS *tds; - res = run_pmc(priv, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg); + res = run_pmc(node, timeout, TLV_TIME_PROPERTIES_DATA_SET, &msg); if (res <= 0) return res; tds = (struct timePropertiesDS *)get_mgt_data(msg); if (tds->flags & PTP_TIMESCALE) { - priv->sync_offset = tds->currentUtcOffset; + node->sync_offset = tds->currentUtcOffset; if (tds->flags & LEAP_61) - priv->leap = 1; + node->leap = 1; else if (tds->flags & LEAP_59) - priv->leap = -1; + node->leap = -1; else - priv->leap = 0; - priv->utc_offset_traceable = tds->flags & UTC_OFF_VALID && + node->leap = 0; + node->utc_offset_traceable = tds->flags & UTC_OFF_VALID && tds->flags & TIME_TRACEABLE; } else { - priv->sync_offset = 0; - priv->leap = 0; - priv->utc_offset_traceable = 0; + node->sync_offset = 0; + node->leap = 0; + node->utc_offset_traceable = 0; } msg_put(msg); return 1; } -static int run_pmc_get_number_ports(struct phc2sys_private *priv, int timeout) +static int run_pmc_get_number_ports(struct pmc_node *node, int timeout) { struct ptp_message *msg; int res; struct defaultDS *dds; - res = run_pmc(priv, timeout, TLV_DEFAULT_DATA_SET, &msg); + res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); if (res <= 0) return res; @@ -1086,36 +1100,36 @@ static int run_pmc_get_number_ports(struct phc2sys_private *priv, int timeout) return res; } -static int run_pmc_subscribe(struct phc2sys_private *priv, int timeout) +static int run_pmc_subscribe(struct pmc_node *node, int timeout) { struct ptp_message *msg; int res; - res = run_pmc(priv, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg); + res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg); if (res <= 0) return res; msg_put(msg); return 1; } -static void run_pmc_events(struct phc2sys_private *priv) +static void run_pmc_events(struct pmc_node *node) { struct ptp_message *msg; - run_pmc(priv, 0, -1, &msg); + run_pmc(node, 0, -1, &msg); } -static int run_pmc_port_properties(struct phc2sys_private *priv, int timeout, - unsigned int port, - int *state, int *tstamping, char *iface) +static int run_pmc_port_properties(struct pmc_node *node, int timeout, + unsigned int port, int *state, + int *tstamping, char *iface) { struct ptp_message *msg; int res, len; struct port_properties_np *ppn; - pmc_target_port(priv->pmc, port); + pmc_target_port(node->pmc, port); while (1) { - res = run_pmc(priv, timeout, TLV_PORT_PROPERTIES_NP, &msg); + res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg); if (res <= 0) goto out; @@ -1138,32 +1152,35 @@ static int run_pmc_port_properties(struct phc2sys_private *priv, int timeout, break; } out: - pmc_target_all(priv->pmc); + pmc_target_all(node->pmc); return res; } -static int run_pmc_clock_identity(struct phc2sys_private *priv, int timeout) +static int run_pmc_clock_identity(struct pmc_node *node, int timeout) { struct ptp_message *msg; struct defaultDS *dds; int res; - res = run_pmc(priv, timeout, TLV_DEFAULT_DATA_SET, &msg); + res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); if (res <= 0) return res; dds = (struct defaultDS *)get_mgt_data(msg); - memcpy(&priv->clock_identity, &dds->clockIdentity, + memcpy(&node->clock_identity, &dds->clockIdentity, sizeof(struct ClockIdentity)); - priv->clock_identity_set = 1; + node->clock_identity_set = 1; msg_put(msg); return 1; } -static void close_pmc(struct phc2sys_private *priv) +static void close_pmc_node(struct pmc_node *node) { - pmc_destroy(priv->pmc); - priv->pmc = NULL; + if (!node->pmc) + return; + + pmc_destroy(node->pmc); + node->pmc = NULL; } static int auto_init_ports(struct phc2sys_private *priv, int add_rt) @@ -1178,7 +1195,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt) while (1) { if (!is_running()) return -1; - res = run_pmc_clock_identity(priv, 1000); + res = run_pmc_clock_identity(&priv->node, 1000); if (res < 0) return -1; if (res > 0) @@ -1187,20 +1204,20 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt) pr_notice("Waiting for ptp4l..."); } - number_ports = run_pmc_get_number_ports(priv, 1000); + number_ports = run_pmc_get_number_ports(&priv->node, 1000); if (number_ports <= 0) { pr_err("failed to get number of ports"); return -1; } - res = run_pmc_subscribe(priv, 1000); + res = run_pmc_subscribe(&priv->node, 1000); if (res <= 0) { pr_err("failed to subscribe"); return -1; } for (i = 1; i <= number_ports; i++) { - res = run_pmc_port_properties(priv, 1000, i, &state, + res = run_pmc_port_properties(&priv->node, 1000, i, &state, ×tamping, iface); if (res == -1) { /* port does not exist, ignore the port */ @@ -1237,7 +1254,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt) } /* get initial offset */ - if (run_pmc_get_utc_offset(priv, 1000) <= 0) { + if (run_pmc_get_utc_offset(&priv->node, 1000) <= 0) { pr_err("failed to get UTC offset"); return -1; } @@ -1245,7 +1262,7 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt) } /* Returns: -1 in case of error, 0 otherwise */ -static int update_pmc(struct phc2sys_private *priv, int subscribe) +static int update_pmc_node(struct pmc_node *node, int subscribe) { struct timespec tp; uint64_t ts; @@ -1256,13 +1273,13 @@ static int update_pmc(struct phc2sys_private *priv, int subscribe) } ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec; - if (priv->pmc && - !(ts > priv->pmc_last_update && - ts - priv->pmc_last_update < PMC_UPDATE_INTERVAL)) { + if (node->pmc && + !(ts > node->pmc_last_update && + ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) { if (subscribe) - run_pmc_subscribe(priv, 0); - if (run_pmc_get_utc_offset(priv, 0) > 0) - priv->pmc_last_update = ts; + run_pmc_subscribe(node, 0); + if (run_pmc_get_utc_offset(node, 0) > 0) + node->pmc_last_update = ts; } return 0; @@ -1272,9 +1289,9 @@ static int update_pmc(struct phc2sys_private *priv, int subscribe) static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock, int64_t offset, uint64_t ts) { - int clock_leap, node_leap = priv->leap; + int clock_leap, node_leap = priv->node.leap; - clock->sync_offset = priv->sync_offset; + clock->sync_offset = priv->node.sync_offset; if ((node_leap || clock->leap_set) && clock->is_utc != priv->master->is_utc) { @@ -1315,7 +1332,7 @@ static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock, } } - if (priv->utc_offset_traceable && + if (priv->node.utc_offset_traceable && clock->utc_offset_set != clock->sync_offset) { if (clock->clkid == CLOCK_REALTIME) sysclk_set_tai_offset(clock->sync_offset); @@ -1370,6 +1387,7 @@ static void usage(char *progname) int main(int argc, char *argv[]) { char *config = NULL, *dst_name = NULL, *progname, *src_name = NULL; + char uds_local[MAX_IFNAME_SIZE + 1]; struct clock *src, *dst; struct config *cfg; struct option *opts; @@ -1478,7 +1496,7 @@ int main(int argc, char *argv[]) goto end; break; case 'O': - if (get_arg_val_i(c, optarg, &priv.sync_offset, + if (get_arg_val_i(c, optarg, &priv.node.sync_offset, INT_MIN, INT_MAX)) goto end; priv.forced_sync_offset = -1; @@ -1605,8 +1623,12 @@ int main(int argc, char *argv[]) priv.sanity_freq_limit = config_get_int(cfg, NULL, "sanity_freq_limit"); priv.default_sync = config_get_int(cfg, NULL, "default_sync"); + snprintf(uds_local, sizeof(uds_local), "/var/run/phc2sys.%d", + getpid()); + if (autocfg) { - if (init_pmc(cfg, &priv)) + if (init_pmc_node(cfg, &priv.node, uds_local, + phc2sys_recv_subscribed)) goto end; if (auto_init_ports(&priv, rt) < 0) goto end; @@ -1643,11 +1665,12 @@ int main(int argc, char *argv[]) r = -1; if (wait_sync) { - if (init_pmc(cfg, &priv)) + if (init_pmc_node(cfg, &priv.node, uds_local, + phc2sys_recv_subscribed)) goto end; while (is_running()) { - r = run_pmc_wait_sync(&priv, 1000); + r = run_pmc_wait_sync(&priv.node, 1000); if (r < 0) goto end; if (r > 0) @@ -1657,7 +1680,7 @@ int main(int argc, char *argv[]) } if (!priv.forced_sync_offset) { - r = run_pmc_get_utc_offset(&priv, 1000); + r = run_pmc_get_utc_offset(&priv.node, 1000); if (r <= 0) { pr_err("failed to get UTC offset"); goto end; @@ -1667,7 +1690,7 @@ int main(int argc, char *argv[]) if (priv.forced_sync_offset || (src->clkid != CLOCK_REALTIME && dst->clkid != CLOCK_REALTIME) || src->clkid == CLOCK_INVALID) - close_pmc(&priv); + close_pmc_node(&priv.node); } if (pps_fd >= 0) { @@ -1680,8 +1703,7 @@ int main(int argc, char *argv[]) } end: - if (priv.pmc) - close_pmc(&priv); + close_pmc_node(&priv.node); clock_cleanup(&priv); port_cleanup(&priv); config_destroy(cfg); -- 2.25.1