config.c

@@ -314,7 +314,6 @@ 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,8 +27,7 @@ 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,59 +97,6 @@ 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,6 +63,12 @@
 #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;
@@ -94,6 +100,24 @@ 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;
@@ -112,11 +136,17 @@ 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)
@@ -773,6 +803,56 @@ 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 &&
@@ -843,6 +923,260 @@ 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;
@@ -921,6 +1255,30 @@ 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,8 +18,6 @@
  * 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>
@@ -58,13 +56,6 @@
 /* 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);
@@ -720,331 +711,3 @@ 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,7 +22,6 @@
 #define HAVE_PMC_COMMON_H
 
 #include "config.h"
-#include "fsm.h"
 #include "msg.h"
 #include "transport.h"
 
@@ -50,38 +49,4 @@ 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,21 +26,6 @@ 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)
@@ -167,11 +152,6 @@ 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,524 +7,27 @@
  * @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.h"
+#include "ts2phc_master.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 ts2phc_private *priv)
+static void ts2phc_cleanup(struct config *cfg, struct ts2phc_master *master)
 {
-	struct port *p, *tmp;
+	ts2phc_slave_cleanup();
-
+	if (master) {
-	ts2phc_slave_cleanup(priv);
+		ts2phc_master_destroy(master);
-	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();
 	}
-
+	if (cfg) {
-/* FIXME: Copied from phc2sys */
+		config_destroy(cfg);
-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;
 	}
-	return state;
-}
-
-/* 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)
@@ -532,7 +35,6 @@ 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"
@@ -554,21 +56,19 @@ static void usage(char *progname)
 int main(int argc, char *argv[])
 {
 	int c, err = 0, have_slave = 0, index, print_level;
-	char uds_local[MAX_IFNAME_SIZE + 1];
+	struct ts2phc_master *master = NULL;
 	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(&priv);
+		ts2phc_cleanup(cfg, master);
 		return -1;
 	}
 
@@ -577,21 +77,18 @@ 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, "ac:f:hi:l:mqs:v", opts, &index))) {
+	while (EOF != (c = getopt_long(argc, argv, "c:f:hi:l:mqs:v", opts, &index))) {
 		switch (c) {
 		case 0:
 			if (config_parse_option(cfg, opts[index].name, optarg)) {
-				ts2phc_cleanup(&priv);
+				ts2phc_cleanup(cfg, master);
 				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(&priv);
+				ts2phc_cleanup(cfg, master);
 				return -1;
 			}
 			have_slave = 1;
@@ -602,7 +99,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(&priv);
+				ts2phc_cleanup(cfg, master);
 				return -1;
 			}
 			config_set_int(cfg, "logging_level", print_level);
@@ -619,29 +116,29 @@ int main(int argc, char *argv[])
 		case 's':
 			if (pps_source) {
 				fprintf(stderr, "too many PPS sources\n");
-				ts2phc_cleanup(&priv);
+				ts2phc_cleanup(cfg, master);
 				return -1;
 			}
 			pps_source = optarg;
 			break;
 		case 'v':
-			ts2phc_cleanup(&priv);
+			ts2phc_cleanup(cfg, master);
 			version_show(stdout);
 			return 0;
 		case 'h':
-			ts2phc_cleanup(&priv);
+			ts2phc_cleanup(cfg, master);
 			usage(progname);
 			return -1;
 		case '?':
 		default:
-			ts2phc_cleanup(&priv);
+			ts2phc_cleanup(cfg, master);
 			usage(progname);
 			return -1;
 		}
 	}
 	if (config && (c = config_read(config, cfg))) {
 		fprintf(stderr, "failed to read config\n");
-		ts2phc_cleanup(&priv);
+		ts2phc_cleanup(cfg, master);
 		return -1;
 	}
 	print_set_progname(progname);
@@ -650,54 +147,18 @@ 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_INIT(&priv.slaves);
-	priv.cfg = cfg;
-
-	snprintf(uds_local, sizeof(uds_local), "/var/run/ts2phc.%d",
-		 getpid());
-
-	if (autocfg) {
-		err = init_pmc_node(cfg, &priv.node, uds_local,
-				    ts2phc_recv_subscribed);
-		if (err) {
-			ts2phc_cleanup(&priv);
-			return -1;
-		}
-		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, interface_name(iface), "ts2phc.master")) {
-
-		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);
+				ts2phc_cleanup(cfg, master);
 				return -1;
 			}
-			pps_source = dev;
+			pps_source = interface_name(iface);
-			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(&priv, interface_name(iface))) {
+			if (ts2phc_slave_add(cfg, interface_name(iface))) {
 				fprintf(stderr, "failed to add slave\n");
-				ts2phc_cleanup(&priv);
+				ts2phc_cleanup(cfg, master);
 				return -1;
 			}
 			have_slave = 1;
@@ -705,19 +166,19 @@ int main(int argc, char *argv[])
 	}
 	if (!have_slave) {
 		fprintf(stderr, "no slave clocks specified\n");
-		ts2phc_cleanup(&priv);
+		ts2phc_cleanup(cfg, master);
 		usage(progname);
 		return -1;
 	}
 	if (!pps_source) {
 		fprintf(stderr, "no PPS source specified\n");
-		ts2phc_cleanup(&priv);
+		ts2phc_cleanup(cfg, master);
 		usage(progname);
 		return -1;
 	}
-	if (ts2phc_slaves_init(&priv)) {
+	if (ts2phc_slave_arm()) {
-		fprintf(stderr, "failed to initialize slaves\n");
+		fprintf(stderr, "failed to arm slaves\n");
-		ts2phc_cleanup(&priv);
+		ts2phc_cleanup(cfg, master);
 		return -1;
 	}
 
@@ -728,50 +189,21 @@ int main(int argc, char *argv[])
 	} else {
 		pps_type = TS2PHC_MASTER_PHC;
 	}
-	priv.master = ts2phc_master_create(&priv, pps_source, pps_type);
+	master = ts2phc_master_create(cfg, pps_source, pps_type);
-	if (!priv.master) {
+	if (!master) {
 		fprintf(stderr, "failed to create master\n");
-		ts2phc_cleanup(&priv);
+		ts2phc_cleanup(cfg, master);
 		return -1;
 	}
 
 	while (is_running()) {
-		struct clock *c;
+		err = ts2phc_slave_poll(master);
-
+		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_synchronize_clocks(&priv, autocfg);
+	ts2phc_cleanup(cfg, master);
-		}
-	}
-
-	ts2phc_cleanup(&priv);
 	return err;
 }

ts2phc.h

@@ -1,64 +0,0 @@
-/**
- * @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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
 						   const char *dev)
 {
 	struct ts2phc_generic_master *master;

ts2phc_generic_master.h

@@ -6,10 +6,9 @@
 #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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
 						   const char *dev);
 
 #endif

ts2phc_master.c

@@ -3,27 +3,25 @@
  * @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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
-					   const char *dev,
 					   enum ts2phc_master_type type)
 {
 	struct ts2phc_master *master = NULL;
 
 	switch (type) {
 	case TS2PHC_MASTER_GENERIC:
-		master = ts2phc_generic_master_create(priv, dev);
+		master = ts2phc_generic_master_create(cfg, dev);
 		break;
 	case TS2PHC_MASTER_NMEA:
-		master = ts2phc_nmea_master_create(priv, dev);
+		master = ts2phc_nmea_master_create(cfg, dev);
 		break;
 	case TS2PHC_MASTER_PHC:
-		master = ts2phc_phc_master_create(priv, dev);
+		master = ts2phc_phc_master_create(cfg, dev);
 		break;
 	}
 	return master;
@@ -38,11 +36,3 @@ 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,7 +13,6 @@ struct config;
 /**
  * Opaque type
  */
-struct ts2phc_private;
 struct ts2phc_master;
 
 /**
@@ -27,13 +26,12 @@ enum ts2phc_master_type {
 
 /**
  * Create a new instance of a PPS master clock.
- * @param priv	Pointer to the program's data structure.
+ * @param cfg	Pointer to a valid configuration.
  * @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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
-					   const char *dev,
 					   enum ts2phc_master_type type);
 
 /**
@@ -51,6 +49,4 @@ 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,7 +15,6 @@
 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,8 +198,7 @@ static int ts2phc_nmea_master_getppstime(struct ts2phc_master *master,
 	return fix_valid ? lstab_error : -1;
 }
 
-struct ts2phc_master *ts2phc_nmea_master_create(struct ts2phc_private *priv,
+struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg, const char *dev)
-						const char *dev)
 {
 	struct ts2phc_nmea_master *master;
 	const char *leapfile = NULL;	// TODO - read from config.
@@ -215,7 +214,7 @@ struct ts2phc_master *ts2phc_nmea_master_create(struct ts2phc_private *priv,
 	}
 	master->master.destroy = ts2phc_nmea_master_destroy;
 	master->master.getppstime = ts2phc_nmea_master_getppstime;
-	master->config = priv->cfg;
+	master->config = cfg;
 	pthread_mutex_init(&master->mutex, NULL);
 	err = pthread_create(&master->worker, NULL, monitor_nmea_status, master);
 	if (err) {

ts2phc_nmea_master.h

@@ -6,9 +6,8 @@
 #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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_nmea_master_create(struct config *cfg,
 						const char *dev);
 #endif

ts2phc_phc_master.c

@@ -12,14 +12,15 @@
 #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;
-	struct clock *clock;
+	clockid_t clkid;
 	int channel;
+	int fd;
 };
 
 static int ts2phc_phc_master_activate(struct config *cfg, const char *dev,
@@ -27,8 +28,6 @@ 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));
@@ -39,36 +38,21 @@ 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->clock->clkid, &desc)) {
+	if (phc_pin_setfunc(master->clkid, &desc)) {
 		pr_warning("Failed to set the pin. Continuing bravely on...");
 	}
-	if (clock_gettime(master->clock->clkid, &ts)) {
+	if (clock_gettime(master->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.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;
-	}
+	perout_request.period.sec = 1;
+	perout_request.period.nsec = 0;
 
-	if (ioctl(CLOCKID_TO_FD(master->clock->clkid), PTP_PEROUT_REQUEST2,
+	if (ioctl(master->fd, PTP_PEROUT_REQUEST2, &perout_request)) {
-		  &perout_request)) {
 		pr_err(PTP_PEROUT_REQUEST_FAILED);
 		return -1;
 	}
@@ -83,11 +67,10 @@ static void ts2phc_phc_master_destroy(struct ts2phc_master *master)
 
 	memset(&perout_request, 0, sizeof(perout_request));
 	perout_request.index = m->channel;
-	if (ioctl(CLOCKID_TO_FD(m->clock->clkid), PTP_PEROUT_REQUEST2,
+	if (ioctl(m->fd, PTP_PEROUT_REQUEST2, &perout_request)) {
-		  &perout_request)) {
 		pr_err(PTP_PEROUT_REQUEST_FAILED);
 	}
-	clock_destroy(m->clock);
+	posix_clock_close(m->clkid);
 	free(m);
 }
 
@@ -96,21 +79,14 @@ 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->clock->clkid, ts);
+	return clock_gettime(master->clkid, ts);
 }
 
-struct clock *ts2phc_phc_master_get_clock(struct ts2phc_master *m)
+struct ts2phc_master *ts2phc_phc_master_create(struct config *cfg,
-{
-	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) {
@@ -118,19 +94,17 @@ struct ts2phc_master *ts2phc_phc_master_create(struct ts2phc_private *priv,
 	}
 	master->master.destroy = ts2phc_phc_master_destroy;
 	master->master.getppstime = ts2phc_phc_master_getppstime;
-	master->master.get_clock = ts2phc_phc_master_get_clock;
 
-	master->clock = clock_add(priv, dev);
+	master->clkid = posix_clock_open(dev, &junk);
-	if (!master->clock) {
+	if (master->clkid == CLOCK_INVALID) {
 		free(master);
 		return NULL;
 	}
-	master->clock->is_destination = 0;
+	master->fd = CLOCKID_TO_FD(master->clkid);
 
-	pr_debug("PHC master %s has ptp index %d", dev,
+	pr_debug("PHC master %s has ptp index %d", dev, junk);
-		 master->clock->phc_index);
 
-	if (ts2phc_phc_master_activate(priv->cfg, dev, master)) {
+	if (ts2phc_phc_master_activate(cfg, dev, master)) {
 		ts2phc_phc_master_destroy(&master->master);
 		return NULL;
 	}

ts2phc_phc_master.h

@@ -6,10 +6,9 @@
 #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 ts2phc_private *priv,
+struct ts2phc_master *ts2phc_phc_master_create(struct config *cfg,
 					       const char *dev);
 
 #endif

ts2phc_slave.c

@@ -15,30 +15,43 @@
 #include <time.h>
 #include <unistd.h>
 
-#include "clockadj.h"
 #include "config.h"
+#include "clockadj.h"
 #include "missing.h"
 #include "phc.h"
 #include "print.h"
 #include "servo.h"
-#include "ts2phc.h"
+#include "ts2phc_master.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;
-	tmv_t correction;
+	int32_t correction;
 	uint32_t ignore_lower;
 	uint32_t ignore_upper;
-	struct clock *clock;
+	struct servo *servo;
+	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 {
@@ -47,77 +60,61 @@ enum extts_result {
 	EXTTS_IGNORE	= 1,
 };
 
-static int ts2phc_slave_array_create(struct ts2phc_private *priv)
+static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
+					     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;
 
-	polling_array = malloc(sizeof(*polling_array));
+	if (polling_array.slave) {
-	if (!polling_array) {
+		return 0;
+	}
+	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));
-	polling_array->slave = malloc(priv->n_slaves *
+	if (!polling_array.pfd) {
-				      sizeof(*polling_array->slave));
-	if (!polling_array->slave) {
 		pr_err("low memory");
-		return -1;
+		free(polling_array.slave);
-	}
+		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, &priv->slaves, list) {
+	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
-		polling_array->slave[i] = slave;
+		polling_array.slave[i] = slave;
 		i++;
 	}
-	for (i = 0; i < priv->n_slaves; i++) {
+	for (i = 0; i < ts2phc_n_slaves; i++) {
-		struct ts2phc_slave *slave = polling_array->slave[i];
+		polling_array.pfd[i].events = POLLIN | POLLPRI;
-
+		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(struct ts2phc_private *priv)
+static void ts2phc_slave_array_destroy(void)
 {
-	struct ts2phc_slave_array *polling_array = priv->polling_array;
+	free(polling_array.slave);
-
+	free(polling_array.pfd);
-	if (!polling_array)
+	polling_array.slave = NULL;
-		return;
+	polling_array.pfd = NULL;
-
-	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 = CLOCKID_TO_FD(slave->clock->clkid),
+		.fd = slave->fd,
 	};
 	struct ptp_extts_event event;
 	int cnt, size;
@@ -146,13 +143,12 @@ static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave)
 	return 0;
 }
 
-static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
+static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *device)
-						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) {
@@ -165,35 +161,44 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
 		free(slave);
 		return NULL;
 	}
-	slave->pin_desc.index = config_get_int(priv->cfg, device,
+	slave->pin_desc.index = config_get_int(cfg, device, "ts2phc.pin_index");
-					       "ts2phc.pin_index");
 	slave->pin_desc.func = PTP_PF_EXTTS;
-	slave->pin_desc.chan = config_get_int(priv->cfg, device,
+	slave->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel");
-					      "ts2phc.channel");
+	slave->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity");
-	slave->polarity = config_get_int(priv->cfg, device,
+	slave->correction = config_get_int(cfg, device, "ts2phc.extts_correction");
-					 "ts2phc.extts_polarity");
-	correction = config_get_int(priv->cfg, device,
-				    "ts2phc.extts_correction");
-	slave->correction = nanoseconds_to_tmv(correction);
 
-	pulsewidth = config_get_int(priv->cfg, device,
+	pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth");
-				    "ts2phc.pulsewidth");
 	pulsewidth /= 2;
 	slave->ignore_upper = 1000000000 - pulsewidth;
 	slave->ignore_lower = pulsewidth;
 
-	slave->clock = clock_add(priv, device);
+	slave->clk = posix_clock_open(device, &junk);
-	if (!slave->clock) {
+	if (slave->clk == CLOCK_INVALID) {
 		pr_err("failed to open clock");
 		goto no_posix_clock;
 	}
-	slave->clock->is_destination = 1;
+	slave->no_adj = config_get_int(cfg, NULL, "free_running");
+	slave->fd = CLOCKID_TO_FD(slave->clk);
 
-	pr_debug("PHC slave %s has ptp index %d", device,
+	pr_debug("PHC slave %s has ptp index %d", device, junk);
-		 slave->clock->phc_index);
 
-	if (phc_number_pins(slave->clock->clkid) > 0) {
+	fadj = (int) clockadj_get_freq(slave->clk);
-		err = phc_pin_setfunc(slave->clock->clkid, &slave->pin_desc);
+	/* 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(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;
@@ -207,8 +212,7 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
 	memset(&extts, 0, sizeof(extts));
 	extts.index = slave->pin_desc.chan;
 	extts.flags = 0;
-	if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
+	if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
-		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
 	if (ts2phc_slave_clear_fifo(slave)) {
@@ -218,7 +222,9 @@ static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
 	return slave;
 no_ext_ts:
 no_pin_func:
-	clock_destroy(slave->clock);
+	servo_destroy(slave->servo);
+no_servo:
+	posix_clock_close(slave->clk);
 no_posix_clock:
 	free(slave->name);
 	free(slave);
@@ -232,101 +238,136 @@ 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(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
+	if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
-		  &extts)) {
 		pr_err(PTP_EXTTS_REQUEST_FAILED);
 	}
-	clock_destroy(slave->clock);
+	servo_destroy(slave->servo);
+	posix_clock_close(slave->clk);
 	free(slave->name);
 	free(slave);
 }
 
-static bool ts2phc_slave_ignore(struct ts2phc_private *priv,
+static int ts2phc_slave_event(struct ts2phc_slave *slave,
-				struct ts2phc_slave *slave,
+			      struct ts2phc_source_timestamp source_ts)
-				struct timespec source_ts)
 {
-	tmv_t source_tmv = timespec_to_tmv(source_ts);
+	enum extts_result result;
+	uint64_t extts_ts;
+	int64_t offset;
+	double adj;
 
-	source_tmv = tmv_sub(source_tmv, priv->perout_phase);
+	result = ts2phc_slave_offset(slave, source_ts, &offset, &extts_ts);
-	source_ts = tmv_to_timespec(source_tmv);
+	switch (result) {
-
+	case EXTTS_ERROR:
-	return source_ts.tv_nsec > slave->ignore_lower &&
+		return -1;
-	       source_ts.tv_nsec < slave->ignore_upper;
+	case EXTTS_OK:
-}
+		break;
-
+	case EXTTS_IGNORE:
-static enum extts_result ts2phc_slave_event(struct ts2phc_private *priv,
-					    struct ts2phc_slave *slave)
-{
-	enum extts_result result = EXTTS_OK;
-	struct ptp_extts_event event;
-	struct timespec source_ts;
-	int err, cnt;
-	tmv_t ts;
-
-	cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event));
-	if (cnt != sizeof(event)) {
-		pr_err("read extts event failed: %m");
-		result = EXTTS_ERROR;
-		goto out;
-	}
-	if (event.index != slave->pin_desc.chan) {
-		pr_err("extts on unexpected channel");
-		result = EXTTS_ERROR;
-		goto out;
-	}
-
-	err = ts2phc_master_getppstime(priv->master, &source_ts);
-	if (err < 0) {
-		pr_debug("source ts not valid");
 		return 0;
 	}
 
+	if (slave->no_adj) {
+		pr_info("%s master offset %10" PRId64, slave->name, offset);
+		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,
+					     struct ts2phc_source_timestamp src,
+					     int64_t *offset,
+					     uint64_t *local_ts)
+{
+	struct timespec source_ts = src.ts;
+	struct ptp_extts_event event;
+	uint64_t event_ns, source_ns;
+	int cnt;
+
+	cnt = read(slave->fd, &event, sizeof(event));
+	if (cnt != sizeof(event)) {
+		pr_err("read extts event failed: %m");
+		return EXTTS_ERROR;
+	}
+	if (event.index != slave->pin_desc.chan) {
+		pr_err("extts on unexpected channel");
+		return EXTTS_ERROR;
+	}
+	event_ns = event.t.sec * NS_PER_SEC;
+	event_ns += event.t.nsec;
+
 	if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
-	    ts2phc_slave_ignore(priv, slave, source_ts)) {
+	    source_ts.tv_nsec > slave->ignore_lower &&
+	    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);
 
-		result = EXTTS_IGNORE;
+		return 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;
 
-out:
+	pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64
-	if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
+		 ".%ld diff %" PRId64,
-		return result;
+		 slave->name, event.index, event.t.sec, event.t.nsec,
-
+		 slave->correction,
-	ts = pct_to_tmv(event.t);
+		 (int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset);
-	ts = tmv_add(ts, slave->correction);
-	clock_add_tstamp(slave->clock, ts);
 
 	return EXTTS_OK;
 }
 
 /* public methods */
 
-int ts2phc_slave_add(struct ts2phc_private *priv, const char *name)
+int ts2phc_slave_add(struct config *cfg, const char *name)
 {
 	struct ts2phc_slave *slave;
 
 	/* Create each interface only once. */
-	STAILQ_FOREACH(slave, &priv->slaves, list) {
+	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
 		if (0 == strcmp(name, slave->name)) {
 			return 0;
 		}
 	}
-	slave = ts2phc_slave_create(priv, name);
+	slave = ts2phc_slave_create(cfg, name);
 	if (!slave) {
 		pr_err("failed to create slave");
 		return -1;
 	}
-	STAILQ_INSERT_TAIL(&priv->slaves, slave, list);
+	STAILQ_INSERT_TAIL(&ts2phc_slaves, slave, list);
-	priv->n_slaves++;
+	ts2phc_n_slaves++;
 
 	return 0;
 }
 
-int ts2phc_slave_arm(struct ts2phc_private *priv)
+int ts2phc_slave_arm(void)
 {
 	struct ptp_extts_request extts;
 	struct ts2phc_slave *slave;
@@ -334,11 +375,10 @@ int ts2phc_slave_arm(struct ts2phc_private *priv)
 
 	memset(&extts, 0, sizeof(extts));
 
-	STAILQ_FOREACH(slave, &priv->slaves, list) {
+	STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
 		extts.index = slave->pin_desc.chan;
 		extts.flags = slave->polarity | PTP_ENABLE_FEATURE;
-		err = ioctl(CLOCKID_TO_FD(slave->clock->clkid),
+		err = ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts);
-			    PTP_EXTTS_REQUEST2, &extts);
 		if (err < 0) {
 			pr_err(PTP_EXTTS_REQUEST_FAILED);
 			return -1;
@@ -347,45 +387,29 @@ int ts2phc_slave_arm(struct ts2phc_private *priv)
 	return 0;
 }
 
-int ts2phc_slaves_init(struct ts2phc_private *priv)
+void ts2phc_slave_cleanup(void)
-{
-	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(priv);
+	ts2phc_slave_array_destroy();
 
-	while ((slave = STAILQ_FIRST(&priv->slaves))) {
+	while ((slave = STAILQ_FIRST(&ts2phc_slaves))) {
-		STAILQ_REMOVE_HEAD(&priv->slaves, list);
+		STAILQ_REMOVE_HEAD(&ts2phc_slaves, list);
 		ts2phc_slave_destroy(slave);
-		priv->n_slaves--;
+		ts2phc_n_slaves--;
 	}
 }
 
-int ts2phc_slave_poll(struct ts2phc_private *priv)
+int ts2phc_slave_poll(struct ts2phc_master *master)
 {
-	struct ts2phc_slave_array *polling_array = priv->polling_array;
+	struct ts2phc_source_timestamp source_ts;
-	int all_slaves_have_events = 0;
-	int ignore_any = 0;
 	unsigned int i;
-	int cnt;
+	int cnt, err;
 
-	for (i = 0; i < priv->n_slaves; i++)
+	if (ts2phc_slave_array_create()) {
-		polling_array->collected_events[i] = 0;
+		return -1;
-
+	}
-	while (!all_slaves_have_events) {
+	cnt = poll(polling_array.pfd, ts2phc_n_slaves, 2000);
-		struct ts2phc_slave *slave;
-
-		cnt = poll(polling_array->pfd, priv->n_slaves, 2000);
 	if (cnt < 0) {
 		if (EINTR == errno) {
 			return 0;
@@ -398,40 +422,13 @@ int ts2phc_slave_poll(struct ts2phc_private *priv)
 		return 0;
 	}
 
-		for (i = 0; i < priv->n_slaves; i++) {
+	err = ts2phc_master_getppstime(master, &source_ts.ts);
-			if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
+	source_ts.valid = err ? false : true;
-				enum extts_result result;
 
-				slave = polling_array->slave[i];
+	for (i = 0; i < ts2phc_n_slaves; i++) {
-
+		if (polling_array.pfd[i].revents & (POLLIN|POLLPRI)) {
-				result = ts2phc_slave_event(priv, slave);
+			ts2phc_slave_event(polling_array.slave[i], source_ts);
-				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.h"
+#include "ts2phc_master.h"
 
-int ts2phc_slave_add(struct ts2phc_private *priv, const char *name);
+int ts2phc_slave_add(struct config *cfg, const char *name);
 
-int ts2phc_slaves_init(struct ts2phc_private *priv);
+int ts2phc_slave_arm(void);
 
-void ts2phc_slave_cleanup(struct ts2phc_private *priv);
+void ts2phc_slave_cleanup(void);
 
-int ts2phc_slave_poll(struct ts2phc_private *priv);
+int ts2phc_slave_poll(struct ts2phc_master *master);
 
 #endif