Add a method to estimate the PHC to system offset using the new ioctl.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>

sysoff.c

@@ -0,0 +1,112 @@
+/**
+ * @file sysoff.c
+ * @brief Implements the system offset estimation method.
+ * @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 <stdio.h>
+#include <sys/ioctl.h>
+#include <linux/ptp_clock.h>
+
+#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 */

sysoff.h

@@ -0,0 +1,44 @@
+/**
+ * @file sysoff.h
+ * @brief Implements the system offset estimation method.
+ * @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 <stdint.h>
+
+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);