phc2sys: Update sync offset periodically with -w.

Modify the pmc to allow non-blocking operation. Run it on each clock
update to have the sync offset updated from currentUtcOffset with every
other call.

Signed-off-by: Miroslav Lichvar <mlichvar@redhat.com>

phc2sys.8

@@ -106,13 +106,10 @@ minimize the error caused by random delays in scheduling and bus utilization.
 The default is 5.
 .TP
 .BI \-O " offset"
-Specify the offset between the slave and master times in seconds. With the
+Specify the offset between the slave and master times in seconds. The default
+is set automatically with the
 .B \-w
-option the default value is set automatically according to the currentUtcOffset
+option, 0 otherwise.
-value obtained from ptp4l and the direction of the clock synchronization.
-Without
-.B \-w
-the default is 0.
 .TP
 .BI \-u " summary-updates"
 Specify the number of clock updates included in summary statistics. The
@@ -124,7 +121,11 @@ statistics. The messages are printed at the LOG_INFO level.
 The default is 0 (disabled).
 .TP
 .B \-w
-Wait until ptp4l is in a synchronized state.
+Wait until ptp4l is in a synchronized state. If the
+.B \-O
+option is not used, also keep the offset between the slave and master
+times updated according to the currentUtcOffset value obtained from ptp4l and
+the direction of the clock synchronization.
 .TP
 .BI \-l " print-level"
 Set the maximum syslog level of messages which should be printed or sent to

phc2sys.c

@@ -55,6 +55,9 @@
 
 #define PHC_PPS_OFFSET_LIMIT 10000000
 
+struct clock;
+static int update_sync_offset(struct clock *clock);
+
 static clockid_t clock_open(char *device)
 {
 	int fd;
@@ -160,6 +163,11 @@ struct clock {
 	struct stats *freq_stats;
 	struct stats *delay_stats;
 	unsigned int stats_max_count;
+	int sync_offset;
+	int sync_offset_direction;
+	struct pmc *pmc;
+	int pmc_ds_idx;
+	int pmc_ds_requested;
 };
 
 static void update_clock_stats(struct clock *clock,
@@ -203,6 +211,13 @@ static void update_clock(struct clock *clock,
 	enum servo_state state;
 	double ppb;
 
+	if (update_sync_offset(clock))
+		return;
+
+	if (clock->sync_offset_direction)
+		offset += clock->sync_offset * NS_PER_SEC *
+			clock->sync_offset_direction;
+
 	ppb = servo_sample(clock->servo, offset, ts, &state);
 
 	switch (state) {
@@ -253,13 +268,17 @@ static int read_pps(int fd, int64_t *offset, uint64_t *ts)
 }
 
 static int do_pps_loop(struct clock *clock, int fd,
-		       clockid_t src, int n_readings, int sync_offset)
+		       clockid_t src, int n_readings)
 {
 	int64_t pps_offset, phc_offset, phc_delay;
 	uint64_t pps_ts, phc_ts;
 
 	clock->source_label = "pps";
 
+	/* The sync offset can't be applied with PPS alone. */
+	if (src == CLOCK_INVALID)
+		clock->sync_offset_direction = 0;
+
 	while (1) {
 		if (!read_pps(fd, &pps_offset, &pps_ts)) {
 			continue;
@@ -284,7 +303,6 @@ static int do_pps_loop(struct clock *clock, int fd,
 
 			phc_ts = phc_ts / NS_PER_SEC * NS_PER_SEC;
 			pps_offset = pps_ts - phc_ts;
-			pps_offset -= sync_offset * NS_PER_SEC;
 		}
 
 		update_clock(clock, pps_offset, pps_ts, -1);
@@ -294,7 +312,7 @@ static int do_pps_loop(struct clock *clock, int fd,
 }
 
 static int do_sysoff_loop(struct clock *clock, clockid_t src,
-			  int rate, int n_readings, int sync_offset)
+			  int rate, int n_readings)
 {
 	uint64_t ts;
 	int64_t offset, delay;
@@ -308,14 +326,13 @@ static int do_sysoff_loop(struct clock *clock, clockid_t src,
 			err = -1;
 			break;
 		}
-		offset -= sync_offset * NS_PER_SEC;
 		update_clock(clock, offset, ts, delay);
 	}
 	return err;
 }
 
 static int do_phc_loop(struct clock *clock, clockid_t src,
-		       int rate, int n_readings, int sync_offset)
+		       int rate, int n_readings)
 {
 	uint64_t ts;
 	int64_t offset, delay;
@@ -328,7 +345,6 @@ static int do_phc_loop(struct clock *clock, clockid_t src,
 			      &offset, &ts, &delay)) {
 			continue;
 		}
-		offset -= sync_offset * NS_PER_SEC;
 		update_clock(clock, offset, ts, delay);
 	}
 	return 0;
@@ -360,58 +376,75 @@ static void *get_mgt_data(struct ptp_message *msg)
 	return ((struct management_tlv *) msg->management.suffix)->data;
 }
 
-static int run_pmc(int wait_sync, int *utc_offset)
+static int init_pmc(struct clock *clock)
+{
+	clock->pmc = pmc_create(TRANS_UDS, "/var/run/phc2sys", 0, 0, 0);
+	if (!clock->pmc) {
+		pr_err("failed to create pmc");
+		return -1;
+	}
+
+	return 0;
+}
+
+static int run_pmc(struct clock *clock, int timeout,
+		   int wait_sync, int get_utc_offset)
 {
 	struct ptp_message *msg;
-	struct pmc *pmc;
 	void *data;
 #define N_FD 1
 	struct pollfd pollfd[N_FD];
+	int cnt, ds_done;
 #define N_ID 2
-	int cnt, i = 0, ds_done, ds_requested = 0;
 	int ds_ids[N_ID] = {
 		PORT_DATA_SET,
 		TIME_PROPERTIES_DATA_SET
 	};
 
-	pmc = pmc_create(TRANS_UDS, "/var/run/phc2sys", 0, 0, 0);
+	while (clock->pmc_ds_idx < N_ID) {
-	if (!pmc) {
+		/* Check if the data set is really needed. */
-		pr_err("failed to create pmc");
+		if ((ds_ids[clock->pmc_ds_idx] == PORT_DATA_SET &&
-		return -1;
+		     !wait_sync) ||
-	}
+		    (ds_ids[clock->pmc_ds_idx] == TIME_PROPERTIES_DATA_SET &&
+		     !get_utc_offset)) {
+			clock->pmc_ds_idx++;
+			continue;
+		}
 
-	while (i < N_ID) {
+		pollfd[0].fd = pmc_get_transport_fd(clock->pmc);
-		pollfd[0].fd = pmc_get_transport_fd(pmc);
 		pollfd[0].events = POLLIN|POLLPRI;
-		if (!ds_requested)
+		if (!clock->pmc_ds_requested)
 			pollfd[0].events |= POLLOUT;
 
-		cnt = poll(pollfd, N_FD, 1000);
+		cnt = poll(pollfd, N_FD, timeout);
 		if (cnt < 0) {
 			pr_err("poll failed");
 			return -1;
 		}
 		if (!cnt) {
-			/* Request the data set again. */
+			/* Request the data set again in the next run. */
-			ds_requested = 0;
+			clock->pmc_ds_requested = 0;
-			pr_notice("Waiting for ptp4l...");
+			return 0;
-			continue;
 		}
 
-		if (pollfd[0].revents & POLLOUT) {
+		/* Send a new request if there are no pending messages. */
-			pmc_send_get_action(pmc, ds_ids[i]);
+		if ((pollfd[0].revents & POLLOUT) &&
-			ds_requested = 1;
+		    !(pollfd[0].revents & (POLLIN|POLLPRI))) {
+			pmc_send_get_action(clock->pmc,
+					    ds_ids[clock->pmc_ds_idx]);
+			clock->pmc_ds_requested = 1;
 		}
 
 		if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
 			continue;
 
-		msg = pmc_recv(pmc);
+		msg = pmc_recv(clock->pmc);
 
 		if (!msg)
 			continue;
 
-		if (!is_msg_mgt(msg) || get_mgt_id(msg) != ds_ids[i]) {
+		if (!is_msg_mgt(msg) ||
+		    get_mgt_id(msg) != ds_ids[clock->pmc_ds_idx]) {
 			msg_put(msg);
 			continue;
 		}
@@ -421,9 +454,6 @@ static int run_pmc(int wait_sync, int *utc_offset)
 
 		switch (get_mgt_id(msg)) {
 		case PORT_DATA_SET:
-			if (!wait_sync)
-				ds_done = 1;
-
 			switch (((struct portDS *)data)->portState) {
 			case PS_MASTER:
 			case PS_SLAVE:
@@ -433,22 +463,35 @@ static int run_pmc(int wait_sync, int *utc_offset)
 
 			break;
 		case TIME_PROPERTIES_DATA_SET:
-			*utc_offset = ((struct timePropertiesDS *)data)->
+			clock->sync_offset = ((struct timePropertiesDS *)data)->
-					currentUtcOffset;
+				currentUtcOffset;
 			ds_done = 1;
 			break;
 		}
 
 		if (ds_done) {
 			/* Proceed with the next data set. */
-			i++;
+			clock->pmc_ds_idx++;
-			ds_requested = 0;
+			clock->pmc_ds_requested = 0;
 		}
 		msg_put(msg);
 	}
 
-	pmc_destroy(pmc);
+	clock->pmc_ds_idx = 0;
+	return 1;
+}
 
+static void close_pmc(struct clock *clock)
+{
+	pmc_destroy(clock->pmc);
+	clock->pmc = NULL;
+}
+
+static int update_sync_offset(struct clock *clock)
+{
+	if (clock->pmc) {
+		run_pmc(clock, 0, 0, 1);
+	}
 	return 0;
 }
 
@@ -479,8 +522,8 @@ int main(int argc, char *argv[])
 {
 	char *progname, *ethdev = NULL;
 	clockid_t src = CLOCK_INVALID;
-	int c, phc_readings = 5, phc_rate = 1, sync_offset = 0, pps_fd = -1;
+	int c, phc_readings = 5, phc_rate = 1, pps_fd = -1;
-	int wait_sync = 0, forced_sync_offset = 0;
+	int r, wait_sync = 0, forced_sync_offset = 0;
 	int print_level = LOG_INFO, use_syslog = 1, verbose = 0;
 	double ppb;
 	struct clock dst_clock = { .clkid = CLOCK_REALTIME };
@@ -524,7 +567,8 @@ int main(int argc, char *argv[])
 			phc_readings = atoi(optarg);
 			break;
 		case 'O':
-			sync_offset = atoi(optarg);
+			dst_clock.sync_offset = atoi(optarg);
+			dst_clock.sync_offset_direction = -1;
 			forced_sync_offset = 1;
 			break;
 		case 'i':
@@ -596,18 +640,33 @@ int main(int argc, char *argv[])
 	print_set_level(print_level);
 
 	if (wait_sync) {
-		int ptp_utc_offset;
+		if (init_pmc(&dst_clock))
+			return -1;
 
-		run_pmc(wait_sync, &ptp_utc_offset);
+		while (1) {
+			r = run_pmc(&dst_clock, 1000,
+				    wait_sync, !forced_sync_offset);
+			if (r < 0)
+				return -1;
+			else if (r > 0)
+				break;
+			else
+				pr_notice("Waiting for ptp4l...");
+		}
 
 		if (!forced_sync_offset) {
 			if (src != CLOCK_REALTIME &&
 			    dst_clock.clkid == CLOCK_REALTIME)
-				sync_offset = -ptp_utc_offset;
+				dst_clock.sync_offset_direction = 1;
 			else if (src == CLOCK_REALTIME &&
 			    dst_clock.clkid != CLOCK_REALTIME)
-				sync_offset = ptp_utc_offset;
+				dst_clock.sync_offset_direction = -1;
+			else
+				dst_clock.sync_offset_direction = 0;
 		}
+
+		if (forced_sync_offset || !dst_clock.sync_offset_direction)
+			close_pmc(&dst_clock);
 	}
 
 	ppb = clock_ppb_read(dst_clock.clkid);
@@ -618,14 +677,11 @@ int main(int argc, char *argv[])
 	dst_clock.servo = servo_create(CLOCK_SERVO_PI, -ppb, max_ppb, 0);
 
 	if (pps_fd >= 0)
-		return do_pps_loop(&dst_clock, pps_fd, src,
+		return do_pps_loop(&dst_clock, pps_fd, src, phc_readings);
-				   phc_readings, sync_offset);
 
 	if (dst_clock.clkid == CLOCK_REALTIME &&
 	    SYSOFF_SUPPORTED == sysoff_probe(CLOCKID_TO_FD(src), phc_readings))
-		return do_sysoff_loop(&dst_clock, src, phc_rate,
+		return do_sysoff_loop(&dst_clock, src, phc_rate, phc_readings);
-				      phc_readings, sync_offset);
 
-	return do_phc_loop(&dst_clock, src, phc_rate,
+	return do_phc_loop(&dst_clock, src, phc_rate, phc_readings);
-			   phc_readings, sync_offset);
 }