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); }