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>
master
Miroslav Lichvar 2013-01-29 18:06:18 +01:00 committed by Richard Cochran
parent cbc1a70c2c
commit 74692bb1b0
3 changed files with 165 additions and 4 deletions

View File

@ -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

View File

@ -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.

152
phc2sys.c
View File

@ -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. */