ts2phc_phc_master: make use of new kernel API for perout waveform

This API was introduced for 2 reasons:

1. Some hardware can emit PPS signals but not starting from arbitrary
   absolute times, but rather phase-aligned to the beginning of a
   second. We _could_ patch ts2phc to always specify a start time of
   0.000000000 to PTP_PEROUT_REQUEST, but in practice, we would never
   know whether that would actually work with all in-tree PHC drivers.
   So there was a need for a new flag that only specifies the phase of
   the periodic signal, and not the absolute start time.

2. Some hardware can, rather unfortunately, not distinguish between a
   rising and a falling extts edge. And, since whatever rises also has
   to fall before rising again, the strategy in ts2phc is to set a
   'large' pulse width (half the period) and ignore the extts event
   corresponding to the mid-way between one second and another. This is
   all fine, but currently, ts2phc.pulsewidth is a read-only property in
   the config file. The kernel is not instructed in any way to use this
   value, it is simply that must be configured based on prior knowledge
   of the PHC's implementation. This API changes that.

The introduction of a phase adjustment for the master PHC means we have
to adjust our approximation of the precise perout timestamp. We put that
code into a common function and convert all call sites to call that. We
also need to do the same thing for the edge ignoring logic.

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>

config.c

@@ -314,6 +314,7 @@ struct config_item config_tab[] = {
 	GLOB_ITEM_STR("ts2phc.nmea_remote_host", ""),
 	GLOB_ITEM_STR("ts2phc.nmea_remote_port", ""),
 	GLOB_ITEM_STR("ts2phc.nmea_serialport", "/dev/ttyS0"),
+	PORT_ITEM_INT("ts2phc.perout_phase", -1, 0, 999999999),
 	PORT_ITEM_INT("ts2phc.pin_index", 0, 0, INT_MAX),
 	GLOB_ITEM_INT("ts2phc.pulsewidth", 500000000, 1000000, 999000000),
 	PORT_ITEM_ENU("tsproc_mode", TSPROC_FILTER, tsproc_enu),

makefile

@@ -27,7 +27,8 @@ FILTERS	= filter.o mave.o mmedian.o
 SERVOS	= linreg.o ntpshm.o nullf.o pi.o servo.o
 TRANSP	= raw.o transport.o udp.o udp6.o uds.o
 TS2PHC	= ts2phc.o lstab.o nmea.o serial.o sock.o ts2phc_generic_master.o \
- ts2phc_master.o ts2phc_phc_master.o ts2phc_nmea_master.o ts2phc_slave.o
+ ts2phc_master.o ts2phc_phc_master.o ts2phc_nmea_master.o ts2phc_slave.o \
+ pmc_common.o transport.o msg.o tlv.o uds.o udp.o udp6.o raw.o
 OBJ	= bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
  e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o monitor.o msg.o phc.o \
  port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) \

missing.h

@@ -97,6 +97,59 @@ struct compat_ptp_clock_caps {
 
 #endif /*LINUX_VERSION_CODE < 5.8*/
 
+/*
+ * Bits of the ptp_perout_request.flags field:
+ */
+
+#ifndef PTP_PEROUT_ONE_SHOT
+#define PTP_PEROUT_ONE_SHOT		(1<<0)
+#endif
+
+#ifndef PTP_PEROUT_DUTY_CYCLE
+#define PTP_PEROUT_DUTY_CYCLE		(1<<1)
+#endif
+
+#ifndef PTP_PEROUT_PHASE
+#define PTP_PEROUT_PHASE		(1<<2)
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(5,9,0)
+
+/* from upcoming Linux kernel version 5.9 */
+struct compat_ptp_perout_request {
+	union {
+		/*
+		 * Absolute start time.
+		 * Valid only if (flags & PTP_PEROUT_PHASE) is unset.
+		 */
+		struct ptp_clock_time start;
+		/*
+		 * Phase offset. The signal should start toggling at an
+		 * unspecified integer multiple of the period, plus this value.
+		 * The start time should be "as soon as possible".
+		 * Valid only if (flags & PTP_PEROUT_PHASE) is set.
+		 */
+		struct ptp_clock_time phase;
+	};
+	struct ptp_clock_time period; /* Desired period, zero means disable. */
+	unsigned int index;           /* Which channel to configure. */
+	unsigned int flags;
+	union {
+		/*
+		 * The "on" time of the signal.
+		 * Must be lower than the period.
+		 * Valid only if (flags & PTP_PEROUT_DUTY_CYCLE) is set.
+		 */
+		struct ptp_clock_time on;
+		/* Reserved for future use. */
+		unsigned int rsv[4];
+	};
+};
+
+#define ptp_perout_request compat_ptp_perout_request
+
+#endif /*LINUX_VERSION_CODE < 5.8*/
+
 #ifndef PTP_MAX_SAMPLES
 #define PTP_MAX_SAMPLES 25 /* Maximum allowed offset measurement samples. */
 #endif /* PTP_MAX_SAMPLES */

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;
@@ -136,17 +112,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)
@@ -803,56 +773,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;
-
-	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;
-}
-
 static int normalize_state(int state)
 {
 	if (state != PS_MASTER && state != PS_SLAVE &&
@@ -923,260 +843,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;
@@ -1255,30 +921,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)

pmc_common.c

@@ -18,6 +18,8 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 #include <errno.h>
+#include <net/if.h>
+#include <poll.h>
 #include <stdlib.h>
 #include <string.h>
 #include <sys/types.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;
+}

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

ts2phc.8

@@ -26,6 +26,21 @@ A single source may be used to distribute time to one or more PHC devices.
 
 .SH OPTIONS
 .TP
+.BI \-a
+Adjust the direction of synchronization automatically. The program determines
+which PHC should be a source of time and which should be a sink by querying the
+port states from the running instance of
+.B ptp4l.
+Note that using this option, the PPS signal distribution hierarchy still
+remains fixed as per the configuration file. This implies that using this
+option, a PHC PPS master may become a time sink, and a PPS slave may become a
+time source. Other, non-PHC types of PPS masters (generic, NMEA) cannot become
+time sinks. Clocks which are not part of
+.B ptp4l's
+list of ports are not synchronized. This option is useful when the
+.B boundary_clock_jbod
+option of ptp4l is also enabled.
+.TP
 .BI \-c " device|name"
 Specifies a PHC slave clock to be synchronized.
 The clock may be identified by its character device (like /dev/ptp0)
@@ -152,6 +167,11 @@ specified, then the given remote connection will be used in preference
 to the configured serial port.
 The default is "/dev/ttyS0".
 .TP
+.B ts2phc.perout_phase
+Configures the offset between the beginning of the second and the PPS
+master's rising edge. Available only for a PHC master. The supported
+range is 0 to 999999999 nanoseconds. The default is 0 nanoseconds.
+.TP
 .B ts2phc.pulsewidth
 The expected pulse width of the external PPS signal in nanoseconds.
 When 'ts2phc.extts_polarity' is "both", the given pulse width is used

ts2phc.c

@@ -7,27 +7,524 @@
  * @note SPDX-License-Identifier: GPL-2.0+
  */
 #include <stdlib.h>
+#include <net/if.h>
+#include <sys/types.h>
+#include <unistd.h>
 
+#include "clockadj.h"
 #include "config.h"
+#include "contain.h"
 #include "interface.h"
+#include "phc.h"
 #include "print.h"
-#include "ts2phc_master.h"
+#include "ts2phc.h"
-#include "ts2phc_slave.h"
 #include "version.h"
 
+#define NS_PER_SEC		1000000000LL
+#define SAMPLE_WEIGHT		1.0
+
 struct interface {
 	STAILQ_ENTRY(interface) list;
 };
 
-static void ts2phc_cleanup(struct config *cfg, struct ts2phc_master *master)
+static void ts2phc_cleanup(struct ts2phc_private *priv)
 {
-	ts2phc_slave_cleanup();
+	struct port *p, *tmp;
-	if (master) {
+
-		ts2phc_master_destroy(master);
+	ts2phc_slave_cleanup(priv);
+	if (priv->master)
+		ts2phc_master_destroy(priv->master);
+	if (priv->cfg)
+		config_destroy(priv->cfg);
+
+	close_pmc_node(&priv->node);
+
+	/*
+	 * Clocks are destroyed by the cleanup methods of the individual
+	 * master and slave PHC modules.
+	 */
+	LIST_FOREACH_SAFE(p, &priv->ports, list, tmp)
+		free(p);
+
+	msg_cleanup();
+}
+
+/* FIXME: Copied from phc2sys */
+static int normalize_state(int state)
+{
+	if (state != PS_MASTER && state != PS_SLAVE &&
+	    state != PS_PRE_MASTER && state != PS_UNCALIBRATED) {
+		/* treat any other state as "not a master nor a slave" */
+		state = PS_DISABLED;
 	}
-	if (cfg) {
+	return state;
-		config_destroy(cfg);
+}
+
+/* FIXME: Copied from phc2sys */
+static struct port *port_get(struct ts2phc_private *priv, unsigned int number)
+{
+	struct port *p;
+
+	LIST_FOREACH(p, &priv->ports, list) {
+		if (p->number == number)
+			return p;
 	}
+	return NULL;
+}
+
+/* FIXME: Copied from phc2sys */
+static int clock_compute_state(struct ts2phc_private *priv,
+			       struct clock *clock)
+{
+	int state = PS_DISABLED;
+	struct port *p;
+
+	LIST_FOREACH(p, &priv->ports, list) {
+		if (p->clock != clock)
+			continue;
+		/* PS_SLAVE takes the highest precedence, PS_UNCALIBRATED
+		 * after that, PS_MASTER is third, PS_PRE_MASTER fourth and
+		 * all of that overrides PS_DISABLED, which corresponds
+		 * nicely with the numerical values */
+		if (p->state > state)
+			state = p->state;
+	}
+	return state;
+}
+
+#define node_to_ts2phc(node) \
+	container_of(node, struct ts2phc_private, node)
+
+static int ts2phc_recv_subscribed(struct pmc_node *node,
+				  struct ptp_message *msg, int excluded)
+{
+	struct ts2phc_private *priv = node_to_ts2phc(node);
+	int mgt_id, state;
+	struct portDS *pds;
+	struct port *port;
+	struct clock *clock;
+
+	mgt_id = get_mgt_id(msg);
+	if (mgt_id == excluded)
+		return 0;
+	switch (mgt_id) {
+	case TLV_PORT_DATA_SET:
+		pds = get_mgt_data(msg);
+		port = port_get(priv, pds->portIdentity.portNumber);
+		if (!port) {
+			pr_info("received data for unknown port %s",
+				pid2str(&pds->portIdentity));
+			return 1;
+		}
+		state = normalize_state(pds->portState);
+		if (port->state != state) {
+			pr_info("port %s changed state",
+				pid2str(&pds->portIdentity));
+			port->state = state;
+			clock = port->clock;
+			state = clock_compute_state(priv, clock);
+			if (clock->state != state || clock->new_state) {
+				clock->new_state = state;
+				priv->state_changed = 1;
+			}
+		}
+		return 1;
+	}
+	return 0;
+}
+
+struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock)
+{
+	enum servo_type type = config_get_int(priv->cfg, NULL, "clock_servo");
+	struct servo *servo;
+	int fadj, max_adj;
+
+	fadj = (int) clockadj_get_freq(clock->clkid);
+	/* Due to a bug in older kernels, the reading may silently fail
+	   and return 0. Set the frequency back to make sure fadj is
+	   the actual frequency of the clock. */
+	clockadj_set_freq(clock->clkid, fadj);
+
+	max_adj = phc_max_adj(clock->clkid);
+
+	servo = servo_create(priv->cfg, type, -fadj, max_adj, 0);
+	if (!servo)
+		return NULL;
+
+	servo_sync_interval(servo, SERVO_SYNC_INTERVAL);
+
+	return servo;
+}
+
+void clock_add_tstamp(struct clock *clock, tmv_t t)
+{
+	struct timespec ts = tmv_to_timespec(t);
+
+	pr_debug("adding tstamp %ld.%09ld to clock %s",
+		 ts.tv_sec, ts.tv_nsec, clock->name);
+	clock->last_ts = t;
+	clock->is_ts_available = 1;
+}
+
+static int clock_get_tstamp(struct clock *clock, tmv_t *ts)
+{
+	if (!clock->is_ts_available)
+		return 0;
+	clock->is_ts_available = 0;
+	*ts = clock->last_ts;
+	return 1;
+}
+
+static void clock_flush_tstamp(struct clock *clock)
+{
+	clock->is_ts_available = 0;
+}
+
+struct clock *clock_add(struct ts2phc_private *priv, const char *device)
+{
+	clockid_t clkid = CLOCK_INVALID;
+	int phc_index = -1;
+	struct clock *c;
+	int err;
+
+	clkid = posix_clock_open(device, &phc_index);
+	if (clkid == CLOCK_INVALID)
+		return NULL;
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		if (c->phc_index == phc_index) {
+			/* Already have the clock, don't add it again */
+			posix_clock_close(clkid);
+			return c;
+		}
+	}
+
+	c = calloc(1, sizeof(*c));
+	if (!c) {
+		pr_err("failed to allocate memory for a clock");
+		return NULL;
+	}
+	c->clkid = clkid;
+	c->phc_index = phc_index;
+	c->servo_state = SERVO_UNLOCKED;
+	c->servo = servo_add(priv, c);
+	c->no_adj = config_get_int(priv->cfg, NULL, "free_running");
+	err = asprintf(&c->name, "/dev/ptp%d", phc_index);
+	if (err < 0) {
+		free(c);
+		posix_clock_close(clkid);
+		return NULL;
+	}
+
+	LIST_INSERT_HEAD(&priv->clocks, c, list);
+	return c;
+}
+
+
+void clock_destroy(struct clock *c)
+{
+	servo_destroy(c->servo);
+	posix_clock_close(c->clkid);
+	free(c->name);
+	free(c);
+}
+
+/* FIXME: Copied from phc2sys */
+static struct port *port_add(struct ts2phc_private *priv, unsigned int number,
+			     char *device)
+{
+	struct clock *c = NULL;
+	struct port *p, *tmp;
+
+	p = port_get(priv, number);
+	if (p)
+		return p;
+	/* port is a new one, look whether we have the device already on
+	 * a different port */
+	LIST_FOREACH(tmp, &priv->ports, list) {
+		if (tmp->number == number) {
+			c = tmp->clock;
+			break;
+		}
+	}
+	if (!c) {
+		c = clock_add(priv, device);
+		if (!c)
+			return NULL;
+	}
+	p = malloc(sizeof(*p));
+	if (!p) {
+		pr_err("failed to allocate memory for a port");
+		clock_destroy(c);
+		return NULL;
+	}
+	p->number = number;
+	p->clock = c;
+	LIST_INSERT_HEAD(&priv->ports, p, list);
+	return p;
+}
+
+static int auto_init_ports(struct ts2phc_private *priv)
+{
+	int state, timestamping;
+	int number_ports, res;
+	char iface[IFNAMSIZ];
+	struct clock *clock;
+	struct port *port;
+	unsigned int i;
+
+	while (1) {
+		if (!is_running())
+			return -1;
+		res = run_pmc_clock_identity(&priv->node, 1000);
+		if (res < 0)
+			return -1;
+		if (res > 0)
+			break;
+		/* res == 0, timeout */
+		pr_notice("Waiting for ptp4l...");
+	}
+
+	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->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->node, 1000, i, &state,
+					      &timestamping, iface);
+		if (res == -1) {
+			/* port does not exist, ignore the port */
+			continue;
+		}
+		if (res <= 0) {
+			pr_err("failed to get port properties");
+			return -1;
+		}
+		if (timestamping == TS_SOFTWARE) {
+			/* ignore ports with software time stamping */
+			continue;
+		}
+		port = port_add(priv, i, iface);
+		if (!port)
+			return -1;
+		port->state = normalize_state(state);
+	}
+	if (LIST_EMPTY(&priv->clocks)) {
+		pr_err("no suitable ports available");
+		return -1;
+	}
+	LIST_FOREACH(clock, &priv->clocks, list) {
+		clock->new_state = clock_compute_state(priv, clock);
+	}
+	priv->state_changed = 1;
+
+	return 0;
+}
+
+static void ts2phc_reconfigure(struct ts2phc_private *priv)
+{
+	struct clock *c, *src = NULL, *last = NULL;
+	int src_cnt = 0, dst_cnt = 0;
+
+	pr_info("reconfiguring after port state change");
+	priv->state_changed = 0;
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		if (c->new_state) {
+			c->state = c->new_state;
+			c->new_state = 0;
+		}
+
+		switch (c->state) {
+		case PS_FAULTY:
+		case PS_DISABLED:
+		case PS_LISTENING:
+		case PS_PRE_MASTER:
+		case PS_MASTER:
+		case PS_PASSIVE:
+			if (!c->is_destination) {
+				pr_info("selecting %s for synchronization",
+					c->name);
+				c->is_destination = 1;
+			}
+			dst_cnt++;
+			break;
+		case PS_UNCALIBRATED:
+			src_cnt++;
+			break;
+		case PS_SLAVE:
+			src = c;
+			src_cnt++;
+			break;
+		}
+		last = c;
+	}
+	if (dst_cnt >= 1 && !src) {
+		priv->source = last;
+		priv->source->is_destination = 0;
+		/* Reset to original state in next reconfiguration. */
+		priv->source->new_state = priv->source->state;
+		priv->source->state = PS_SLAVE;
+		pr_info("no source, selecting %s as the default clock",
+			last->name);
+		return;
+	}
+	if (src_cnt > 1) {
+		pr_info("multiple source clocks available, postponing sync...");
+		priv->source = NULL;
+		return;
+	}
+	if (src_cnt > 0 && !src) {
+		pr_info("source clock not ready, waiting...");
+		priv->source = NULL;
+		return;
+	}
+	if (!src_cnt && !dst_cnt) {
+		pr_info("no PHC ready, waiting...");
+		priv->source = NULL;
+		return;
+	}
+	if (!src_cnt) {
+		pr_info("nothing to synchronize");
+		priv->source = NULL;
+		return;
+	}
+	src->is_destination = 0;
+	priv->source = src;
+	pr_info("selecting %s as the source clock", src->name);
+}
+
+static int ts2phc_approximate_master_tstamp(struct ts2phc_private *priv,
+					    tmv_t *master_tmv)
+{
+	struct timespec master_ts;
+	tmv_t tmv;
+	int err;
+
+	err = ts2phc_master_getppstime(priv->master, &master_ts);
+	if (err < 0) {
+		pr_err("master ts not valid");
+		return err;
+	}
+
+	tmv = timespec_to_tmv(master_ts);
+	tmv = tmv_sub(tmv, priv->perout_phase);
+	master_ts = tmv_to_timespec(tmv);
+
+	/*
+	 * As long as the kernel doesn't support a proper API for reporting
+	 * a precise perout timestamp, we'll have to use this crude
+	 * approximation.
+	 */
+	if (master_ts.tv_nsec > NS_PER_SEC / 2)
+		master_ts.tv_sec++;
+	master_ts.tv_nsec = 0;
+
+	tmv = timespec_to_tmv(master_ts);
+	tmv = tmv_add(tmv, priv->perout_phase);
+
+	*master_tmv = tmv;
+
+	return 0;
+}
+
+static void ts2phc_synchronize_clocks(struct ts2phc_private *priv, int autocfg)
+{
+	tmv_t source_tmv;
+	struct clock *c;
+	int valid, err;
+
+	if (autocfg) {
+		if (!priv->source) {
+			pr_debug("no source, skipping");
+			return;
+		}
+		valid = clock_get_tstamp(priv->source, &source_tmv);
+		if (!valid) {
+			pr_err("source clock (%s) timestamp not valid, skipping",
+				priv->source->name);
+			return;
+		}
+	} else {
+		err = ts2phc_approximate_master_tstamp(priv, &source_tmv);
+		if (err < 0)
+			return;
+	}
+
+	LIST_FOREACH(c, &priv->clocks, list) {
+		int64_t offset;
+		double adj;
+		tmv_t ts;
+
+		if (!c->is_destination)
+			continue;
+
+		valid = clock_get_tstamp(c, &ts);
+		if (!valid) {
+			pr_debug("%s timestamp not valid, skipping", c->name);
+			continue;
+		}
+
+		offset = tmv_to_nanoseconds(tmv_sub(ts, source_tmv));
+
+		if (c->no_adj) {
+			pr_info("%s offset %10" PRId64, c->name,
+				offset);
+			continue;
+		}
+
+		adj = servo_sample(c->servo, offset, tmv_to_nanoseconds(ts),
+				   SAMPLE_WEIGHT, &c->servo_state);
+
+		pr_info("%s offset %10" PRId64 " s%d freq %+7.0f",
+			c->name, offset, c->servo_state, adj);
+
+		switch (c->servo_state) {
+		case SERVO_UNLOCKED:
+			break;
+		case SERVO_JUMP:
+			clockadj_set_freq(c->clkid, -adj);
+			clockadj_step(c->clkid, -offset);
+			break;
+		case SERVO_LOCKED:
+		case SERVO_LOCKED_STABLE:
+			clockadj_set_freq(c->clkid, -adj);
+			break;
+		}
+	}
+}
+
+static int ts2phc_collect_master_tstamp(struct ts2phc_private *priv)
+{
+	struct clock *master_clock;
+	tmv_t master_tmv;
+	int err;
+
+	master_clock = ts2phc_master_get_clock(priv->master);
+	/*
+	 * Master isn't a PHC (it may be a generic or a GPS master),
+	 * don't error out, just don't do anything. If it doesn't have a PHC,
+	 * there is nothing to synchronize, which is the only point of
+	 * collecting its perout timestamp in the first place.
+	 */
+	if (!master_clock)
+		return 0;
+
+	err = ts2phc_approximate_master_tstamp(priv, &master_tmv);
+	if (err < 0)
+		return err;
+
+	clock_add_tstamp(master_clock, master_tmv);
+
+	return 0;
 }
 
 static void usage(char *progname)
@@ -35,6 +532,7 @@ static void usage(char *progname)
 	fprintf(stderr,
 		"\n"
 		"usage: %s [options]\n\n"
+		" -a             turn on autoconfiguration\n"
 		" -c [dev|name]  phc slave clock (like /dev/ptp0 or eth0)\n"
 		"                (may be specified multiple times)\n"
 		" -f [file]      read configuration from 'file'\n"
@@ -56,19 +554,21 @@ static void usage(char *progname)
 int main(int argc, char *argv[])
 {
 	int c, err = 0, have_slave = 0, index, print_level;
-	struct ts2phc_master *master = NULL;
+	char uds_local[MAX_IFNAME_SIZE + 1];
 	enum ts2phc_master_type pps_type;
+	struct ts2phc_private priv = {0};
 	char *config = NULL, *progname;
 	const char *pps_source = NULL;
 	struct config *cfg = NULL;
 	struct interface *iface;
 	struct option *opts;
+	int autocfg = 0;
 
 	handle_term_signals();
 
 	cfg = config_create();
 	if (!cfg) {
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		return -1;
 	}
 
@@ -77,18 +577,21 @@ int main(int argc, char *argv[])
 	/* Process the command line arguments. */
 	progname = strrchr(argv[0], '/');
 	progname = progname ? 1 + progname : argv[0];
-	while (EOF != (c = getopt_long(argc, argv, "c:f:hi:l:mqs:v", opts, &index))) {
+	while (EOF != (c = getopt_long(argc, argv, "ac:f:hi:l:mqs:v", opts, &index))) {
 		switch (c) {
 		case 0:
 			if (config_parse_option(cfg, opts[index].name, optarg)) {
-				ts2phc_cleanup(cfg, master);
+				ts2phc_cleanup(&priv);
 				return -1;
 			}
 			break;
+		case 'a':
+			autocfg = 1;
+			break;
 		case 'c':
 			if (!config_create_interface(optarg, cfg)) {
 				fprintf(stderr, "failed to add slave\n");
-				ts2phc_cleanup(cfg, master);
+				ts2phc_cleanup(&priv);
 				return -1;
 			}
 			have_slave = 1;
@@ -99,7 +602,7 @@ int main(int argc, char *argv[])
 		case 'l':
 			if (get_arg_val_i(c, optarg, &print_level,
 					  PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) {
-				ts2phc_cleanup(cfg, master);
+				ts2phc_cleanup(&priv);
 				return -1;
 			}
 			config_set_int(cfg, "logging_level", print_level);
@@ -116,29 +619,29 @@ int main(int argc, char *argv[])
 		case 's':
 			if (pps_source) {
 				fprintf(stderr, "too many PPS sources\n");
-				ts2phc_cleanup(cfg, master);
+				ts2phc_cleanup(&priv);
 				return -1;
 			}
 			pps_source = optarg;
 			break;
 		case 'v':
-			ts2phc_cleanup(cfg, master);
+			ts2phc_cleanup(&priv);
 			version_show(stdout);
 			return 0;
 		case 'h':
-			ts2phc_cleanup(cfg, master);
+			ts2phc_cleanup(&priv);
 			usage(progname);
 			return -1;
 		case '?':
 		default:
-			ts2phc_cleanup(cfg, master);
+			ts2phc_cleanup(&priv);
 			usage(progname);
 			return -1;
 		}
 	}
 	if (config && (c = config_read(config, cfg))) {
 		fprintf(stderr, "failed to read config\n");
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		return -1;
 	}
 	print_set_progname(progname);
@@ -147,18 +650,54 @@ int main(int argc, char *argv[])
 	print_set_syslog(config_get_int(cfg, NULL, "use_syslog"));
 	print_set_level(config_get_int(cfg, NULL, "logging_level"));
 
-	STAILQ_FOREACH(iface, &cfg->interfaces, list) {
+	STAILQ_INIT(&priv.slaves);
-		if (1 == config_get_int(cfg, interface_name(iface), "ts2phc.master")) {
+	priv.cfg = cfg;
-			if (pps_source) {
+
-				fprintf(stderr, "too many PPS sources\n");
+	snprintf(uds_local, sizeof(uds_local), "/var/run/ts2phc.%d",
-				ts2phc_cleanup(cfg, master);
+		 getpid());
+
+	if (autocfg) {
+		err = init_pmc_node(cfg, &priv.node, uds_local,
+				    ts2phc_recv_subscribed);
+		if (err) {
+			ts2phc_cleanup(&priv);
 			return -1;
 		}
-			pps_source = interface_name(iface);
+		err = auto_init_ports(&priv);
+		if (err) {
+			ts2phc_cleanup(&priv);
+			return -1;
+		}
+	}
+
+	STAILQ_FOREACH(iface, &cfg->interfaces, list) {
+		const char *dev = interface_name(iface);
+
+		if (1 == config_get_int(cfg, dev, "ts2phc.master")) {
+			int perout_phase;
+
+			if (pps_source) {
+				fprintf(stderr, "too many PPS sources\n");
+				ts2phc_cleanup(&priv);
+				return -1;
+			}
+			pps_source = dev;
+			perout_phase = config_get_int(cfg, dev,
+						      "ts2phc.perout_phase");
+			/*
+			 * We use a default value of -1 to distinguish whether
+			 * to use the PTP_PEROUT_PHASE API or not. But if we
+			 * don't use that (and therefore we use absolute start
+			 * time), the phase is still zero, by our application's
+			 * convention.
+			 */
+			if (perout_phase < 0)
+				perout_phase = 0;
+			priv.perout_phase = nanoseconds_to_tmv(perout_phase);
 		} else {
-			if (ts2phc_slave_add(cfg, interface_name(iface))) {
+			if (ts2phc_slave_add(&priv, interface_name(iface))) {
 				fprintf(stderr, "failed to add slave\n");
-				ts2phc_cleanup(cfg, master);
+				ts2phc_cleanup(&priv);
 				return -1;
 			}
 			have_slave = 1;
@@ -166,19 +705,19 @@ int main(int argc, char *argv[])
 	}
 	if (!have_slave) {
 		fprintf(stderr, "no slave clocks specified\n");
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		usage(progname);
 		return -1;
 	}
 	if (!pps_source) {
 		fprintf(stderr, "no PPS source specified\n");
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		usage(progname);
 		return -1;
 	}
-	if (ts2phc_slave_arm()) {
+	if (ts2phc_slaves_init(&priv)) {
-		fprintf(stderr, "failed to arm slaves\n");
+		fprintf(stderr, "failed to initialize slaves\n");
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		return -1;
 	}
 
@@ -189,21 +728,50 @@ int main(int argc, char *argv[])
 	} else {
 		pps_type = TS2PHC_MASTER_PHC;
 	}
-	master = ts2phc_master_create(cfg, pps_source, pps_type);
+	priv.master = ts2phc_master_create(&priv, pps_source, pps_type);
-	if (!master) {
+	if (!priv.master) {
 		fprintf(stderr, "failed to create master\n");
-		ts2phc_cleanup(cfg, master);
+		ts2phc_cleanup(&priv);
 		return -1;
 	}
 
 	while (is_running()) {
-		err = ts2phc_slave_poll(master);
+		struct clock *c;
-		if (err) {
+
+		if (autocfg) {
+			/*
+			 * Make sure ptp4l sees us as alive and doesn't prune
+			 * us from the list of subscribers
+			 */
+			err = update_pmc_node(&priv.node, 1);
+			if (err < 0) {
+				pr_err("update_pmc_node returned %d", err);
+				break;
+			}
+			run_pmc_events(&priv.node);
+			if (priv.state_changed)
+				ts2phc_reconfigure(&priv);
+		}
+
+		LIST_FOREACH(c, &priv.clocks, list)
+			clock_flush_tstamp(c);
+
+		err = ts2phc_slave_poll(&priv);
+		if (err < 0) {
 			pr_err("poll failed");
 			break;
 		}
+		if (err > 0) {
+			err = ts2phc_collect_master_tstamp(&priv);
+			if (err) {
+				pr_err("failed to collect master tstamp");
+				break;
 			}
 
-	ts2phc_cleanup(cfg, master);
+			ts2phc_synchronize_clocks(&priv, autocfg);
+		}
+	}
+
+	ts2phc_cleanup(&priv);
 	return err;
 }

ts2phc.h

@@ -0,0 +1,64 @@
+/**
+ * @file ts2phc.h
+ * @brief Structure definitions for ts2phc
+ * @note Copyright 2020 Vladimir Oltean <olteanv@gmail.com>
+ * @note SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef HAVE_TS2PHC_H
+#define HAVE_TS2PHC_H
+
+#include <sys/queue.h>
+#include <time.h>
+#include "pmc_common.h"
+#include "servo.h"
+
+struct ts2phc_slave_array;
+
+#define SERVO_SYNC_INTERVAL    1.0
+
+struct clock {
+	LIST_ENTRY(clock) list;
+	LIST_ENTRY(clock) dst_list;
+	clockid_t clkid;
+	int phc_index;
+	int state;
+	int new_state;
+	struct servo *servo;
+	enum servo_state servo_state;
+	char *name;
+	int no_adj;
+	int is_destination;
+	int is_ts_available;
+	tmv_t last_ts;
+};
+
+struct port {
+	LIST_ENTRY(port) list;
+	unsigned int number;
+	int state;
+	struct clock *clock;
+};
+
+struct ts2phc_private {
+	struct ts2phc_master *master;
+	STAILQ_HEAD(slave_ifaces_head, ts2phc_slave) slaves;
+	unsigned int n_slaves;
+	struct ts2phc_slave_array *polling_array;
+	tmv_t perout_phase;
+	struct config *cfg;
+	struct pmc_node node;
+	int state_changed;
+	struct clock *source;
+	LIST_HEAD(port_head, port) ports;
+	LIST_HEAD(clock_head, clock) clocks;
+};
+
+struct servo *servo_add(struct ts2phc_private *priv, struct clock *clock);
+struct clock *clock_add(struct ts2phc_private *priv, const char *device);
+void clock_add_tstamp(struct clock *clock, tmv_t ts);
+void clock_destroy(struct clock *clock);
+
+#include "ts2phc_master.h"
+#include "ts2phc_slave.h"
+
+#endif

ts2phc_generic_master.c

@@ -47,7 +47,7 @@ static int ts2phc_generic_master_getppstime(struct ts2phc_master *m,
 	return 0;
 }
 
-struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
+struct ts2phc_master *ts2phc_generic_master_create(struct ts2phc_private *priv,
 						   const char *dev)
 {
 	struct ts2phc_generic_master *master;

ts2phc_generic_master.h

@@ -6,9 +6,10 @@
 #ifndef HAVE_TS2PHC_GENERIC_MASTER_H
 #define HAVE_TS2PHC_GENERIC_MASTER_H
 
+#include "ts2phc.h"
 #include "ts2phc_master.h"
 
-struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
+struct ts2phc_master *ts2phc_generic_master_create(struct ts2phc_private *priv,
 						   const char *dev);
 
 #endif

ts2phc_master.c

@@ -3,25 +3,27 @@
  * @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
  * @note SPDX-License-Identifier: GPL-2.0+
  */
+#include "ts2phc.h"
 #include "ts2phc_generic_master.h"
 #include "ts2phc_master_private.h"
 #include "ts2phc_nmea_master.h"
 #include "ts2phc_phc_master.h"
 
-struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
+struct ts2phc_master *ts2phc_master_create(struct ts2phc_private *priv,
+					   const char *dev,
 					   enum ts2phc_master_type type)
 {
 	struct ts2phc_master *master = NULL;
 
 	switch (type) {
 	case TS2PHC_MASTER_GENERIC:
-		master = ts2phc_generic_master_create(cfg, dev);
+		master = ts2phc_generic_master_create(priv, dev);
 		break;
 	case TS2PHC_MASTER_NMEA:
-		master = ts2phc_nmea_master_create(cfg, dev);
+		master = ts2phc_nmea_master_create(priv, dev);
 		break;
 	case TS2PHC_MASTER_PHC:
-		master = ts2phc_phc_master_create(cfg, dev);
+		master = ts2phc_phc_master_create(priv, dev);
 		break;
 	}
 	return master;
@@ -36,3 +38,11 @@ int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts)
 {
 	return master->getppstime(master, ts);
 }
+
+struct clock *ts2phc_master_get_clock(struct ts2phc_master *m)
+{
+	if (m->get_clock)
+		return m->get_clock(m);
+
+	return NULL;
+}

ts2phc_master.h

@@ -13,6 +13,7 @@ struct config;
 /**
  * Opaque type
  */
+struct ts2phc_private;
 struct ts2phc_master;
 
 /**
@@ -26,12 +27,13 @@ enum ts2phc_master_type {
 
 /**
  * Create a new instance of a PPS master clock.
- * @param cfg	Pointer to a valid configuration.
+ * @param priv	Pointer to the program's data structure.
  * @param dev   Name of the master clock or NULL.
  * @param type	The type of the clock to create.
  * @return	A pointer to a new PPS master clock on success, NULL otherwise.
  */
-struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
+struct ts2phc_master *ts2phc_master_create(struct ts2phc_private *priv,
+					   const char *dev,
 					   enum ts2phc_master_type type);
 
 /**
@@ -49,4 +51,6 @@ void ts2phc_master_destroy(struct ts2phc_master *master);
  */
 int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts);
 
+struct clock *ts2phc_master_get_clock(struct ts2phc_master *m);
+
 #endif

ts2phc_master_private.h

@@ -15,6 +15,7 @@
 struct ts2phc_master {
 	void (*destroy)(struct ts2phc_master *ts2phc_master);
 	int (*getppstime)(struct ts2phc_master *master, struct timespec *ts);
+	struct clock *(*get_clock)(struct ts2phc_master *m);
 };
 
 #endif

ts2phc_nmea_master.c

@@ -198,7 +198,8 @@ static int ts2phc_nmea_master_getppstime(struct ts2phc_master *master,
 	return fix_valid ? lstab_error : -1;
 }
 
-struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg, const char *dev)
+struct ts2phc_master *ts2phc_nmea_master_create(struct ts2phc_private *priv,
+						const char *dev)
 {
 	struct ts2phc_nmea_master *master;
 	const char *leapfile = NULL;	// TODO - read from config.
@@ -214,7 +215,7 @@ struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg, const char *
 	}
 	master->master.destroy = ts2phc_nmea_master_destroy;
 	master->master.getppstime = ts2phc_nmea_master_getppstime;
-	master->config = cfg;
+	master->config = priv->cfg;
 	pthread_mutex_init(&master->mutex, NULL);
 	err = pthread_create(&master->worker, NULL, monitor_nmea_status, master);
 	if (err) {

ts2phc_nmea_master.h

@@ -6,8 +6,9 @@
 #ifndef HAVE_TS2PHC_NMEA_MASTER_H
 #define HAVE_TS2PHC_NMEA_MASTER_H
 
+#include "ts2phc.h"
 #include "ts2phc_master.h"
 
-struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg,
+struct ts2phc_master *ts2phc_nmea_master_create(struct ts2phc_private *priv,
 						const char *dev);
 #endif

ts2phc_phc_master.c

@@ -12,15 +12,14 @@
 #include "phc.h"
 #include "print.h"
 #include "missing.h"
+#include "ts2phc.h"
 #include "ts2phc_master_private.h"
-#include "ts2phc_phc_master.h"
 #include "util.h"
 
 struct ts2phc_phc_master {
 	struct ts2phc_master master;
-	clockid_t clkid;
+	struct clock *clock;
 	int channel;
-	int fd;
 };
 
 static int ts2phc_phc_master_activate(struct config *cfg, const char *dev,
@@ -28,6 +27,8 @@ static int ts2phc_phc_master_activate(struct config *cfg, const char *dev,
 {
 	struct ptp_perout_request perout_request;
 	struct ptp_pin_desc desc;
+	int32_t perout_phase;
+	int32_t pulsewidth;
 	struct timespec ts;
 
 	memset(&desc, 0, sizeof(desc));
@@ -38,21 +39,36 @@ static int ts2phc_phc_master_activate(struct config *cfg, const char *dev,
 	desc.func = PTP_PF_PEROUT;
 	desc.chan = master->channel;
 
-	if (phc_pin_setfunc(master->clkid, &desc)) {
+	if (phc_pin_setfunc(master->clock->clkid, &desc)) {
 		pr_warning("Failed to set the pin. Continuing bravely on...");
 	}
-	if (clock_gettime(master->clkid, &ts)) {
+	if (clock_gettime(master->clock->clkid, &ts)) {
 		perror("clock_gettime");
 		return -1;
 	}
+	perout_phase = config_get_int(cfg, dev, "ts2phc.perout_phase");
 	memset(&perout_request, 0, sizeof(perout_request));
 	perout_request.index = master->channel;
-	perout_request.start.sec = ts.tv_sec + 2;
-	perout_request.start.nsec = 0;
 	perout_request.period.sec = 1;
 	perout_request.period.nsec = 0;
+	perout_request.flags = 0;
+	pulsewidth = config_get_int(cfg, dev, "ts2phc.pulsewidth");
+	if (pulsewidth) {
+		perout_request.flags |= PTP_PEROUT_DUTY_CYCLE;
+		perout_request.on.sec = pulsewidth / NS_PER_SEC;
+		perout_request.on.nsec = pulsewidth % NS_PER_SEC;
+	}
+	if (perout_phase != -1) {
+		perout_request.flags |= PTP_PEROUT_PHASE;
+		perout_request.phase.sec = perout_phase / NS_PER_SEC;
+		perout_request.phase.nsec = perout_phase % NS_PER_SEC;
+	} else {
+		perout_request.start.sec = ts.tv_sec + 2;
+		perout_request.start.nsec = 0;
+	}
 
-	if (ioctl(master->fd, PTP_PEROUT_REQUEST2, &perout_request)) {
+	if (ioctl(CLOCKID_TO_FD(master->clock->clkid), PTP_PEROUT_REQUEST2,
+		  &perout_request)) {
 		pr_err(PTP_PEROUT_REQUEST_FAILED);
 		return -1;
 	}
@@ -67,10 +83,11 @@ static void ts2phc_phc_master_destroy(struct ts2phc_master *master)
 
 	memset(&perout_request, 0, sizeof(perout_request));
 	perout_request.index = m->channel;
-	if (ioctl(m->fd, PTP_PEROUT_REQUEST2, &perout_request)) {
+	if (ioctl(CLOCKID_TO_FD(m->clock->clkid), PTP_PEROUT_REQUEST2,
+		  &perout_request)) {
 		pr_err(PTP_PEROUT_REQUEST_FAILED);
 	}
-	posix_clock_close(m->clkid);
+	clock_destroy(m->clock);
 	free(m);
 }
 
@@ -79,14 +96,21 @@ static int ts2phc_phc_master_getppstime(struct ts2phc_master *m,
 {
 	struct ts2phc_phc_master *master =
 		container_of(m, struct ts2phc_phc_master, master);
-	return clock_gettime(master->clkid, ts);
+	return clock_gettime(master->clock->clkid, ts);
 }
 
-struct ts2phc_master *ts2phc_phc_master_create(struct config *cfg,
+struct clock *ts2phc_phc_master_get_clock(struct ts2phc_master *m)
+{
+	struct ts2phc_phc_master *master =
+		container_of(m, struct ts2phc_phc_master, master);
+
+	return master->clock;
+}
+
+struct ts2phc_master *ts2phc_phc_master_create(struct ts2phc_private *priv,
 					       const char *dev)
 {
 	struct ts2phc_phc_master *master;
-	int junk;
 
 	master = calloc(1, sizeof(*master));
 	if (!master) {
@@ -94,17 +118,19 @@ struct ts2phc_master *ts2phc_phc_master_create(struct config *cfg,
 	}
 	master->master.destroy = ts2phc_phc_master_destroy;
 	master->master.getppstime = ts2phc_phc_master_getppstime;
+	master->master.get_clock = ts2phc_phc_master_get_clock;
 
-	master->clkid = posix_clock_open(dev, &junk);
+	master->clock = clock_add(priv, dev);
-	if (master->clkid == CLOCK_INVALID) {
+	if (!master->clock) {
 		free(master);
 		return NULL;
 	}
-	master->fd = CLOCKID_TO_FD(master->clkid);
+	master->clock->is_destination = 0;
 
-	pr_debug("PHC master %s has ptp index %d", dev, junk);
+	pr_debug("PHC master %s has ptp index %d", dev,
+		 master->clock->phc_index);
 
-	if (ts2phc_phc_master_activate(cfg, dev, master)) {
+	if (ts2phc_phc_master_activate(priv->cfg, dev, master)) {
 		ts2phc_phc_master_destroy(&master->master);
 		return NULL;
 	}

ts2phc_phc_master.h

@@ -6,9 +6,10 @@
 #ifndef HAVE_TS2PHC_PHC_MASTER_H
 #define HAVE_TS2PHC_PHC_MASTER_H
 
+#include "ts2phc.h"
 #include "ts2phc_master.h"
 
-struct ts2phc_master *ts2phc_phc_master_create(struct config *cfg,
+struct ts2phc_master *ts2phc_phc_master_create(struct ts2phc_private *priv,
 					       const char *dev);
 
 #endif

ts2phc_slave.c

@@ -15,43 +15,30 @@
 #include <time.h>
 #include <unistd.h>
 
-#include "config.h"
 #include "clockadj.h"
+#include "config.h"
 #include "missing.h"
 #include "phc.h"
 #include "print.h"
 #include "servo.h"
-#include "ts2phc_master.h"
+#include "ts2phc.h"
-#include "ts2phc_slave.h"
 #include "util.h"
 
-#define NS_PER_SEC		1000000000LL
-#define SAMPLE_WEIGHT		1.0
-#define SERVO_SYNC_INTERVAL	1.0
-
 struct ts2phc_slave {
 	char *name;
 	STAILQ_ENTRY(ts2phc_slave) list;
 	struct ptp_pin_desc pin_desc;
-	enum servo_state state;
 	unsigned int polarity;
-	int32_t correction;
+	tmv_t correction;
 	uint32_t ignore_lower;
 	uint32_t ignore_upper;
-	struct servo *servo;
+	struct clock *clock;
-	clockid_t clk;
-	int no_adj;
-	int fd;
 };
 
 struct ts2phc_slave_array {
 	struct ts2phc_slave **slave;
+	int *collected_events;
 	struct pollfd *pfd;
-} polling_array;
-
-struct ts2phc_source_timestamp {
-	struct timespec ts;
-	bool valid;
 };
 
 enum extts_result {
@@ -60,61 +47,77 @@ enum extts_result {
 	EXTTS_IGNORE	= 1,
 };
 
-static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
+static int ts2phc_slave_array_create(struct ts2phc_private *priv)
-					     struct ts2phc_source_timestamp ts,
-					     int64_t *offset,
-					     uint64_t *local_ts);
-
-static STAILQ_HEAD(slave_ifaces_head, ts2phc_slave) ts2phc_slaves =
-	STAILQ_HEAD_INITIALIZER(ts2phc_slaves);
-
-static unsigned int ts2phc_n_slaves;
-
-static int ts2phc_slave_array_create(void)
 {
+	struct ts2phc_slave_array *polling_array;
 	struct ts2phc_slave *slave;
 	unsigned int i;
 
-	if (polling_array.slave) {
+	polling_array = malloc(sizeof(*polling_array));
-		return 0;
+	if (!polling_array) {
-	}
-	polling_array.slave = malloc(ts2phc_n_slaves * sizeof(*polling_array.slave));
-	if (!polling_array.slave) {
 		pr_err("low memory");
 		return -1;
 	}
-	polling_array.pfd = malloc(ts2phc_n_slaves * sizeof(*polling_array.pfd));
+
-	if (!polling_array.pfd) {
+	polling_array->slave = malloc(priv->n_slaves *
+				      sizeof(*polling_array->slave));
+	if (!polling_array->slave) {
 		pr_err("low memory");
-		free(polling_array.slave);
+		return -1;
-		polling_array.slave = NULL;
+	}
+	polling_array->pfd = malloc(priv->n_slaves *
+				    sizeof(*polling_array->pfd));
+	if (!polling_array->pfd) {
+		pr_err("low memory");
+		free(polling_array->slave);
+		polling_array->slave = NULL;
+		return -1;
+	}
+	polling_array->collected_events = malloc(priv->n_slaves * sizeof(int));
+	if (!polling_array->collected_events) {
+		pr_err("low memory");
+		free(polling_array->slave);
+		free(polling_array->pfd);
+		polling_array->pfd = NULL;
+		polling_array->slave = NULL;
 		return -1;
 	}
 	i = 0;
-	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
+	STAILQ_FOREACH(slave, &priv->slaves, list) {
-		polling_array.slave[i] = slave;
+		polling_array->slave[i] = slave;
 		i++;
 	}
-	for (i = 0; i < ts2phc_n_slaves; i++) {
+	for (i = 0; i < priv->n_slaves; i++) {
-		polling_array.pfd[i].events = POLLIN | POLLPRI;
+		struct ts2phc_slave *slave = polling_array->slave[i];
-		polling_array.pfd[i].fd = polling_array.slave[i]->fd;
+
+		polling_array->pfd[i].events = POLLIN | POLLPRI;
+		polling_array->pfd[i].fd = CLOCKID_TO_FD(slave->clock->clkid);
 	}
+
+	priv->polling_array = polling_array;
+
 	return 0;
 }
 
-static void ts2phc_slave_array_destroy(void)
+static void ts2phc_slave_array_destroy(struct ts2phc_private *priv)
 {
-	free(polling_array.slave);
+	struct ts2phc_slave_array *polling_array = priv->polling_array;
-	free(polling_array.pfd);
+
-	polling_array.slave = NULL;
+	if (!polling_array)
-	polling_array.pfd = NULL;
+		return;
+
+	free(polling_array->slave);
+	free(polling_array->pfd);
+	free(polling_array->collected_events);
+	free(polling_array);
+	priv->polling_array = NULL;
 }
 
 static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave)
 {
 	struct pollfd pfd = {
 		.events = POLLIN | POLLPRI,
-		.fd = slave->fd,
+		.fd = CLOCKID_TO_FD(slave->clock->clkid),
 	};
 	struct ptp_extts_event event;
 	int cnt, size;
@@ -143,12 +146,13 @@ static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave)
 	return 0;
 }
 
-static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *device)
+static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
+						const char *device)
 {
-	enum servo_type servo = config_get_int(cfg, NULL, "clock_servo");
-	int err, fadj, junk, max_adj, pulsewidth;
 	struct ptp_extts_request extts;
 	struct ts2phc_slave *slave;
+	int err, pulsewidth;
+	int32_t correction;
 
 	slave = calloc(1, sizeof(*slave));
 	if (!slave) {
@@ -161,44 +165,35 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *
 		free(slave);
 		return NULL;
 	}
-	slave->pin_desc.index = config_get_int(cfg, device, "ts2phc.pin_index");
+	slave->pin_desc.index = config_get_int(priv->cfg, device,
+					       "ts2phc.pin_index");
 	slave->pin_desc.func = PTP_PF_EXTTS;
-	slave->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel");
+	slave->pin_desc.chan = config_get_int(priv->cfg, device,
-	slave->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity");
+					      "ts2phc.channel");
-	slave->correction = config_get_int(cfg, device, "ts2phc.extts_correction");
+	slave->polarity = config_get_int(priv->cfg, device,
+					 "ts2phc.extts_polarity");
+	correction = config_get_int(priv->cfg, device,
+				    "ts2phc.extts_correction");
+	slave->correction = nanoseconds_to_tmv(correction);
 
-	pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth");
+	pulsewidth = config_get_int(priv->cfg, device,
+				    "ts2phc.pulsewidth");
 	pulsewidth /= 2;
 	slave->ignore_upper = 1000000000 - pulsewidth;
 	slave->ignore_lower = pulsewidth;
 
-	slave->clk = posix_clock_open(device, &junk);
+	slave->clock = clock_add(priv, device);
-	if (slave->clk == CLOCK_INVALID) {
+	if (!slave->clock) {
 		pr_err("failed to open clock");
 		goto no_posix_clock;
 	}
-	slave->no_adj = config_get_int(cfg, NULL, "free_running");
+	slave->clock->is_destination = 1;
-	slave->fd = CLOCKID_TO_FD(slave->clk);
 
-	pr_debug("PHC slave %s has ptp index %d", device, junk);
+	pr_debug("PHC slave %s has ptp index %d", device,
+		 slave->clock->phc_index);
 
-	fadj = (int) clockadj_get_freq(slave->clk);
+	if (phc_number_pins(slave->clock->clkid) > 0) {
-	/* Due to a bug in older kernels, the reading may silently fail
+		err = phc_pin_setfunc(slave->clock->clkid, &slave->pin_desc);
-	   and return 0. Set the frequency back to make sure fadj is
-	   the actual frequency of the clock. */
-	clockadj_set_freq(slave->clk, fadj);
-
-	max_adj = phc_max_adj(slave->clk);
-
-	slave->servo = servo_create(cfg, servo, -fadj, max_adj, 0);
-	if (!slave->servo) {
-		pr_err("failed to create servo");
-		goto no_servo;
-	}
-	servo_sync_interval(slave->servo, SERVO_SYNC_INTERVAL);
-
-	if (phc_number_pins(slave->clk) > 0) {
-		err = phc_pin_setfunc(slave->clk, &slave->pin_desc);
 		if (err < 0) {
 			pr_err("PTP_PIN_SETFUNC request failed");
 			goto no_pin_func;
@@ -212,7 +207,8 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *
 	memset(&extts, 0, sizeof(extts));
 	extts.index = slave->pin_desc.chan;
 	extts.flags = 0;
-	if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
+	if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
+		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
 	if (ts2phc_slave_clear_fifo(slave)) {
@@ -222,9 +218,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *
 	return slave;
 no_ext_ts:
 no_pin_func:
-	servo_destroy(slave->servo);
+	clock_destroy(slave->clock);
-no_servo:
-	posix_clock_close(slave->clk);
 no_posix_clock:
 	free(slave->name);
 	free(slave);
@@ -238,136 +232,101 @@ static void ts2phc_slave_destroy(struct ts2phc_slave *slave)
 	memset(&extts, 0, sizeof(extts));
 	extts.index = slave->pin_desc.chan;
 	extts.flags = 0;
-	if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
+	if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
+		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
-	servo_destroy(slave->servo);
+	clock_destroy(slave->clock);
-	posix_clock_close(slave->clk);
 	free(slave->name);
 	free(slave);
 }
 
-static int ts2phc_slave_event(struct ts2phc_slave *slave,
+static bool ts2phc_slave_ignore(struct ts2phc_private *priv,
-			      struct ts2phc_source_timestamp source_ts)
+				struct ts2phc_slave *slave,
+				struct timespec source_ts)
 {
-	enum extts_result result;
+	tmv_t source_tmv = timespec_to_tmv(source_ts);
-	uint64_t extts_ts;
-	int64_t offset;
-	double adj;
 
-	result = ts2phc_slave_offset(slave, source_ts, &offset, &extts_ts);
+	source_tmv = tmv_sub(source_tmv, priv->perout_phase);
-	switch (result) {
+	source_ts = tmv_to_timespec(source_tmv);
-	case EXTTS_ERROR:
-		return -1;
-	case EXTTS_OK:
-		break;
-	case EXTTS_IGNORE:
-		return 0;
-	}
 
-	if (slave->no_adj) {
+	return source_ts.tv_nsec > slave->ignore_lower &&
-		pr_info("%s master offset %10" PRId64, slave->name, offset);
+	       source_ts.tv_nsec < slave->ignore_upper;
-		return 0;
-	}
-
-	if (!source_ts.valid) {
-		pr_debug("%s ignoring invalid master time stamp", slave->name);
-		return 0;
-	}
-
-	adj = servo_sample(slave->servo, offset, extts_ts,
-			   SAMPLE_WEIGHT, &slave->state);
-
-	pr_debug("%s master offset %10" PRId64 " s%d freq %+7.0f",
-		 slave->name, offset, slave->state, adj);
-
-	switch (slave->state) {
-	case SERVO_UNLOCKED:
-		break;
-	case SERVO_JUMP:
-		clockadj_set_freq(slave->clk, -adj);
-		clockadj_step(slave->clk, -offset);
-		break;
-	case SERVO_LOCKED:
-	case SERVO_LOCKED_STABLE:
-		clockadj_set_freq(slave->clk, -adj);
-		break;
-	}
-	return 0;
 }
 
-static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
+static enum extts_result ts2phc_slave_event(struct ts2phc_private *priv,
-					     struct ts2phc_source_timestamp src,
+					    struct ts2phc_slave *slave)
-					     int64_t *offset,
-					     uint64_t *local_ts)
 {
-	struct timespec source_ts = src.ts;
+	enum extts_result result = EXTTS_OK;
 	struct ptp_extts_event event;
-	uint64_t event_ns, source_ns;
+	struct timespec source_ts;
-	int cnt;
+	int err, cnt;
+	tmv_t ts;
 
-	cnt = read(slave->fd, &event, sizeof(event));
+	cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event));
 	if (cnt != sizeof(event)) {
 		pr_err("read extts event failed: %m");
-		return EXTTS_ERROR;
+		result = EXTTS_ERROR;
+		goto out;
 	}
 	if (event.index != slave->pin_desc.chan) {
 		pr_err("extts on unexpected channel");
-		return EXTTS_ERROR;
+		result = EXTTS_ERROR;
+		goto out;
+	}
+
+	err = ts2phc_master_getppstime(priv->master, &source_ts);
+	if (err < 0) {
+		pr_debug("source ts not valid");
+		return 0;
 	}
-	event_ns = event.t.sec * NS_PER_SEC;
-	event_ns += event.t.nsec;
 
 	if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
-	    source_ts.tv_nsec > slave->ignore_lower &&
+	    ts2phc_slave_ignore(priv, slave, source_ts)) {
-	    source_ts.tv_nsec < slave->ignore_upper) {
 
 		pr_debug("%s SKIP extts index %u at %lld.%09u src %" PRIi64 ".%ld",
 		 slave->name, event.index, event.t.sec, event.t.nsec,
 		 (int64_t) source_ts.tv_sec, source_ts.tv_nsec);
 
-		return EXTTS_IGNORE;
+		result = EXTTS_IGNORE;
+		goto out;
 	}
-	if (source_ts.tv_nsec > 500000000) {
-		source_ts.tv_sec++;
-	}
-	source_ns = source_ts.tv_sec * NS_PER_SEC;
-	*offset = event_ns + slave->correction - source_ns;
-	*local_ts = event_ns + slave->correction;
 
-	pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64
+out:
-		 ".%ld diff %" PRId64,
+	if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
-		 slave->name, event.index, event.t.sec, event.t.nsec,
+		return result;
-		 slave->correction,
+
-		 (int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset);
+	ts = pct_to_tmv(event.t);
+	ts = tmv_add(ts, slave->correction);
+	clock_add_tstamp(slave->clock, ts);
 
 	return EXTTS_OK;
 }
 
 /* public methods */
 
-int ts2phc_slave_add(struct config *cfg, const char *name)
+int ts2phc_slave_add(struct ts2phc_private *priv, const char *name)
 {
 	struct ts2phc_slave *slave;
 
 	/* Create each interface only once. */
-	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
+	STAILQ_FOREACH(slave, &priv->slaves, list) {
 		if (0 == strcmp(name, slave->name)) {
 			return 0;
 		}
 	}
-	slave = ts2phc_slave_create(cfg, name);
+	slave = ts2phc_slave_create(priv, name);
 	if (!slave) {
 		pr_err("failed to create slave");
 		return -1;
 	}
-	STAILQ_INSERT_TAIL(&ts2phc_slaves, slave, list);
+	STAILQ_INSERT_TAIL(&priv->slaves, slave, list);
-	ts2phc_n_slaves++;
+	priv->n_slaves++;
 
 	return 0;
 }
 
-int ts2phc_slave_arm(void)
+int ts2phc_slave_arm(struct ts2phc_private *priv)
 {
 	struct ptp_extts_request extts;
 	struct ts2phc_slave *slave;
@@ -375,10 +334,11 @@ int ts2phc_slave_arm(void)
 
 	memset(&extts, 0, sizeof(extts));
 
-	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
+	STAILQ_FOREACH(slave, &priv->slaves, list) {
 		extts.index = slave->pin_desc.chan;
 		extts.flags = slave->polarity | PTP_ENABLE_FEATURE;
-		err = ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts);
+		err = ioctl(CLOCKID_TO_FD(slave->clock->clkid),
+			    PTP_EXTTS_REQUEST2, &extts);
 		if (err < 0) {
 			pr_err(PTP_EXTTS_REQUEST_FAILED);
 			return -1;
@@ -387,29 +347,45 @@ int ts2phc_slave_arm(void)
 	return 0;
 }
 
-void ts2phc_slave_cleanup(void)
+int ts2phc_slaves_init(struct ts2phc_private *priv)
+{
+	int err;
+
+	err = ts2phc_slave_array_create(priv);
+	if (err)
+		return err;
+
+	return ts2phc_slave_arm(priv);
+}
+
+void ts2phc_slave_cleanup(struct ts2phc_private *priv)
 {
 	struct ts2phc_slave *slave;
 
-	ts2phc_slave_array_destroy();
+	ts2phc_slave_array_destroy(priv);
 
-	while ((slave = STAILQ_FIRST(&ts2phc_slaves))) {
+	while ((slave = STAILQ_FIRST(&priv->slaves))) {
-		STAILQ_REMOVE_HEAD(&ts2phc_slaves, list);
+		STAILQ_REMOVE_HEAD(&priv->slaves, list);
 		ts2phc_slave_destroy(slave);
-		ts2phc_n_slaves--;
+		priv->n_slaves--;
 	}
 }
 
-int ts2phc_slave_poll(struct ts2phc_master *master)
+int ts2phc_slave_poll(struct ts2phc_private *priv)
 {
-	struct ts2phc_source_timestamp source_ts;
+	struct ts2phc_slave_array *polling_array = priv->polling_array;
+	int all_slaves_have_events = 0;
+	int ignore_any = 0;
 	unsigned int i;
-	int cnt, err;
+	int cnt;
 
-	if (ts2phc_slave_array_create()) {
+	for (i = 0; i < priv->n_slaves; i++)
-		return -1;
+		polling_array->collected_events[i] = 0;
-	}
+
-	cnt = poll(polling_array.pfd, ts2phc_n_slaves, 2000);
+	while (!all_slaves_have_events) {
+		struct ts2phc_slave *slave;
+
+		cnt = poll(polling_array->pfd, priv->n_slaves, 2000);
 		if (cnt < 0) {
 			if (EINTR == errno) {
 				return 0;
@@ -422,13 +398,40 @@ int ts2phc_slave_poll(struct ts2phc_master *master)
 			return 0;
 		}
 
-	err = ts2phc_master_getppstime(master, &source_ts.ts);
+		for (i = 0; i < priv->n_slaves; i++) {
-	source_ts.valid = err ? false : true;
+			if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
+				enum extts_result result;
 
-	for (i = 0; i < ts2phc_n_slaves; i++) {
+				slave = polling_array->slave[i];
-		if (polling_array.pfd[i].revents & (POLLIN|POLLPRI)) {
+
-			ts2phc_slave_event(polling_array.slave[i], source_ts);
+				result = ts2phc_slave_event(priv, slave);
+				if (result == EXTTS_ERROR)
+					return -EIO;
+				if (result == EXTTS_IGNORE)
+					ignore_any = 1;
+
+				/*
+				 * Collect the events anyway, even if we'll
+				 * ignore this master edge anyway. We don't
+				 * want slave events from different edges
+				 * to pile up and mix.
+				 */
+				polling_array->collected_events[i]++;
 			}
 		}
+
+		all_slaves_have_events = true;
+
+		for (i = 0; i < priv->n_slaves; i++) {
+			if (!polling_array->collected_events[i]) {
+				all_slaves_have_events = false;
+				break;
+			}
+		}
+	}
+
+	if (ignore_any)
 		return 0;
+
+	return 1;
 }

ts2phc_slave.h

@@ -7,14 +7,14 @@
 #ifndef HAVE_TS2PHC_SLAVE_H
 #define HAVE_TS2PHC_SLAVE_H
 
-#include "ts2phc_master.h"
+#include "ts2phc.h"
 
-int ts2phc_slave_add(struct config *cfg, const char *name);
+int ts2phc_slave_add(struct ts2phc_private *priv, const char *name);
 
-int ts2phc_slave_arm(void);
+int ts2phc_slaves_init(struct ts2phc_private *priv);
 
-void ts2phc_slave_cleanup(void);
+void ts2phc_slave_cleanup(struct ts2phc_private *priv);
 
-int ts2phc_slave_poll(struct ts2phc_master *master);
+int ts2phc_slave_poll(struct ts2phc_private *priv);
 
 #endif