diff --git a/refclock_phc.c b/refclock_phc.c index e0e206e..3b68c84 100644 --- a/refclock_phc.c +++ b/refclock_phc.c @@ -33,6 +33,9 @@ #include "sysincl.h" +#include + +#include "array.h" #include "refclock.h" #include "hwclock.h" #include "local.h" @@ -44,14 +47,19 @@ struct phc_instance { int fd; + int dev_index; int mode; int nocrossts; int extpps; int pin; int channel; + struct timespec last_extts; HCL_Instance clock; }; +/* Array of RCL_Instance with enabled extpps */ +static ARR_Instance extts_phcs = NULL; + static void read_ext_pulse(int sockfd, int event, void *anything); static int phc_initialise(RCL_Instance instance) @@ -59,6 +67,7 @@ static int phc_initialise(RCL_Instance instance) const char *options[] = {"nocrossts", "extpps", "pin", "channel", "clear", NULL}; struct phc_instance *phc; int phc_fd, rising_edge; + struct stat st; char *path, *s; RCL_CheckDriverOptions(instance, options); @@ -71,10 +80,13 @@ static int phc_initialise(RCL_Instance instance) phc = MallocNew(struct phc_instance); phc->fd = phc_fd; + if (fstat(phc_fd, &st) < 0 || !S_ISCHR(st.st_mode)) + LOG_FATAL("Could not get PHC index"); + phc->dev_index = minor(st.st_rdev); phc->mode = 0; phc->nocrossts = RCL_GetDriverOption(instance, "nocrossts") ? 1 : 0; phc->extpps = RCL_GetDriverOption(instance, "extpps") ? 1 : 0; - + UTI_ZeroTimespec(&phc->last_extts); phc->clock = HCL_CreateInstance(0, 16, UTI_Log2ToDouble(RCL_GetDriverPoll(instance)), RCL_GetPrecision(instance)); @@ -90,6 +102,10 @@ static int phc_initialise(RCL_Instance instance) LOG_FATAL("Could not enable external PHC timestamping"); SCH_AddFileHandler(phc->fd, SCH_FILE_INPUT, read_ext_pulse, instance); + + if (!extts_phcs) + extts_phcs = ARR_CreateInstance(sizeof (RCL_Instance)); + ARR_AppendElement(extts_phcs, &instance); } else { phc->pin = phc->channel = 0; } @@ -101,12 +117,22 @@ static int phc_initialise(RCL_Instance instance) static void phc_finalise(RCL_Instance instance) { struct phc_instance *phc; + unsigned int i; 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); + + for (i = 0; i < ARR_GetSize(extts_phcs); i++) { + if ((*(RCL_Instance *)ARR_GetElement(extts_phcs, i)) == instance) + ARR_RemoveElement(extts_phcs, i--); + } + if (ARR_GetSize(extts_phcs) == 0) { + ARR_DestroyInstance(extts_phcs); + extts_phcs = NULL; + } } HCL_DestroyInstance(phc->clock); @@ -114,30 +140,52 @@ static void phc_finalise(RCL_Instance instance) Free(phc); } -static void read_ext_pulse(int fd, int event, void *anything) +static void process_ext_pulse(RCL_Instance instance, struct timespec *phc_ts) { - RCL_Instance instance; struct phc_instance *phc; - struct timespec phc_ts, local_ts; + struct timespec 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); + if (UTI_CompareTimespecs(&phc->last_extts, phc_ts) == 0) { + DEBUG_LOG("Ignoring duplicated PHC timestamp"); return; } + phc->last_extts = *phc_ts; - if (!HCL_CookTime(phc->clock, &phc_ts, &local_ts, &local_err)) + 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)); + UTI_DiffTimespecsToDouble(phc_ts, &local_ts)); +} + +static void read_ext_pulse(int fd, int event, void *anything) +{ + RCL_Instance instance; + struct phc_instance *phc1, *phc2; + struct timespec phc_ts; + unsigned int i; + int channel; + + if (!SYS_Linux_ReadPHCExtTimestamp(fd, &phc_ts, &channel)) + return; + + instance = anything; + phc1 = RCL_GetDriverData(instance); + + /* The Linux kernel (as of 6.2) has one shared queue of timestamps for all + descriptors of the same PHC. Search for all refclocks that expect + the timestamp. */ + + for (i = 0; i < ARR_GetSize(extts_phcs); i++) { + instance = *(RCL_Instance *)ARR_GetElement(extts_phcs, i); + phc2 = RCL_GetDriverData(instance); + if (!phc2->extpps || phc2->dev_index != phc1->dev_index || phc2->channel != channel) + continue; + process_ext_pulse(instance, &phc_ts); + } } #define PHC_READINGS 25