From be87b05f523f9d4e293b27580a863827b58f3d1f Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Thu, 30 Aug 2012 14:45:50 +0200 Subject: [PATCH] Revert "Add non-PPS mode to phc2sys." This reverts commit 998093b6f7dbf1fa633ecd61c757da8b52a7ffae. Will apply this patch again, with the correct author info. --- phc2sys.c | 115 +++++++++++++++++++++++------------------------------- 1 file changed, 49 insertions(+), 66 deletions(-) diff --git a/phc2sys.c b/phc2sys.c index 24cc8d6..c9e051c 100644 --- a/phc2sys.c +++ b/phc2sys.c @@ -85,36 +85,33 @@ static void clock_step(clockid_t clkid, int64_t ns) fprintf(stderr, "failed to step clock: %m\n"); } -static int read_phc(clockid_t clkid, clockid_t sysclk, int rdelay, int64_t *offset, uint64_t *ts) +static int64_t read_phc(clockid_t clkid, clockid_t sysclk, int rdelay) { - struct timespec tsrc, tdst; + int64_t offset = 0; if (clkid == CLOCK_INVALID) { return 0; } + if (rdelay) { + struct timespec tsrc, tdst; - if (clock_gettime(clkid, &tsrc)) { - perror("clock_gettime"); - return 0; + 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; } - - if (clock_gettime(sysclk, &tdst)) { - perror("clock_gettime"); - return 0; - } - - *offset = tdst.tv_sec * NS_PER_SEC - tsrc.tv_sec * NS_PER_SEC + - tdst.tv_nsec - tsrc.tv_nsec - rdelay; - *ts = tdst.tv_sec * NS_PER_SEC + tdst.tv_nsec; - - return 1; + return offset; } struct servo { uint64_t last_ts; double drift; enum { - SAMPLE_0, SAMPLE_1, SAMPLE_2, SAMPLE_3, SAMPLE_N + PPS_0, PPS_1, PPS_2, PPS_3, PPS_N } state; }; @@ -122,34 +119,42 @@ static struct servo servo; static void do_servo(struct servo *srv, clockid_t src, clockid_t dst, - int64_t offset, uint64_t ts, double kp, double ki) + uint64_t ts, double kp, double ki, int rdelay) { double ki_term, ppb; - int64_t delta; + int64_t delta, offset, phc; - printf("s%d %lld.%09llu drift %.2f\n", - srv->state, ts / NS_PER_SEC, ts % NS_PER_SEC, srv->drift); + 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 SAMPLE_0: + case PPS_0: clock_ppb(dst, 0.0); - srv->state = SAMPLE_1; + srv->state = PPS_1; break; - case SAMPLE_1: - srv->state = SAMPLE_2; + case PPS_1: + srv->state = PPS_2; break; - case SAMPLE_2: + case PPS_2: delta = ts - srv->last_ts; offset = delta - NS_PER_SEC; srv->drift = offset; clock_ppb(dst, -offset); - srv->state = SAMPLE_3; + srv->state = PPS_3; break; - case SAMPLE_3: + case PPS_3: clock_step(dst, -offset); - srv->state = SAMPLE_N; + srv->state = PPS_N; break; - case SAMPLE_N: + case PPS_N: ki_term = ki * offset; ppb = kp * offset + srv->drift + ki_term; if (ppb < min_ppb) { @@ -166,10 +171,10 @@ static void do_servo(struct servo *srv, srv->last_ts = ts; } -static int read_pps(int fd, int64_t *offset, uint64_t *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; @@ -177,15 +182,9 @@ static int read_pps(int fd, int64_t *offset, uint64_t *ts) perror("ioctl PPS_FETCH"); return 0; } - - *ts = pfd.info.assert_tu.sec * NS_PER_SEC; - *ts += pfd.info.assert_tu.nsec; - - *offset = *ts % NS_PER_SEC; - if (*offset > NS_PER_SEC / 2) - *offset -= NS_PER_SEC; - - return 1; + ts = pfd.info.assert_tu.sec * NS_PER_SEC; + ts += pfd.info.assert_tu.nsec; + return ts; } static void usage(char *progname) @@ -209,9 +208,8 @@ 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 pps_ts, phc_ts; - int64_t pps_offset, phc_offset; - int c, fd = 0, rdelay = 0; + uint64_t ts; + int c, fd, rdelay = 0; /* Process the command line arguments. */ progname = strrchr(argv[0], '/'); @@ -245,16 +243,14 @@ int main(int argc, char *argv[]) } } - if (!(device || src != CLOCK_INVALID) || dst == CLOCK_INVALID) { + if (!device || dst == CLOCK_INVALID) { usage(progname); return -1; } - if (device) { - fd = open(device, O_RDONLY); - if (fd < 0) { - fprintf(stderr, "cannot open %s: %m\n", device); - 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; @@ -264,21 +260,8 @@ int main(int argc, char *argv[]) perror("clock_settime"); } while (1) { - if (fd > 0) { - if (!read_pps(fd, &pps_offset, &pps_ts)) - continue; - printf("pps %9lld ", pps_offset); - } else - usleep(1000000); - - if (!read_phc(src, dst, rdelay, &phc_offset, &phc_ts)) - continue; - printf("phc %9lld ", phc_offset); - - if (fd > 0) - do_servo(&servo, src, dst, pps_offset, pps_ts, kp, ki); - else - do_servo(&servo, src, dst, phc_offset, phc_ts, kp, ki); + ts = read_pps(fd); + do_servo(&servo, src, dst, ts, kp, ki, rdelay); } return 0; }