From 31b6a14444a8f23147077df3c6a64518d082c35e Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Thu, 19 Jan 2017 16:05:49 +0100
Subject: [PATCH] sys_linux: add support for PTP_SYS_OFFSET_PRECISE

This is for hardware that can precisely cross timestamp the PHC with the
system clock.
---
 ntp_io_linux.c |  2 +-
 refclock_phc.c |  2 +-
 sys_linux.c    | 40 ++++++++++++++++++++++++++++++++++++++--
 sys_linux.h    |  2 +-
 4 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/ntp_io_linux.c b/ntp_io_linux.c
index 9e5c354..686d175 100644
--- a/ntp_io_linux.c
+++ b/ntp_io_linux.c
@@ -368,7 +368,7 @@ process_hw_timestamp(struct Interface *iface, struct timespec *hw_ts,
   int l2_length;
 
   if (HCL_NeedsNewSample(iface->clock, &local_ts->ts)) {
-    if (!SYS_Linux_GetPHCSample(iface->phc_fd, iface->precision, &iface->phc_mode,
+    if (!SYS_Linux_GetPHCSample(iface->phc_fd, 0, iface->precision, &iface->phc_mode,
                                 &sample_phc_ts, &sample_sys_ts, &err))
       return;
 
diff --git a/refclock_phc.c b/refclock_phc.c
index 3185920..2bbed3d 100644
--- a/refclock_phc.c
+++ b/refclock_phc.c
@@ -83,7 +83,7 @@ static int phc_poll(RCL_Instance instance)
 
   phc = (struct phc_instance *)RCL_GetDriverData(instance);
 
-  if (!SYS_Linux_GetPHCSample(phc->fd, RCL_GetPrecision(instance), &phc->mode,
+  if (!SYS_Linux_GetPHCSample(phc->fd, 0, RCL_GetPrecision(instance), &phc->mode,
                               &phc_ts, &sys_ts, &err))
     return 0;
 
diff --git a/sys_linux.c b/sys_linux.c
index d423362..b121ee8 100644
--- a/sys_linux.c
+++ b/sys_linux.c
@@ -517,6 +517,9 @@ SYS_Linux_EnableSystemCallFilter(int level)
     FIONREAD, TCGETS,
 #if defined(FEAT_PHC) || defined(HAVE_LINUX_TIMESTAMPING)
     PTP_SYS_OFFSET,
+#ifdef PTP_SYS_OFFSET_PRECISE
+    PTP_SYS_OFFSET_PRECISE,
+#endif
 #endif
 #ifdef FEAT_PPS
     PPS_FETCH,
@@ -729,6 +732,35 @@ get_phc_sample(int phc_fd, double precision, struct timespec *phc_ts,
 
   return 1;
 }
+/* ================================================== */
+
+static int
+get_precise_phc_sample(int phc_fd, double precision, struct timespec *phc_ts,
+		       struct timespec *sys_ts, double *err)
+{
+#ifdef PTP_SYS_OFFSET_PRECISE
+  struct ptp_sys_offset_precise sys_off;
+
+  /* Silence valgrind */
+  memset(&sys_off, 0, sizeof (sys_off));
+
+  if (ioctl(phc_fd, PTP_SYS_OFFSET_PRECISE, &sys_off)) {
+    DEBUG_LOG(LOGF_SysLinux, "ioctl(%s) failed : %s", "PTP_SYS_OFFSET_PRECISE",
+              strerror(errno));
+    return 0;
+  }
+
+  phc_ts->tv_sec = sys_off.device.sec;
+  phc_ts->tv_nsec = sys_off.device.nsec;
+  sys_ts->tv_sec = sys_off.sys_realtime.sec;
+  sys_ts->tv_nsec = sys_off.sys_realtime.nsec;
+  *err = MAX(LCL_GetSysPrecisionAsQuantum(), precision);
+
+  return 1;
+#else
+  return 0;
+#endif
+}
 
 /* ================================================== */
 
@@ -766,10 +798,14 @@ SYS_Linux_OpenPHC(const char *path, int phc_index)
 /* ================================================== */
 
 int
-SYS_Linux_GetPHCSample(int fd, double precision, int *reading_mode,
+SYS_Linux_GetPHCSample(int fd, int nocrossts, double precision, int *reading_mode,
                        struct timespec *phc_ts, struct timespec *sys_ts, double *err)
 {
-  if ((*reading_mode == 1 || !*reading_mode) &&
+  if ((*reading_mode == 2 || !*reading_mode) && !nocrossts &&
+      get_precise_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
+    *reading_mode = 2;
+    return 1;
+  } else if ((*reading_mode == 1 || !*reading_mode) &&
       get_phc_sample(fd, precision, phc_ts, sys_ts, err)) {
     *reading_mode = 1;
     return 1;
diff --git a/sys_linux.h b/sys_linux.h
index 5e9f1d1..4108401 100644
--- a/sys_linux.h
+++ b/sys_linux.h
@@ -43,7 +43,7 @@ extern int SYS_Linux_CheckKernelVersion(int req_major, int req_minor);
 
 extern int SYS_Linux_OpenPHC(const char *path, int phc_index);
 
-extern int SYS_Linux_GetPHCSample(int fd, double precision, int *reading_mode,
+extern int SYS_Linux_GetPHCSample(int fd, int nocrossts, double precision, int *reading_mode,
                                   struct timespec *phc_ts, struct timespec *sys_ts, double *err);
 
 #endif  /* GOT_SYS_LINUX_H */