From eceb8d99371a129ad6e11f483a0442d97a895b19 Mon Sep 17 00:00:00 2001
From: Miroslav Lichvar <mlichvar@redhat.com>
Date: Fri, 5 May 2017 16:07:34 +0200
Subject: [PATCH] refclock_phc: add support for timestamping of external PPS

Add extpps driver option to the PHC refclock to enable external
timestamping of PPS signal and also options to configure the channel and
pin index. In this mode, the driver polling function accumulates samples
for hwclock, which is used to convert received timestamping events to
local time.
---
 refclock.c     |  6 ++++
 refclock.h     |  1 +
 refclock_phc.c | 79 ++++++++++++++++++++++++++++++++++++++++++++++----
 3 files changed, 80 insertions(+), 6 deletions(-)

diff --git a/refclock.c b/refclock.c
index 706e2ba..0cdc061 100644
--- a/refclock.c
+++ b/refclock.c
@@ -522,6 +522,12 @@ RCL_GetPrecision(RCL_Instance instance)
   return instance->precision;
 }
 
+int
+RCL_GetDriverPoll(RCL_Instance instance)
+{
+  return instance->driver_poll;
+}
+
 static int
 valid_sample_time(RCL_Instance instance, struct timespec *sample_time)
 {
diff --git a/refclock.h b/refclock.h
index a4201c2..0eb4caa 100644
--- a/refclock.h
+++ b/refclock.h
@@ -75,5 +75,6 @@ extern int RCL_AddPulse(RCL_Instance instance, struct timespec *pulse_time, doub
 extern int RCL_AddCookedPulse(RCL_Instance instance, struct timespec *cooked_time,
                               double second, double dispersion, double raw_correction);
 extern double RCL_GetPrecision(RCL_Instance instance);
+extern int RCL_GetDriverPoll(RCL_Instance instance);
 
 #endif
diff --git a/refclock_phc.c b/refclock_phc.c
index 2621d4a..6aa5edd 100644
--- a/refclock_phc.c
+++ b/refclock_phc.c
@@ -34,22 +34,31 @@
 #include "sysincl.h"
 
 #include "refclock.h"
+#include "hwclock.h"
+#include "local.h"
 #include "logging.h"
 #include "memory.h"
 #include "util.h"
+#include "sched.h"
 #include "sys_linux.h"
 
 struct phc_instance {
   int fd;
   int mode;
   int nocrossts;
+  int extpps;
+  int pin;
+  int channel;
+  HCL_Instance clock;
 };
 
+static void read_ext_pulse(int sockfd, int event, void *anything);
+
 static int phc_initialise(RCL_Instance instance)
 {
   struct phc_instance *phc;
-  int phc_fd;
-  char *path;
+  int phc_fd, rising_edge;
+  char *path, *s;
 
   path = RCL_GetDriverParameter(instance);
  
@@ -63,6 +72,25 @@ static int phc_initialise(RCL_Instance instance)
   phc->fd = phc_fd;
   phc->mode = 0;
   phc->nocrossts = RCL_GetDriverOption(instance, "nocrossts") ? 1 : 0;
+  phc->extpps = RCL_GetDriverOption(instance, "extpps") ? 1 : 0;
+
+  if (phc->extpps) {
+    s = RCL_GetDriverOption(instance, "pin");
+    phc->pin = s ? atoi(s) : 0;
+    s = RCL_GetDriverOption(instance, "channel");
+    phc->channel = s ? atoi(s) : 0;
+    rising_edge = RCL_GetDriverOption(instance, "clear") ? 0 : 1;
+    phc->clock = HCL_CreateInstance(UTI_Log2ToDouble(RCL_GetDriverPoll(instance)));
+
+    if (!SYS_Linux_SetPHCExtTimestamping(phc->fd, phc->pin, phc->channel,
+                                         rising_edge, !rising_edge, 1))
+      LOG_FATAL("Could not enable external PHC timestamping");
+
+    SCH_AddFileHandler(phc->fd, SCH_FILE_INPUT, read_ext_pulse, instance);
+  } else {
+    phc->pin = phc->channel = 0;
+    phc->clock = NULL;
+  }
 
   RCL_SetDriverData(instance, phc);
   return 1;
@@ -73,25 +101,64 @@ static void phc_finalise(RCL_Instance instance)
   struct phc_instance *phc;
 
   phc = (struct phc_instance *)RCL_GetDriverData(instance);
+
+  if (phc->extpps) {
+    SCH_RemoveFileHandler(phc->fd);
+    SYS_Linux_SetPHCExtTimestamping(phc->fd, phc->pin, phc->channel, 0, 0, 0);
+    HCL_DestroyInstance(phc->clock);
+  }
+
   close(phc->fd);
   Free(phc);
 }
 
+static void read_ext_pulse(int fd, int event, void *anything)
+{
+  RCL_Instance instance;
+  struct phc_instance *phc;
+  struct timespec phc_ts, local_ts;
+  double local_err;
+  int channel;
+
+  instance = anything;
+  phc = RCL_GetDriverData(instance);
+
+  if (!SYS_Linux_ReadPHCExtTimestamp(phc->fd, &phc_ts, &channel))
+    return;
+
+  if (channel != phc->channel) {
+    DEBUG_LOG("Unexpected extts channel %d\n", channel);
+    return;
+  }
+
+  if (!HCL_CookTime(phc->clock, &phc_ts, &local_ts, &local_err))
+    return;
+
+  RCL_AddCookedPulse(instance, &local_ts, 1.0e-9 * local_ts.tv_nsec, local_err,
+                     UTI_DiffTimespecsToDouble(&phc_ts, &local_ts));
+}
+
 static int phc_poll(RCL_Instance instance)
 {
   struct phc_instance *phc;
-  struct timespec phc_ts, sys_ts;
-  double offset, err;
+  struct timespec phc_ts, sys_ts, local_ts;
+  double offset, phc_err, local_err;
 
   phc = (struct phc_instance *)RCL_GetDriverData(instance);
 
   if (!SYS_Linux_GetPHCSample(phc->fd, phc->nocrossts, RCL_GetPrecision(instance),
-                              &phc->mode, &phc_ts, &sys_ts, &err))
+                              &phc->mode, &phc_ts, &sys_ts, &phc_err))
     return 0;
 
+  if (phc->extpps) {
+    LCL_CookTime(&sys_ts, &local_ts, &local_err);
+    HCL_AccumulateSample(phc->clock, &phc_ts, &local_ts, phc_err + local_err);
+    return 0;
+  }
+
   offset = UTI_DiffTimespecsToDouble(&phc_ts, &sys_ts);
 
-  DEBUG_LOG("PHC offset: %+.9f err: %.9f", offset, err);
+  DEBUG_LOG("PHC offset: %+.9f err: %.9f", offset, phc_err);
 
   return RCL_AddSample(instance, &sys_ts, offset, LEAP_Normal);
 }