phc2sys: include PTP management client.

Add a new option to wait for ptp4l to be in a synchronized state.
Periodically check PORT_DATA_SET and wait until there is a port in
SLAVE, MASTER or GRAND_MASTER state. Also, set the default
synchronization offset according to the currentUtcOffset value from
TIME_PROPERTIES_DATA_SET and the direction of the clock synchronization.

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

makefile

@@ -52,7 +52,8 @@ ptp4l: $(OBJ)
 pmc: msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o transport.o udp.o \
  udp6.o uds.o util.o version.o
 
-phc2sys: phc2sys.o pi.o servo.o sk.o sysoff.o print.o version.o
+phc2sys: msg.o phc2sys.o pmc_common.o print.o pi.o servo.o raw.o sk.o sysoff.o \
+ tlv.o transport.o udp.o udp6.o uds.o util.o version.o
 
 hwstamp_ctl: hwstamp_ctl.o version.o
 

phc2sys.8

@@ -27,6 +27,8 @@ phc2sys \- synchronize two clocks
 ] [
 .BI \-O " offset"
 ] [
+.B \-w
+] [
 .B \-v
 ]
 
@@ -90,8 +92,16 @@ 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.
-The default is 0 seconds.
+Specify the offset between the slave and master times in seconds. With the
+.B \-w
+option the default value is set automatically according to the currentUtcOffset
+value obtained from ptp4l and the direction of the clock synchronization.
+Without
+.B \-w
+the default is 0.
+.TP
+.B \-w
+Wait until ptp4l is in a synchronized state.
 .TP
 .BI \-h
 Display a help message.

phc2sys.c

@@ -19,6 +19,7 @@
  */
 #include <errno.h>
 #include <fcntl.h>
+#include <poll.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -33,11 +34,16 @@
 #include <linux/pps.h>
 #include <linux/ptp_clock.h>
 
+#include "ds.h"
+#include "fsm.h"
 #include "missing.h"
 #include "pi.h"
+#include "pmc_common.h"
+#include "print.h"
 #include "servo.h"
 #include "sk.h"
 #include "sysoff.h"
+#include "tlv.h"
 #include "version.h"
 
 #define KP 0.7
@@ -285,6 +291,125 @@ static int do_phc_loop(struct clock *clock, clockid_t src,
 	return 0;
 }
 
+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 != 1)
+		return 0;
+	tlv = (struct TLV *) msg->management.suffix;
+	if (tlv->type != TLV_MANAGEMENT)
+		return 0;
+	return 1;
+}
+
+static int get_mgt_id(struct ptp_message *msg)
+{
+	return ((struct management_tlv *) msg->management.suffix)->id;
+}
+
+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)
+{
+	struct ptp_message *msg;
+	struct pmc *pmc;
+	void *data;
+#define N_FD 1
+	struct pollfd pollfd[N_FD];
+#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, "/tmp/phc2sys", 0, 0, 0);
+	if (!pmc) {
+		fprintf(stderr, "failed to create pmc\n");
+		return -1;
+	}
+
+	while (i < N_ID) {
+		pollfd[0].fd = pmc_get_transport_fd(pmc);
+		pollfd[0].events = POLLIN|POLLPRI;
+		if (!ds_requested)
+			pollfd[0].events |= POLLOUT;
+
+		cnt = poll(pollfd, N_FD, 1000);
+		if (cnt < 0) {
+			fprintf(stderr, "poll failed\n");
+			return -1;
+		}
+		if (!cnt) {
+			/* Request the data set again. */
+			ds_requested = 0;
+			fprintf(stderr, "Waiting for ptp4l...\n");
+			continue;
+		}
+
+		if (pollfd[0].revents & POLLOUT) {
+			pmc_send_get_action(pmc, ds_ids[i]);
+			ds_requested = 1;
+		}
+
+		if (!(pollfd[0].revents & (POLLIN|POLLPRI)))
+			continue;
+
+		msg = pmc_recv(pmc);
+
+		if (!msg)
+			continue;
+
+		if (!is_msg_mgt(msg) || get_mgt_id(msg) != ds_ids[i]) {
+			msg_put(msg);
+			continue;
+		}
+
+		data = get_mgt_data(msg);
+		ds_done = 0;
+
+		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_GRAND_MASTER:
+			case PS_SLAVE:
+				ds_done = 1;
+				break;
+			}
+
+			break;
+		case TIME_PROPERTIES_DATA_SET:
+			*utc_offset = ((struct timePropertiesDS *)data)->
+					currentUtcOffset;
+			ds_done = 1;
+			break;
+		}
+
+		if (ds_done) {
+			/* Proceed with the next data set. */
+			i++;
+			ds_requested = 0;
+		}
+		msg_put(msg);
+	}
+
+	pmc_destroy(pmc);
+
+	return 0;
+}
+
 static void usage(char *progname)
 {
 	fprintf(stderr,
@@ -299,6 +424,7 @@ static void usage(char *progname)
 		" -R [rate]      slave clock update rate in HZ (1)\n"
 		" -N [num]       number of master clock readings per update (5)\n"
 		" -O [offset]    slave-master time offset (0)\n"
+		" -w             wait for ptp4l\n"
 		" -h             prints this message and exits\n"
 		" -v             prints the software version and exits\n"
 		"\n",
@@ -310,6 +436,7 @@ int main(int argc, char *argv[])
 	char *device = NULL, *progname, *ethdev = NULL;
 	clockid_t src = CLOCK_INVALID;
 	int c, phc_readings = 5, phc_rate = 1, sync_offset = 0;
+	int wait_sync = 0, forced_sync_offset = 0;
 	double ppb;
 	struct clock dst_clock = {
 		.clkid = CLOCK_REALTIME,
@@ -322,7 +449,7 @@ 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(argc, argv, "c:d:hs:P:I:R:N:O:i:v"))) {
+	while (EOF != (c = getopt(argc, argv, "c:d:hs:P:I:R:N:O:i:wv"))) {
 		switch (c) {
 		case 'c':
 			dst_clock.clkid = clock_open(optarg);
@@ -347,10 +474,14 @@ int main(int argc, char *argv[])
 			break;
 		case 'O':
 			sync_offset = atoi(optarg);
+			forced_sync_offset = 1;
 			break;
 		case 'i':
 			ethdev = optarg;
 			break;
+		case 'w':
+			wait_sync = 1;
+			break;
 		case 'v':
 			version_show(stdout);
 			return 0;
@@ -384,6 +515,25 @@ int main(int argc, char *argv[])
 		return -1;
 	}
 
+	print_set_progname(progname);
+	print_set_verbose(1);
+	print_set_syslog(0);
+
+	if (wait_sync) {
+		int ptp_utc_offset;
+
+		run_pmc(wait_sync, &ptp_utc_offset);
+
+		if (!forced_sync_offset) {
+			if (src != CLOCK_REALTIME &&
+			    dst_clock.clkid == CLOCK_REALTIME)
+				sync_offset = -ptp_utc_offset;
+			else if (src == CLOCK_REALTIME &&
+			    dst_clock.clkid != CLOCK_REALTIME)
+				sync_offset = ptp_utc_offset;
+		}
+	}
+
 	ppb = clock_ppb_read(dst_clock.clkid);
 	/* The reading may silently fail and return 0, reset the frequency to
 	   make sure ppb is the actual frequency of the clock. */