Add a tool to synchronize the system time from a PTP hardware clock.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
master
Richard Cochran 2012-01-24 19:12:57 +01:00
parent 43bd313eb1
commit af3cbb5fbd
2 changed files with 275 additions and 5 deletions

View File

@ -23,21 +23,24 @@ INC = -I$(KBUILD_OUTPUT)/usr/include
CFLAGS = -Wall $(INC) $(DEBUG) CFLAGS = -Wall $(INC) $(DEBUG)
LDFLAGS = LDFLAGS =
LDLIBS = -lm -lrt LDLIBS = -lm -lrt
PRG = ptp4l PRG = ptp4l phc2sys
OBJ = bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \ OBJ = bmc.o clock.o config.o fsm.o ptp4l.o mave.o msg.o phc.o pi.o port.o \
print.o servo.o tmtab.o transport.o udp.o util.o print.o servo.o tmtab.o transport.o udp.o util.o
SRC = $(OBJ:.o=.c) OBJECTS = $(OBJ) phc2sys.o
DEPEND = $(OBJ:.o=.d) SRC = $(OBJECTS:.o=.c)
DEPEND = $(OBJECTS:.o=.d)
srcdir := $(dir $(lastword $(MAKEFILE_LIST))) srcdir := $(dir $(lastword $(MAKEFILE_LIST)))
VPATH = $(srcdir) VPATH = $(srcdir)
all: ptp4l all: ptp4l phc2sys
ptp4l: $(OBJ) ptp4l: $(OBJ)
phc2sys: phc2sys.o
clean: clean:
rm -f $(OBJ) $(DEPEND) rm -f $(OBJECTS) $(DEPEND)
distclean: clean distclean: clean
rm -f $(PRG) rm -f $(PRG)

267
phc2sys.c 100644
View File

@ -0,0 +1,267 @@
/**
* @file phc2sys.c
* @brief Utility program to synchronize two clocks via a PPS.
* @note Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
#include <errno.h>
#include <fcntl.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#include <linux/pps.h>
#include <linux/ptp_clock.h>
#include "missing.h"
#define KP 0.7
#define KI 0.3
#define NS_PER_SEC 1000000000LL
#define max_ppb 512000
#define min_ppb -512000
static clockid_t clock_open(char *device)
{
int fd = open(device, O_RDWR);
if (fd < 0) {
fprintf(stderr, "cannot open %s: %m\n", device);
return CLOCK_INVALID;
}
return FD_TO_CLOCKID(fd);
}
static void clock_ppb(clockid_t clkid, double ppb)
{
struct timex tx;
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_FREQUENCY;
tx.freq = (long) (ppb * 65.536);
if (clock_adjtime(clkid, &tx) < 0)
fprintf(stderr, "failed to adjust the clock: %m\n");
}
static void clock_step(clockid_t clkid, int64_t ns)
{
struct timex tx;
int sign = 1;
if (ns < 0) {
sign = -1;
ns *= -1;
}
memset(&tx, 0, sizeof(tx));
tx.modes = ADJ_SETOFFSET | ADJ_NANO;
tx.time.tv_sec = sign * (ns / NS_PER_SEC);
tx.time.tv_usec = sign * (ns % NS_PER_SEC);
/*
* The value of a timeval is the sum of its fields, but the
* field tv_usec must always be non-negative.
*/
if (tx.time.tv_usec < 0) {
tx.time.tv_sec -= 1;
tx.time.tv_usec += 1000000000;
}
if (clock_adjtime(clkid, &tx) < 0)
fprintf(stderr, "failed to step clock: %m\n");
}
static int64_t read_phc(clockid_t clkid, clockid_t sysclk, int rdelay)
{
int64_t offset = 0;
if (clkid == CLOCK_INVALID) {
return 0;
}
if (rdelay) {
struct timespec tsrc, tdst;
if (clock_gettime(clkid, &tsrc))
perror("clock_gettime");
if (clock_gettime(sysclk, &tdst))
perror("clock_gettime");
offset = tdst.tv_sec * NS_PER_SEC - tsrc.tv_sec * NS_PER_SEC +
tdst.tv_nsec - tsrc.tv_nsec - rdelay;
}
return offset;
}
struct servo {
uint64_t last_ts;
double drift;
enum {
PPS_0, PPS_1, PPS_2, PPS_3, PPS_N
} state;
};
static struct servo servo;
static void do_servo(struct servo *srv,
clockid_t src, clockid_t dst,
uint64_t ts, double kp, double ki, int rdelay)
{
double ki_term, ppb;
int64_t delta, offset, phc;
offset = ts % NS_PER_SEC;
if (offset > NS_PER_SEC / 2) {
offset -= NS_PER_SEC;
}
phc = read_phc(src, dst, rdelay);
printf("s%d %lld.%09llu offset %9lld phc %9lld drift %.2f\n",
srv->state, ts / NS_PER_SEC, ts % NS_PER_SEC,
offset, phc, srv->drift);
switch (srv->state) {
case PPS_0:
clock_ppb(dst, 0.0);
srv->state = PPS_1;
break;
case PPS_1:
srv->state = PPS_2;
break;
case PPS_2:
delta = ts - srv->last_ts;
offset = delta - NS_PER_SEC;
srv->drift = offset;
clock_ppb(dst, -offset);
srv->state = PPS_3;
break;
case PPS_3:
clock_step(dst, -offset);
srv->state = PPS_N;
break;
case PPS_N:
ki_term = ki * offset;
ppb = kp * offset + srv->drift + ki_term;
if (ppb < min_ppb) {
ppb = min_ppb;
} else if (ppb > max_ppb) {
ppb = max_ppb;
} else {
srv->drift += ki_term;
}
clock_ppb(dst, -ppb);
break;
}
srv->last_ts = ts;
}
static uint64_t read_pps(int fd)
{
struct pps_fdata pfd;
uint64_t ts;
pfd.timeout.sec = 10;
pfd.timeout.nsec = 0;
pfd.timeout.flags = ~PPS_TIME_INVALID;
if (ioctl(fd, PPS_FETCH, &pfd)) {
perror("ioctl PPS_FETCH");
return 0;
}
ts = pfd.info.assert_tu.sec * NS_PER_SEC;
ts += pfd.info.assert_tu.nsec;
return ts;
}
static void usage(char *progname)
{
fprintf(stderr,
"\n"
"usage: %s [options]\n\n"
" -c [device] slave clock device, default CLOCK_REALTIME\n"
" -d [device] master device, source of PPS events\n"
" -h prints this message and exits\n"
" -r [val] reading the PHC device takes 'val' nanoseconds\n"
" -s [device] set the time from this PHC device\n"
" -P [val] set proportional constant to 'val'\n"
" -I [val] set integration constant to 'val'\n"
"\n",
progname);
}
int main(int argc, char *argv[])
{
double kp = KP, ki = KI;
char *device = NULL, *progname;
clockid_t src = CLOCK_INVALID, dst = CLOCK_REALTIME;
uint64_t ts;
int c, fd, rdelay = 0;
/* Process the command line arguments. */
progname = strrchr(argv[0], '/');
progname = progname ? 1+progname : argv[0];
while (EOF != (c = getopt(argc, argv, "c:d:hr:s:P:I:"))) {
switch (c) {
case 'c':
dst = clock_open(optarg);
break;
case 'd':
device = optarg;
break;
case 'r':
rdelay = atoi(optarg);
break;
case 's':
src = clock_open(optarg);
break;
case 'P':
kp = atof(optarg);
break;
case 'I':
ki = atof(optarg);
break;
case 'h':
usage(progname);
return 0;
default:
usage(progname);
return -1;
}
}
if (!device || dst == CLOCK_INVALID) {
usage(progname);
return -1;
}
fd = open(device, O_RDONLY);
if (fd < 0) {
fprintf(stderr, "cannot open %s: %m\n", device);
return -1;
}
if (src != CLOCK_INVALID) {
struct timespec now;
if (clock_gettime(src, &now))
perror("clock_gettime");
if (clock_settime(dst, &now))
perror("clock_settime");
}
while (1) {
ts = read_pps(fd);
do_servo(&servo, src, dst, ts, kp, ki, rdelay);
}
return 0;
}