Re: [Linuxptp-devel] [RFC PATCH 03/15] phc2sys: break out pmc code into pmc_common.c

[Jacob K. <jac...@in...>]

On 8/1/2020 10:46 AM, Vladimir Oltean wrote:
> 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.

Ah, perfect: pmc_node is not too generic, so this is great.

I looked this over using git diff with the moved lines coloring options,
and the only places where the new code differs is in name change from
priv to pmc_node.

Makes sense.

Reviewed-by: Jacob Keller <jac...@in...>

> 
> Signed-off-by: Vladimir Oltean <ol...@gm...>
> ---
>  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, &timestamping,
>  						      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,
>  					      &timestamping, 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
>