From 36a5224b5a23c2d05889287c5cdf5231ef727ba5 Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Mon, 26 Nov 2012 13:22:07 +0100 Subject: [PATCH] Add a method to estimate the PHC to system offset using the new ioctl. Signed-off-by: Richard Cochran --- sysoff.c | 112 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ sysoff.h | 44 ++++++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 sysoff.c create mode 100644 sysoff.h diff --git a/sysoff.c b/sysoff.c new file mode 100644 index 0000000..37a9664 --- /dev/null +++ b/sysoff.c @@ -0,0 +1,112 @@ +/** + * @file sysoff.c + * @brief Implements the system offset estimation method. + * @note Copyright (C) 2012 Richard Cochran + * + * 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 +#include +#include + +#include "sysoff.h" + +#define NS_PER_SEC 1000000000LL + +#ifdef PTP_SYS_OFFSET + +static int64_t pctns(struct ptp_clock_time *t) +{ + return t->sec * NS_PER_SEC + t->nsec; +} + +static struct { + int64_t interval; + int64_t offset; + uint64_t timestamp; +} samples[PTP_MAX_SAMPLES]; + +static void insertion_sort(int length, int64_t interval, int64_t offset, uint64_t ts) +{ + int i = length - 1; + while (i >= 0) { + if (samples[i].interval < interval) + break; + samples[i+1] = samples[i]; + i--; + } + samples[i+1].interval = interval; + samples[i+1].offset = offset; + samples[i+1].timestamp = ts; +} + +static int64_t sysoff_estimate(struct ptp_clock_time *pct, int n_samples, uint64_t *ts) +{ + int64_t t1, t2, tp; + int64_t interval, offset; + int i; + + for (i = 0; i < n_samples; i++) { + t1 = pctns(&pct[2*i]); + tp = pctns(&pct[2*i+1]); + t2 = pctns(&pct[2*i+2]); + interval = t2 - t1; + offset = (t2 + t1) / 2 - tp; + insertion_sort(i, interval, offset, (t2 + t1) / 2); + } + *ts = samples[0].timestamp; + return samples[0].offset; +} + +int sysoff_measure(int fd, int n_samples, int64_t *result, uint64_t *ts) +{ + struct ptp_sys_offset pso; + pso.n_samples = n_samples; + if (ioctl(fd, PTP_SYS_OFFSET, &pso)) { + perror("ioctl PTP_SYS_OFFSET"); + return SYSOFF_RUN_TIME_MISSING; + } + *result = sysoff_estimate(pso.ts, n_samples, ts); + return SYSOFF_SUPPORTED; +} + +int sysoff_probe(int fd, int n_samples) +{ + int64_t junk; + uint64_t ts; + + if (n_samples > PTP_MAX_SAMPLES) { + fprintf(stderr, "warning: %d exceeds kernel max readings %d\n", + n_samples, PTP_MAX_SAMPLES); + fprintf(stderr, "falling back to clock_gettime method\n"); + return SYSOFF_RUN_TIME_MISSING; + } + + return sysoff_measure(fd, n_samples, &junk, &ts); +} + +#else /* !PTP_SYS_OFFSET */ + +int sysoff_measure(int fd, int n_samples, int64_t *result, uint64_t *ts) +{ + return SYSOFF_COMPILE_TIME_MISSING; +} + +int sysoff_probe(int fd, int n_samples) +{ + return SYSOFF_COMPILE_TIME_MISSING; +} + +#endif /* PTP_SYS_OFFSET */ diff --git a/sysoff.h b/sysoff.h new file mode 100644 index 0000000..f2e481e --- /dev/null +++ b/sysoff.h @@ -0,0 +1,44 @@ +/** + * @file sysoff.h + * @brief Implements the system offset estimation method. + * @note Copyright (C) 2012 Richard Cochran + * + * 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 + +enum { + SYSOFF_SUPPORTED, + SYSOFF_COMPILE_TIME_MISSING, + SYSOFF_RUN_TIME_MISSING, +}; + +/** + * Check to see if the PTP_SYS_OFFSET ioctl is supported. + * @param fd An open file descriptor to a PHC device. + * @return One of the SYSOFF_ enumeration values. + */ +int sysoff_probe(int fd, int n_samples); + +/** + * Measure the offset between a PHC and the system time. + * @param fd An open file descriptor to a PHC device. + * @param n_samples The number of consecutive readings to make. + * @param result The estimated offset in nanoseconds. + * @param ts The system time corresponding to the 'result'. + * @return One of the SYSOFF_ enumeration values. + */ +int sysoff_measure(int fd, int n_samples, int64_t *result, uint64_t *ts);