From ba9c2ea0e1f7a96093bca1147d4745a7d0ce17b6 Mon Sep 17 00:00:00 2001 From: Andre Mauricio Zelak Date: Mon, 12 Jun 2023 14:34:19 -0300 Subject: [PATCH 13/50] phc2sys: break out pmc code into pmc_common.c The code through which phc2sys sends various PTP management messages to ptp4l via pmc can be reused. This patch is a trivial movement of that code to a separate translation module, outside of phc2sys. This makes it available to other programs that want to subscribe to port state change events too, such as ts2phc. Signed-off-by: Vladimir Oltean Reviewed-by: Jacob Keller [commit abc75482332752b630b023178ccdf636f5fe7de7 upstream] Signed-off-by: Andre Mauricio Zelak --- phc2sys.c | 354 --------------------------------------------------- pmc_common.c | 337 ++++++++++++++++++++++++++++++++++++++++++++++++ pmc_common.h | 35 +++++ 3 files changed, 372 insertions(+), 354 deletions(-) diff --git a/phc2sys.c b/phc2sys.c index d5b8e71..9184db6 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -63,12 +63,6 @@ #define NS_PER_SEC 1000000000LL #define PHC_PPS_OFFSET_LIMIT 10000000 -#define PMC_UPDATE_INTERVAL (60 * NS_PER_SEC) -#define PMC_SUBSCRIBE_DURATION 180 /* 3 minutes */ -/* Note that PMC_SUBSCRIBE_DURATION has to be longer than - * PMC_UPDATE_INTERVAL otherwise subscription will time out before it is - * renewed. - */ struct clock { LIST_ENTRY(clock) list; @@ -100,24 +94,6 @@ 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; @@ -137,17 +113,11 @@ struct phc2sys_private { static struct config *phc2sys_config; -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); -int run_pmc_get_utc_offset(struct pmc_node *node, int timeout); -void run_pmc_events(struct pmc_node *node); static int normalize_state(int state); -int run_pmc_port_properties(struct pmc_node *node, int timeout, - unsigned int port, int *state, - int *tstamping, char *iface); static struct servo *servo_add(struct phc2sys_private *priv, struct clock *clock) @@ -811,52 +781,6 @@ static int do_loop(struct phc2sys_private *priv, int subscriptions) return 0; } -static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg) -{ - if (!node->clock_identity_set) - return 1; - return cid_eq(&node->clock_identity, - &msg->header.sourcePortIdentity.clockIdentity); -} - -static int is_msg_mgt(struct ptp_message *msg) -{ - struct TLV *tlv; - - if (msg_type(msg) != MANAGEMENT) - return 0; - if (management_action(msg) != RESPONSE) - return 0; - if (msg_tlv_count(msg) != 1) - return 0; - tlv = (struct TLV *) msg->management.suffix; - if (tlv->type == TLV_MANAGEMENT) - return 1; - if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS) - return -1; - return 0; -} - -int get_mgt_id(struct ptp_message *msg) -{ - struct management_tlv *mgt = (struct management_tlv *) msg->management.suffix; - return mgt->id; -} - -void *get_mgt_data(struct ptp_message *msg) -{ - struct management_tlv *mgt = (struct management_tlv *) msg->management.suffix; - return mgt->data; -} - -static int get_mgt_err_id(struct ptp_message *msg) -{ - struct management_error_status *mgt; - - mgt = (struct management_error_status *)msg->management.suffix; - return mgt->id; -} - static int normalize_state(int state) { if (state != PS_MASTER && state != PS_SLAVE && @@ -927,260 +851,6 @@ static int phc2sys_recv_subscribed(struct pmc_node *node, return 0; } -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(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen)); -} - -int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds, - pmc_node_recv_subscribed_t *recv_subscribed) -{ - node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0, - config_get_int(cfg, NULL, "domainNumber"), - config_get_int(cfg, NULL, "transportSpecific") << 4, 1); - if (!node->pmc) { - pr_err("failed to create pmc"); - return -1; - } - node->recv_subscribed = recv_subscribed; - - return 0; -} - -/* Return values: - * 1: success - * 0: timeout - * -1: error reported by the other side - * -2: local error, fatal - */ -static int run_pmc(struct pmc_node *node, int timeout, int ds_id, - struct ptp_message **msg) -{ -#define N_FD 1 - struct pollfd pollfd[N_FD]; - int cnt, res; - - while (1) { - pollfd[0].fd = pmc_get_transport_fd(node->pmc); - pollfd[0].events = POLLIN|POLLPRI; - if (!node->pmc_ds_requested && ds_id >= 0) - pollfd[0].events |= POLLOUT; - - cnt = poll(pollfd, N_FD, timeout); - if (cnt < 0) { - pr_err("poll failed"); - return -2; - } - if (!cnt) { - /* Request the data set again in the next run. */ - node->pmc_ds_requested = 0; - return 0; - } - - /* Send a new request if there are no pending messages. */ - if ((pollfd[0].revents & POLLOUT) && - !(pollfd[0].revents & (POLLIN|POLLPRI))) { - switch (ds_id) { - case TLV_SUBSCRIBE_EVENTS_NP: - send_subscription(node); - break; - default: - pmc_send_get_action(node->pmc, ds_id); - break; - } - node->pmc_ds_requested = 1; - } - - if (!(pollfd[0].revents & (POLLIN|POLLPRI))) - continue; - - *msg = pmc_recv(node->pmc); - - if (!*msg) - continue; - - if (!check_clock_identity(node, *msg)) { - msg_put(*msg); - *msg = NULL; - continue; - } - - res = is_msg_mgt(*msg); - if (res < 0 && get_mgt_err_id(*msg) == ds_id) { - node->pmc_ds_requested = 0; - return -1; - } - if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) || - get_mgt_id(*msg) != ds_id) { - msg_put(*msg); - *msg = NULL; - continue; - } - node->pmc_ds_requested = 0; - return 1; - } -} - -static int run_pmc_wait_sync(struct pmc_node *node, int timeout) -{ - struct ptp_message *msg; - Enumeration8 portState; - void *data; - int res; - - while (1) { - res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg); - if (res <= 0) - return res; - - data = get_mgt_data(msg); - portState = ((struct portDS *)data)->portState; - msg_put(msg); - - switch (portState) { - case PS_MASTER: - case PS_SLAVE: - return 1; - } - /* try to get more data sets (for other ports) */ - node->pmc_ds_requested = 1; - } -} - -int run_pmc_get_utc_offset(struct pmc_node *node, int timeout) -{ - struct ptp_message *msg; - int res; - struct timePropertiesDS *tds; - - 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) { - node->sync_offset = tds->currentUtcOffset; - if (tds->flags & LEAP_61) - node->leap = 1; - else if (tds->flags & LEAP_59) - node->leap = -1; - else - node->leap = 0; - node->utc_offset_traceable = tds->flags & UTC_OFF_VALID && - tds->flags & TIME_TRACEABLE; - } else { - node->sync_offset = 0; - node->leap = 0; - node->utc_offset_traceable = 0; - } - msg_put(msg); - return 1; -} - -int run_pmc_get_number_ports(struct pmc_node *node, int timeout) -{ - struct ptp_message *msg; - int res; - struct defaultDS *dds; - - res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); - if (res <= 0) - return res; - - dds = (struct defaultDS *)get_mgt_data(msg); - res = dds->numberPorts; - msg_put(msg); - return res; -} - -int run_pmc_subscribe(struct pmc_node *node, int timeout) -{ - struct ptp_message *msg; - int res; - - res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg); - if (res <= 0) - return res; - msg_put(msg); - return 1; -} - -void run_pmc_events(struct pmc_node *node) -{ - struct ptp_message *msg; - - run_pmc(node, 0, -1, &msg); -} - -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(node->pmc, port); - while (1) { - res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg); - if (res <= 0) - goto out; - - ppn = get_mgt_data(msg); - if (ppn->portIdentity.portNumber != port) { - msg_put(msg); - continue; - } - - *state = ppn->port_state; - *tstamping = ppn->timestamping; - len = ppn->interface.length; - if (len > IFNAMSIZ - 1) - len = IFNAMSIZ - 1; - memcpy(iface, ppn->interface.text, len); - iface[len] = '\0'; - - msg_put(msg); - res = 1; - break; - } -out: - pmc_target_all(node->pmc); - return res; -} - -static int run_pmc_clock_identity(struct pmc_node *node, int timeout) -{ - struct ptp_message *msg; - struct defaultDS *dds; - int res; - - res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); - if (res <= 0) - return res; - - dds = (struct defaultDS *)get_mgt_data(msg); - memcpy(&node->clock_identity, &dds->clockIdentity, - sizeof(struct ClockIdentity)); - node->clock_identity_set = 1; - msg_put(msg); - return 1; -} - -void close_pmc_node(struct pmc_node *node) -{ - if (!node->pmc) - return; - - pmc_destroy(node->pmc); - node->pmc = NULL; -} - static int auto_init_ports(struct phc2sys_private *priv, int add_rt) { struct port *port; @@ -1259,30 +929,6 @@ static int auto_init_ports(struct phc2sys_private *priv, int add_rt) return 0; } -/* Returns: -1 in case of error, 0 otherwise */ -int update_pmc_node(struct pmc_node *node, int subscribe) -{ - struct timespec tp; - uint64_t ts; - - if (clock_gettime(CLOCK_MONOTONIC, &tp)) { - pr_err("failed to read clock: %m"); - return -1; - } - ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec; - - if (node->pmc && - !(ts > node->pmc_last_update && - ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) { - if (subscribe) - run_pmc_subscribe(node, 0); - if (run_pmc_get_utc_offset(node, 0) > 0) - node->pmc_last_update = ts; - } - - return 0; -} - /* Returns: non-zero to skip clock update */ static int clock_handle_leap(struct phc2sys_private *priv, struct clock *clock, int64_t offset, uint64_t ts) diff --git a/pmc_common.c b/pmc_common.c index f07f6f6..c9cdf18 100644 --- a/pmc_common.c +++ b/pmc_common.c @@ -18,6 +18,8 @@ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. */ #include +#include +#include #include #include #include @@ -56,6 +58,13 @@ /* Includes one extra byte to make length even. */ #define EMPTY_PTP_TEXT 2 +#define PMC_UPDATE_INTERVAL (60 * NS_PER_SEC) +#define PMC_SUBSCRIBE_DURATION 180 /* 3 minutes */ +/* Note that PMC_SUBSCRIBE_DURATION has to be longer than + * PMC_UPDATE_INTERVAL otherwise subscription will time out before it is + * renewed. + */ + static void do_get_action(struct pmc *pmc, int action, int index, char *str); static void do_set_action(struct pmc *pmc, int action, int index, char *str); static void not_supported(struct pmc *pmc, int action, int index, char *str); @@ -711,3 +720,331 @@ int pmc_do_command(struct pmc *pmc, char *str) return 0; } + +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(node->pmc, TLV_SUBSCRIBE_EVENTS_NP, &sen, sizeof(sen)); +} + +static int check_clock_identity(struct pmc_node *node, struct ptp_message *msg) +{ + if (!node->clock_identity_set) + return 1; + return cid_eq(&node->clock_identity, + &msg->header.sourcePortIdentity.clockIdentity); +} + +static int is_msg_mgt(struct ptp_message *msg) +{ + struct TLV *tlv; + + if (msg_type(msg) != MANAGEMENT) + return 0; + if (management_action(msg) != RESPONSE) + return 0; + if (msg_tlv_count(msg) != 1) + return 0; + tlv = (struct TLV *) msg->management.suffix; + if (tlv->type == TLV_MANAGEMENT) + return 1; + if (tlv->type == TLV_MANAGEMENT_ERROR_STATUS) + return -1; + return 0; +} + +int get_mgt_id(struct ptp_message *msg) +{ + struct management_tlv *mgt; + + mgt = (struct management_tlv *) msg->management.suffix; + return mgt->id; +} + +void *get_mgt_data(struct ptp_message *msg) +{ + struct management_tlv *mgt; + + mgt = (struct management_tlv *) msg->management.suffix; + return mgt->data; +} + +static int get_mgt_err_id(struct ptp_message *msg) +{ + struct management_error_status *mgt; + + mgt = (struct management_error_status *)msg->management.suffix; + return mgt->id; +} + +/* Return values: + * 1: success + * 0: timeout + * -1: error reported by the other side + * -2: local error, fatal + */ +static int run_pmc(struct pmc_node *node, int timeout, int ds_id, + struct ptp_message **msg) +{ +#define N_FD 1 + struct pollfd pollfd[N_FD]; + int cnt, res; + + while (1) { + pollfd[0].fd = pmc_get_transport_fd(node->pmc); + pollfd[0].events = POLLIN|POLLPRI; + if (!node->pmc_ds_requested && ds_id >= 0) + pollfd[0].events |= POLLOUT; + + cnt = poll(pollfd, N_FD, timeout); + if (cnt < 0) { + pr_err("poll failed"); + return -2; + } + if (!cnt) { + /* Request the data set again in the next run. */ + node->pmc_ds_requested = 0; + return 0; + } + + /* Send a new request if there are no pending messages. */ + if ((pollfd[0].revents & POLLOUT) && + !(pollfd[0].revents & (POLLIN|POLLPRI))) { + switch (ds_id) { + case TLV_SUBSCRIBE_EVENTS_NP: + send_subscription(node); + break; + default: + pmc_send_get_action(node->pmc, ds_id); + break; + } + node->pmc_ds_requested = 1; + } + + if (!(pollfd[0].revents & (POLLIN|POLLPRI))) + continue; + + *msg = pmc_recv(node->pmc); + + if (!*msg) + continue; + + if (!check_clock_identity(node, *msg)) { + msg_put(*msg); + *msg = NULL; + continue; + } + + res = is_msg_mgt(*msg); + if (res < 0 && get_mgt_err_id(*msg) == ds_id) { + node->pmc_ds_requested = 0; + return -1; + } + if (res <= 0 || node->recv_subscribed(node, *msg, ds_id) || + get_mgt_id(*msg) != ds_id) { + msg_put(*msg); + *msg = NULL; + continue; + } + node->pmc_ds_requested = 0; + return 1; + } +} + +int run_pmc_wait_sync(struct pmc_node *node, int timeout) +{ + struct ptp_message *msg; + Enumeration8 portState; + void *data; + int res; + + while (1) { + res = run_pmc(node, timeout, TLV_PORT_DATA_SET, &msg); + if (res <= 0) + return res; + + data = get_mgt_data(msg); + portState = ((struct portDS *)data)->portState; + msg_put(msg); + + switch (portState) { + case PS_MASTER: + case PS_SLAVE: + return 1; + } + /* try to get more data sets (for other ports) */ + node->pmc_ds_requested = 1; + } +} + +int run_pmc_get_utc_offset(struct pmc_node *node, int timeout) +{ + struct ptp_message *msg; + int res; + struct timePropertiesDS *tds; + + 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) { + node->sync_offset = tds->currentUtcOffset; + if (tds->flags & LEAP_61) + node->leap = 1; + else if (tds->flags & LEAP_59) + node->leap = -1; + else + node->leap = 0; + node->utc_offset_traceable = tds->flags & UTC_OFF_VALID && + tds->flags & TIME_TRACEABLE; + } else { + node->sync_offset = 0; + node->leap = 0; + node->utc_offset_traceable = 0; + } + msg_put(msg); + return 1; +} + +int run_pmc_get_number_ports(struct pmc_node *node, int timeout) +{ + struct ptp_message *msg; + int res; + struct defaultDS *dds; + + res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); + if (res <= 0) + return res; + + dds = (struct defaultDS *)get_mgt_data(msg); + res = dds->numberPorts; + msg_put(msg); + return res; +} + +int run_pmc_subscribe(struct pmc_node *node, int timeout) +{ + struct ptp_message *msg; + int res; + + res = run_pmc(node, timeout, TLV_SUBSCRIBE_EVENTS_NP, &msg); + if (res <= 0) + return res; + msg_put(msg); + return 1; +} + +void run_pmc_events(struct pmc_node *node) +{ + struct ptp_message *msg; + + run_pmc(node, 0, -1, &msg); +} + +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(node->pmc, port); + while (1) { + res = run_pmc(node, timeout, TLV_PORT_PROPERTIES_NP, &msg); + if (res <= 0) + goto out; + + ppn = get_mgt_data(msg); + if (ppn->portIdentity.portNumber != port) { + msg_put(msg); + continue; + } + + *state = ppn->port_state; + *tstamping = ppn->timestamping; + len = ppn->interface.length; + if (len > IFNAMSIZ - 1) + len = IFNAMSIZ - 1; + memcpy(iface, ppn->interface.text, len); + iface[len] = '\0'; + + msg_put(msg); + res = 1; + break; + } +out: + pmc_target_all(node->pmc); + return res; +} + +int run_pmc_clock_identity(struct pmc_node *node, int timeout) +{ + struct ptp_message *msg; + struct defaultDS *dds; + int res; + + res = run_pmc(node, timeout, TLV_DEFAULT_DATA_SET, &msg); + if (res <= 0) + return res; + + dds = (struct defaultDS *)get_mgt_data(msg); + memcpy(&node->clock_identity, &dds->clockIdentity, + sizeof(struct ClockIdentity)); + node->clock_identity_set = 1; + msg_put(msg); + return 1; +} + +/* Returns: -1 in case of error, 0 otherwise */ +int update_pmc_node(struct pmc_node *node, int subscribe) +{ + struct timespec tp; + uint64_t ts; + + if (clock_gettime(CLOCK_MONOTONIC, &tp)) { + pr_err("failed to read clock: %m"); + return -1; + } + ts = tp.tv_sec * NS_PER_SEC + tp.tv_nsec; + + if (node->pmc && + !(ts > node->pmc_last_update && + ts - node->pmc_last_update < PMC_UPDATE_INTERVAL)) { + if (subscribe) + run_pmc_subscribe(node, 0); + if (run_pmc_get_utc_offset(node, 0) > 0) + node->pmc_last_update = ts; + } + + return 0; +} + +int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds, + pmc_node_recv_subscribed_t *recv_subscribed) +{ + node->pmc = pmc_create(cfg, TRANS_UDS, uds, 0, + config_get_int(cfg, NULL, "domainNumber"), + config_get_int(cfg, NULL, "transportSpecific") << 4, 1); + if (!node->pmc) { + pr_err("failed to create pmc"); + return -1; + } + node->recv_subscribed = recv_subscribed; + + return 0; +} + +void close_pmc_node(struct pmc_node *node) +{ + if (!node->pmc) + return; + + pmc_destroy(node->pmc); + node->pmc = NULL; +} diff --git a/pmc_common.h b/pmc_common.h index 9fa72de..476ccea 100644 --- a/pmc_common.h +++ b/pmc_common.h @@ -22,6 +22,7 @@ #define HAVE_PMC_COMMON_H #include "config.h" +#include "fsm.h" #include "msg.h" #include "transport.h" @@ -49,4 +50,38 @@ void pmc_target_all(struct pmc *pmc); const char *pmc_action_string(int action); int pmc_do_command(struct pmc *pmc, char *str); +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; +}; + +int init_pmc_node(struct config *cfg, struct pmc_node *node, const char *uds, + pmc_node_recv_subscribed_t *recv_subscribed); +void close_pmc_node(struct pmc_node *node); +int update_pmc_node(struct pmc_node *node, int subscribe); +int run_pmc_subscribe(struct pmc_node *node, int timeout); +int run_pmc_clock_identity(struct pmc_node *node, int timeout); +int run_pmc_wait_sync(struct pmc_node *node, int timeout); +int run_pmc_get_number_ports(struct pmc_node *node, int timeout); +void run_pmc_events(struct pmc_node *node); +int run_pmc_port_properties(struct pmc_node *node, int timeout, + unsigned int port, int *state, + int *tstamping, char *iface); +int run_pmc_get_utc_offset(struct pmc_node *node, int timeout); +int get_mgt_id(struct ptp_message *msg); +void *get_mgt_data(struct ptp_message *msg); + #endif -- 2.25.1