diff --git a/makefile b/makefile index 2fa6a09..9f2b587 100644 --- a/makefile +++ b/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 diff --git a/phc2sys.8 b/phc2sys.8 index a5051c8..e26641a 100644 --- a/phc2sys.8 +++ b/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. diff --git a/phc2sys.c b/phc2sys.c index 988a3f0..17d00db 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -19,6 +19,7 @@ */ #include #include +#include #include #include #include @@ -33,11 +34,16 @@ #include #include +#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. */