[Linuxptp-devel] [PATCH RFC] phc2sys: add PTP_SYS_OFFSET support

[Richard Cochran]

This is just a WIP that shows how to use the new ioctl proposed on netdev.
The current code just sorts the offset readings and picks the one with the
shortest interval. It also prints the existing user land estimate for
the purpose of comparison.

Signed-off-by: Richard Cochran <***@gmail.com>
---
phc2sys.c | 69 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
1 files changed, 67 insertions(+), 2 deletions(-)

diff --git a/phc2sys.c b/phc2sys.c
index 1e406ba..3c9ceb8 100644
--- a/phc2sys.c
+++ b/phc2sys.c
@@ -85,6 +85,11 @@ static void clock_step(clockid_t clkid, int64_t ns)
fprintf(stderr, "failed to step clock: %m\n");
}

+static int64_t pctns(struct ptp_clock_time *t)
+{
+ return t->sec * NS_PER_SEC + t->nsec;
+}
+
static int read_phc(clockid_t clkid, clockid_t sysclk, int readings,
int64_t *offset, uint64_t *ts)
{
@@ -234,13 +239,65 @@ static void usage(char *progname)
progname);
}

+static struct {
+ int64_t interval;
+ int64_t offset;
+} samples[PTP_MAX_SAMPLES];
+
+static void insertion_sort(int length, int64_t interval, int64_t offset)
+{
+ 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;
+}
+
+static int64_t estimate_offset(struct ptp_clock_time *ts, int n_samples)
+{
+ int64_t t1, t2, tp;
+ int64_t interval, offset;
+ int i;
+
+ for (i = 0; i < n_samples; i++) {
+ t1 = pctns(&ts[2*i]);
+ tp = pctns(&ts[2*i+1]);
+ t2 = pctns(&ts[2*i+2]);
+ interval = t2 - t1;
+ offset = (t2 + t1) / 2 - tp;
+ insertion_sort(i, interval, offset);
+ }
+ return samples[0].offset;
+}
+
+static int64_t ptp_sys_offset(clockid_t clk, int n_samples)
+{
+ static int warned;
+ int fd = CLOCKID_TO_FD(clk);
+ struct ptp_sys_offset pso;
+
+ pso.n_samples = n_samples;
+ if (ioctl(fd, PTP_SYS_OFFSET, &pso)) {
+ if (!warned) {
+ perror("ioctl PTP_SYS_OFFSET");
+ warned = 1;
+ }
+ return 0;
+ }
+ return estimate_offset(pso.ts, n_samples);
+}
+
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 phc_ts;
- int64_t phc_offset;
+ int64_t phc_offset, ps_offset;
int c, phc_readings = 5, phc_rate = 1;

/* Process the command line arguments. */
@@ -298,7 +355,15 @@ int main(int argc, char *argv[])
if (!read_phc(src, dst, phc_readings, &phc_offset, &phc_ts)) {
continue;
}
- do_servo(&servo, dst, phc_offset, phc_ts, kp, ki);
+
+ ps_offset = ptp_sys_offset(src, phc_readings);
+
+ printf("ioctl_offset %9lld ", ps_offset);
+
+ do_servo(&servo, dst,
+ ps_offset,
+// phc_offset,
+ phc_ts, kp, ki);
show_servo(stdout, "phc", phc_offset, phc_ts);
}
return 0;