Move the code for sending various messages to ptp4l via pmc to a common
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.
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.
Signed-off-by: Vladimir Oltean <olte...@gmail.com>
Reviewed-by: Jacob Keller <jacob.e.kel...@intel.com>
---
Changes in RFC v2:
- Added Jacob's review tag.
phc2sys.c | 404 +++++----------------------------------------------
pmc_common.c | 337 ++++++++++++++++++++++++++++++++++++++++++
pmc_common.h | 35 +++++
3 files changed, 407 insertions(+), 369 deletions(-)
diff --git a/phc2sys.c b/phc2sys.c
index a36cbe071d7d..c4d72bd7d17a 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -56,18 +56,13 @@
#include "uds.h"
#include "util.h"
#include "version.h"
+#include "contain.h"
#define KP 0.7
#define KI 0.3
#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;
@@ -105,17 +100,10 @@ struct phc2sys_private {
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;
@@ -124,18 +112,11 @@ struct phc2sys_private {
static struct config *phc2sys_config;
-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);
-static int run_pmc_get_utc_offset(struct phc2sys_private *priv,
- int timeout);
-static void run_pmc_events(struct phc2sys_private *priv);
static int normalize_state(int state);
-static int run_pmc_port_properties(struct phc2sys_private *priv,
- int timeout, unsigned int port,
- int *state, int *tstamping, char *iface);
static struct servo *servo_add(struct phc2sys_private *priv,
struct clock *clock)
@@ -324,7 +305,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)
@@ -659,7 +640,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);
}
@@ -690,7 +671,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);
}
@@ -727,15 +708,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;
}
@@ -792,53 +773,6 @@ 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)
-{
- if (!priv->clock_identity_set)
- return 1;
- return cid_eq(&priv->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;
-}
-
-static int get_mgt_id(struct ptp_message *msg)
-{
- struct management_tlv *mgt = (struct management_tlv *)
msg->management.suffix;
- return mgt->id;
-}
-
-static 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 &&
@@ -868,9 +802,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;
@@ -905,259 +843,6 @@ static int recv_subscribed(struct phc2sys_private *priv,
return 0;
}
-static void send_subscription(struct phc2sys_private *priv)
-{
- 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));
-}
-
-static int init_pmc(struct config *cfg, struct phc2sys_private *priv)
-{
- 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,
- config_get_int(cfg, NULL, "domainNumber"),
- config_get_int(cfg, NULL, "transportSpecific")
<< 4, 1);
- if (!priv->pmc) {
- pr_err("failed to create pmc");
- return -1;
- }
-
- return 0;
-}
-
-/* Return values:
- * 1: success
- * 0: timeout
- * -1: error reported by the other side
- * -2: local error, fatal
- */
-static int run_pmc(struct phc2sys_private *priv, 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(priv->pmc);
- pollfd[0].events = POLLIN|POLLPRI;
- if (!priv->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. */
- priv->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(priv);
- break;
- default:
- pmc_send_get_action(priv->pmc, ds_id);
- break;
- }
- priv->pmc_ds_requested = 1;
- }
-
- if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
- continue;
-
- *msg = pmc_recv(priv->pmc);
-
- if (!*msg)
- continue;
-
- if (!check_clock_identity(priv, *msg)) {
- msg_put(*msg);
- *msg = NULL;
- continue;
- }
-
- res = is_msg_mgt(*msg);
- if (res < 0 && get_mgt_err_id(*msg) == ds_id) {
- priv->pmc_ds_requested = 0;
- return -1;
- }
- if (res <= 0 || recv_subscribed(priv, *msg, ds_id) ||
- get_mgt_id(*msg) != ds_id) {
- msg_put(*msg);
- *msg = NULL;
- continue;
- }
- priv->pmc_ds_requested = 0;
- return 1;
- }
-}
-
-static int run_pmc_wait_sync(struct phc2sys_private *priv, int timeout)
-{
- struct ptp_message *msg;
- int res;
- void *data;
- Enumeration8 portState;
-
- while (1) {
- res = run_pmc(priv, 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) */
- priv->pmc_ds_requested = 1;
- }
-}
-
-static int run_pmc_get_utc_offset(struct phc2sys_private *priv, int timeout)
-{
- struct ptp_message *msg;
- int res;
- struct timePropertiesDS *tds;
-
- res = run_pmc(priv, 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;
- if (tds->flags & LEAP_61)
- priv->leap = 1;
- else if (tds->flags & LEAP_59)
- priv->leap = -1;
- else
- priv->leap = 0;
- priv->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;
- }
- msg_put(msg);
- return 1;
-}
-
-static int run_pmc_get_number_ports(struct phc2sys_private *priv, int timeout)
-{
- struct ptp_message *msg;
- int res;
- struct defaultDS *dds;
-
- res = run_pmc(priv, 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;
-}
-
-static int run_pmc_subscribe(struct phc2sys_private *priv, int timeout)
-{
- struct ptp_message *msg;
- int res;
-
- res = run_pmc(priv, 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)
-{
- struct ptp_message *msg;
-
- run_pmc(priv, 0, -1, &msg);
-}
-
-static int run_pmc_port_properties(struct phc2sys_private *priv, 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);
- while (1) {
- res = run_pmc(priv, 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(priv->pmc);
- return res;
-}
-
-static int run_pmc_clock_identity(struct phc2sys_private *priv, int timeout)
-{
- struct ptp_message *msg;
- struct defaultDS *dds;
- int res;
-
- res = run_pmc(priv, timeout, TLV_DEFAULT_DATA_SET, &msg);
- if (res <= 0)
- return res;
-
- dds = (struct defaultDS *)get_mgt_data(msg);
- memcpy(&priv->clock_identity, &dds->clockIdentity,
- sizeof(struct ClockIdentity));
- priv->clock_identity_set = 1;
- msg_put(msg);
- return 1;
-}
-
-static void close_pmc(struct phc2sys_private *priv)
-{
- pmc_destroy(priv->pmc);
- priv->pmc = NULL;
-}
-
static int auto_init_ports(struct phc2sys_private *priv, int add_rt)
{
struct port *port;
@@ -1170,7 +855,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)
@@ -1179,20 +864,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 */
@@ -1229,44 +914,20 @@ 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;
}
return 0;
}
-/* Returns: -1 in case of error, 0 otherwise */
-static int update_pmc(struct phc2sys_private *priv, 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 (priv->pmc &&
- !(ts > priv->pmc_last_update &&
- ts - priv->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;
- }
-
- 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)
{
- 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) {
@@ -1307,7 +968,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);
@@ -1361,6 +1022,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;
@@ -1469,7 +1131,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;
@@ -1589,8 +1251,12 @@ int main(int argc, char *argv[])
priv.kernel_leap = config_get_int(cfg, NULL, "kernel_leap");
priv.sanity_freq_limit = config_get_int(cfg, NULL, "sanity_freq_limit");
+ 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;
@@ -1627,11 +1293,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)
@@ -1641,7 +1308,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;
@@ -1651,7 +1318,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) {
@@ -1664,8 +1331,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);
diff --git a/pmc_common.c b/pmc_common.c
index f07f6f65568f..89d7f40b84fe 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -22,6 +22,8 @@
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
+#include <net/if.h>
+#include <poll.h>
#include "notification.h"
#include "print.h"
@@ -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 9fa72deb4c87..a28bab767e9c 100644
--- a/pmc_common.h
+++ b/pmc_common.h
@@ -24,6 +24,7 @@
#include "config.h"
#include "msg.h"
#include "transport.h"
+#include "fsm.h"
struct pmc;
@@ -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
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
