2019-07-08 07:47:48 +08:00
|
|
|
/**
|
|
|
|
* @file ts2phc_slave.c
|
|
|
|
* @brief Utility program to synchronize the PHC clock to external events
|
|
|
|
* @note Copyright (C) 2019 Balint Ferencz <fernya@sch.bme.hu>
|
|
|
|
* @note SPDX-License-Identifier: GPL-2.0+
|
|
|
|
*/
|
|
|
|
#include <errno.h>
|
|
|
|
#include <linux/ptp_clock.h>
|
|
|
|
#include <poll.h>
|
2020-07-23 05:56:36 +08:00
|
|
|
#include <stdbool.h>
|
2019-07-08 07:47:48 +08:00
|
|
|
#include <stdint.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <sys/queue.h>
|
|
|
|
#include <time.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
|
|
|
|
#include "clockadj.h"
|
ts2phc: instantiate a full clock structure for every PHC master
This propagates the use of "struct ts2phc_private" all the way into the
master API, in preparation of a new use case that will be supported
soon: some PPS masters (to be precise, the "PHC" kind) instantiate a
struct clock which could be disciplined by ts2phc.
When a PHC A emits a pulse and another PHC B timestamps it, the offset
between their precise timestamps can be used to synchronize either one
of them. So far in ts2phc, only the slave PHC (the one using extts) has
been synchronized to the master (the one using perout).
This is partly because there is no proper kernel API to report the
precise timestamp of a perout pulse. We only have the periodic API, and
that doesn't report precise timestamps either; we just use vague
approximations of what the PPS master PHC's time was, based on reading
that PHC immediately after a slave extts event was received by the
application. While this is far from ideal, it does work, and does allow
PHC A to be synchronized to B.
This is particularly useful with the yet-to-be-introduced "automatic"
mode of ts2phc (similar to '-a' of phc2sys), and the PPS distribution
tree is fixed in hardware (as opposed to port states, which in
"automatic" mode are dynamic, as the name suggests).
Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
2020-07-28 07:52:16 +08:00
|
|
|
#include "config.h"
|
2019-07-08 07:47:48 +08:00
|
|
|
#include "missing.h"
|
|
|
|
#include "phc.h"
|
|
|
|
#include "print.h"
|
|
|
|
#include "servo.h"
|
2020-06-29 03:02:38 +08:00
|
|
|
#include "ts2phc.h"
|
2019-07-08 07:47:48 +08:00
|
|
|
#include "util.h"
|
|
|
|
|
|
|
|
struct ts2phc_slave {
|
|
|
|
char *name;
|
|
|
|
STAILQ_ENTRY(ts2phc_slave) list;
|
|
|
|
struct ptp_pin_desc pin_desc;
|
|
|
|
unsigned int polarity;
|
2020-08-01 22:45:00 +08:00
|
|
|
tmv_t correction;
|
2019-07-08 07:47:48 +08:00
|
|
|
uint32_t ignore_lower;
|
|
|
|
uint32_t ignore_upper;
|
2020-07-28 07:51:28 +08:00
|
|
|
struct clock *clock;
|
2019-07-08 07:47:48 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
struct ts2phc_slave_array {
|
|
|
|
struct ts2phc_slave **slave;
|
2020-08-01 22:45:00 +08:00
|
|
|
int *collected_events;
|
2019-07-08 07:47:48 +08:00
|
|
|
struct pollfd *pfd;
|
2020-06-29 03:02:38 +08:00
|
|
|
};
|
2019-07-08 07:47:48 +08:00
|
|
|
|
|
|
|
enum extts_result {
|
|
|
|
EXTTS_ERROR = -1,
|
|
|
|
EXTTS_OK = 0,
|
|
|
|
EXTTS_IGNORE = 1,
|
|
|
|
};
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
static int ts2phc_slave_array_create(struct ts2phc_private *priv)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
2020-06-29 03:02:38 +08:00
|
|
|
struct ts2phc_slave_array *polling_array;
|
2019-07-08 07:47:48 +08:00
|
|
|
struct ts2phc_slave *slave;
|
|
|
|
unsigned int i;
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
polling_array = malloc(sizeof(*polling_array));
|
|
|
|
if (!polling_array) {
|
|
|
|
pr_err("low memory");
|
|
|
|
return -1;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
|
|
|
|
polling_array->slave = malloc(priv->n_slaves *
|
|
|
|
sizeof(*polling_array->slave));
|
|
|
|
if (!polling_array->slave) {
|
2019-07-08 07:47:48 +08:00
|
|
|
pr_err("low memory");
|
|
|
|
return -1;
|
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
polling_array->pfd = malloc(priv->n_slaves *
|
|
|
|
sizeof(*polling_array->pfd));
|
|
|
|
if (!polling_array->pfd) {
|
2019-07-08 07:47:48 +08:00
|
|
|
pr_err("low memory");
|
2020-06-29 03:02:38 +08:00
|
|
|
free(polling_array->slave);
|
|
|
|
polling_array->slave = NULL;
|
2019-07-08 07:47:48 +08:00
|
|
|
return -1;
|
|
|
|
}
|
2020-08-01 22:45:00 +08:00
|
|
|
polling_array->collected_events = malloc(priv->n_slaves * sizeof(int));
|
|
|
|
if (!polling_array->collected_events) {
|
|
|
|
pr_err("low memory");
|
|
|
|
free(polling_array->slave);
|
|
|
|
free(polling_array->pfd);
|
|
|
|
polling_array->pfd = NULL;
|
|
|
|
polling_array->slave = NULL;
|
|
|
|
return -1;
|
|
|
|
}
|
2019-07-08 07:47:48 +08:00
|
|
|
i = 0;
|
2020-06-29 03:02:38 +08:00
|
|
|
STAILQ_FOREACH(slave, &priv->slaves, list) {
|
|
|
|
polling_array->slave[i] = slave;
|
2019-07-08 07:47:48 +08:00
|
|
|
i++;
|
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
for (i = 0; i < priv->n_slaves; i++) {
|
2020-07-28 07:51:28 +08:00
|
|
|
struct ts2phc_slave *slave = polling_array->slave[i];
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
polling_array->pfd[i].events = POLLIN | POLLPRI;
|
2020-07-28 07:51:28 +08:00
|
|
|
polling_array->pfd[i].fd = CLOCKID_TO_FD(slave->clock->clkid);
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
|
|
|
|
priv->polling_array = polling_array;
|
|
|
|
|
2019-07-08 07:47:48 +08:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
static void ts2phc_slave_array_destroy(struct ts2phc_private *priv)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
2020-06-29 03:02:38 +08:00
|
|
|
struct ts2phc_slave_array *polling_array = priv->polling_array;
|
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
if (!polling_array)
|
|
|
|
return;
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
free(polling_array->slave);
|
|
|
|
free(polling_array->pfd);
|
2020-08-01 22:45:00 +08:00
|
|
|
free(polling_array->collected_events);
|
2020-07-28 07:51:28 +08:00
|
|
|
free(polling_array);
|
|
|
|
priv->polling_array = NULL;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave)
|
|
|
|
{
|
|
|
|
struct pollfd pfd = {
|
|
|
|
.events = POLLIN | POLLPRI,
|
2020-07-28 07:51:28 +08:00
|
|
|
.fd = CLOCKID_TO_FD(slave->clock->clkid),
|
2019-07-08 07:47:48 +08:00
|
|
|
};
|
|
|
|
struct ptp_extts_event event;
|
|
|
|
int cnt, size;
|
|
|
|
|
|
|
|
while (1) {
|
|
|
|
cnt = poll(&pfd, 1, 0);
|
|
|
|
if (cnt < 0) {
|
|
|
|
if (EINTR == errno) {
|
|
|
|
continue;
|
|
|
|
} else {
|
|
|
|
pr_emerg("poll failed");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
} else if (!cnt) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
size = read(pfd.fd, &event, sizeof(event));
|
|
|
|
if (size != sizeof(event)) {
|
|
|
|
pr_err("read failed");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
pr_debug("%s SKIP extts index %u at %lld.%09u",
|
|
|
|
slave->name, event.index, event.t.sec, event.t.nsec);
|
|
|
|
}
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
static struct ts2phc_slave *ts2phc_slave_create(struct ts2phc_private *priv,
|
|
|
|
const char *device)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
|
|
|
struct ptp_extts_request extts;
|
|
|
|
struct ts2phc_slave *slave;
|
2020-07-28 07:51:28 +08:00
|
|
|
int err, pulsewidth;
|
2020-08-01 22:45:00 +08:00
|
|
|
int32_t correction;
|
2019-07-08 07:47:48 +08:00
|
|
|
|
|
|
|
slave = calloc(1, sizeof(*slave));
|
|
|
|
if (!slave) {
|
|
|
|
pr_err("low memory");
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
slave->name = strdup(device);
|
|
|
|
if (!slave->name) {
|
|
|
|
pr_err("low memory");
|
|
|
|
free(slave);
|
|
|
|
return NULL;
|
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
slave->pin_desc.index = config_get_int(priv->cfg, device,
|
|
|
|
"ts2phc.pin_index");
|
2019-07-08 07:47:48 +08:00
|
|
|
slave->pin_desc.func = PTP_PF_EXTTS;
|
2020-06-29 03:02:38 +08:00
|
|
|
slave->pin_desc.chan = config_get_int(priv->cfg, device,
|
|
|
|
"ts2phc.channel");
|
|
|
|
slave->polarity = config_get_int(priv->cfg, device,
|
|
|
|
"ts2phc.extts_polarity");
|
2020-08-01 22:45:00 +08:00
|
|
|
correction = config_get_int(priv->cfg, device,
|
|
|
|
"ts2phc.extts_correction");
|
|
|
|
slave->correction = nanoseconds_to_tmv(correction);
|
2020-06-29 03:02:38 +08:00
|
|
|
|
|
|
|
pulsewidth = config_get_int(priv->cfg, device,
|
|
|
|
"ts2phc.pulsewidth");
|
2019-07-08 07:47:48 +08:00
|
|
|
pulsewidth /= 2;
|
|
|
|
slave->ignore_upper = 1000000000 - pulsewidth;
|
|
|
|
slave->ignore_lower = pulsewidth;
|
|
|
|
|
2020-07-28 07:51:28 +08:00
|
|
|
slave->clock = clock_add(priv, device);
|
|
|
|
if (!slave->clock) {
|
2019-07-08 07:47:48 +08:00
|
|
|
pr_err("failed to open clock");
|
|
|
|
goto no_posix_clock;
|
|
|
|
}
|
2020-07-28 08:01:16 +08:00
|
|
|
slave->clock->is_destination = 1;
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-07-28 07:51:28 +08:00
|
|
|
pr_debug("PHC slave %s has ptp index %d", device,
|
|
|
|
slave->clock->phc_index);
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-07-28 07:51:28 +08:00
|
|
|
if (phc_number_pins(slave->clock->clkid) > 0) {
|
|
|
|
err = phc_pin_setfunc(slave->clock->clkid, &slave->pin_desc);
|
2019-07-08 07:47:48 +08:00
|
|
|
if (err < 0) {
|
|
|
|
pr_err("PTP_PIN_SETFUNC request failed");
|
|
|
|
goto no_pin_func;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Disable external time stamping, and then read out any stale
|
|
|
|
* time stamps.
|
|
|
|
*/
|
|
|
|
memset(&extts, 0, sizeof(extts));
|
|
|
|
extts.index = slave->pin_desc.chan;
|
|
|
|
extts.flags = 0;
|
2020-07-28 07:51:28 +08:00
|
|
|
if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
|
|
|
|
&extts)) {
|
2019-07-08 07:47:48 +08:00
|
|
|
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
|
|
|
}
|
|
|
|
if (ts2phc_slave_clear_fifo(slave)) {
|
|
|
|
goto no_ext_ts;
|
|
|
|
}
|
|
|
|
|
|
|
|
return slave;
|
|
|
|
no_ext_ts:
|
|
|
|
no_pin_func:
|
2020-07-28 07:51:28 +08:00
|
|
|
clock_destroy(slave->clock);
|
2019-07-08 07:47:48 +08:00
|
|
|
no_posix_clock:
|
|
|
|
free(slave->name);
|
|
|
|
free(slave);
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void ts2phc_slave_destroy(struct ts2phc_slave *slave)
|
|
|
|
{
|
|
|
|
struct ptp_extts_request extts;
|
|
|
|
|
|
|
|
memset(&extts, 0, sizeof(extts));
|
|
|
|
extts.index = slave->pin_desc.chan;
|
|
|
|
extts.flags = 0;
|
2020-07-28 07:51:28 +08:00
|
|
|
if (ioctl(CLOCKID_TO_FD(slave->clock->clkid), PTP_EXTTS_REQUEST2,
|
|
|
|
&extts)) {
|
2019-07-08 07:47:48 +08:00
|
|
|
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
|
|
|
}
|
2020-07-28 07:51:28 +08:00
|
|
|
clock_destroy(slave->clock);
|
2019-07-08 07:47:48 +08:00
|
|
|
free(slave->name);
|
|
|
|
free(slave);
|
|
|
|
}
|
|
|
|
|
ts2phc_phc_master: make use of new kernel API for perout waveform
This API was introduced for 2 reasons:
1. Some hardware can emit PPS signals but not starting from arbitrary
absolute times, but rather phase-aligned to the beginning of a
second. We _could_ patch ts2phc to always specify a start time of
0.000000000 to PTP_PEROUT_REQUEST, but in practice, we would never
know whether that would actually work with all in-tree PHC drivers.
So there was a need for a new flag that only specifies the phase of
the periodic signal, and not the absolute start time.
2. Some hardware can, rather unfortunately, not distinguish between a
rising and a falling extts edge. And, since whatever rises also has
to fall before rising again, the strategy in ts2phc is to set a
'large' pulse width (half the period) and ignore the extts event
corresponding to the mid-way between one second and another. This is
all fine, but currently, ts2phc.pulsewidth is a read-only property in
the config file. The kernel is not instructed in any way to use this
value, it is simply that must be configured based on prior knowledge
of the PHC's implementation. This API changes that.
The introduction of a phase adjustment for the master PHC means we have
to adjust our approximation of the precise perout timestamp. We put that
code into a common function and convert all call sites to call that. We
also need to do the same thing for the edge ignoring logic.
Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
2020-05-25 15:57:44 +08:00
|
|
|
static bool ts2phc_slave_ignore(struct ts2phc_private *priv,
|
|
|
|
struct ts2phc_slave *slave,
|
|
|
|
struct timespec source_ts)
|
|
|
|
{
|
|
|
|
tmv_t source_tmv = timespec_to_tmv(source_ts);
|
|
|
|
|
|
|
|
source_tmv = tmv_sub(source_tmv, priv->perout_phase);
|
|
|
|
source_ts = tmv_to_timespec(source_tmv);
|
|
|
|
|
|
|
|
return source_ts.tv_nsec > slave->ignore_lower &&
|
|
|
|
source_ts.tv_nsec < slave->ignore_upper;
|
|
|
|
}
|
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
static enum extts_result ts2phc_slave_event(struct ts2phc_private *priv,
|
|
|
|
struct ts2phc_slave *slave)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
2020-08-01 22:45:00 +08:00
|
|
|
enum extts_result result = EXTTS_OK;
|
2019-07-08 07:47:48 +08:00
|
|
|
struct ptp_extts_event event;
|
2020-08-01 22:45:00 +08:00
|
|
|
struct timespec source_ts;
|
|
|
|
int err, cnt;
|
|
|
|
tmv_t ts;
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-07-28 07:51:28 +08:00
|
|
|
cnt = read(CLOCKID_TO_FD(slave->clock->clkid), &event, sizeof(event));
|
2019-07-08 07:47:48 +08:00
|
|
|
if (cnt != sizeof(event)) {
|
|
|
|
pr_err("read extts event failed: %m");
|
2020-08-01 22:45:00 +08:00
|
|
|
result = EXTTS_ERROR;
|
|
|
|
goto out;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
if (event.index != slave->pin_desc.chan) {
|
|
|
|
pr_err("extts on unexpected channel");
|
2020-08-01 22:45:00 +08:00
|
|
|
result = EXTTS_ERROR;
|
|
|
|
goto out;
|
|
|
|
}
|
|
|
|
|
|
|
|
err = ts2phc_master_getppstime(priv->master, &source_ts);
|
|
|
|
if (err < 0) {
|
|
|
|
pr_debug("source ts not valid");
|
|
|
|
return 0;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
|
ts2phc_phc_master: make use of new kernel API for perout waveform
This API was introduced for 2 reasons:
1. Some hardware can emit PPS signals but not starting from arbitrary
absolute times, but rather phase-aligned to the beginning of a
second. We _could_ patch ts2phc to always specify a start time of
0.000000000 to PTP_PEROUT_REQUEST, but in practice, we would never
know whether that would actually work with all in-tree PHC drivers.
So there was a need for a new flag that only specifies the phase of
the periodic signal, and not the absolute start time.
2. Some hardware can, rather unfortunately, not distinguish between a
rising and a falling extts edge. And, since whatever rises also has
to fall before rising again, the strategy in ts2phc is to set a
'large' pulse width (half the period) and ignore the extts event
corresponding to the mid-way between one second and another. This is
all fine, but currently, ts2phc.pulsewidth is a read-only property in
the config file. The kernel is not instructed in any way to use this
value, it is simply that must be configured based on prior knowledge
of the PHC's implementation. This API changes that.
The introduction of a phase adjustment for the master PHC means we have
to adjust our approximation of the precise perout timestamp. We put that
code into a common function and convert all call sites to call that. We
also need to do the same thing for the edge ignoring logic.
Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
2020-05-25 15:57:44 +08:00
|
|
|
ts2phc_slave_ignore(priv, slave, source_ts)) {
|
2019-07-08 07:47:48 +08:00
|
|
|
|
|
|
|
pr_debug("%s SKIP extts index %u at %lld.%09u src %" PRIi64 ".%ld",
|
|
|
|
slave->name, event.index, event.t.sec, event.t.nsec,
|
|
|
|
(int64_t) source_ts.tv_sec, source_ts.tv_nsec);
|
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
result = EXTTS_IGNORE;
|
|
|
|
goto out;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
out:
|
|
|
|
if (result == EXTTS_ERROR || result == EXTTS_IGNORE)
|
|
|
|
return result;
|
|
|
|
|
|
|
|
ts = pct_to_tmv(event.t);
|
|
|
|
ts = tmv_add(ts, slave->correction);
|
|
|
|
clock_add_tstamp(slave->clock, ts);
|
2019-07-08 07:47:48 +08:00
|
|
|
|
|
|
|
return EXTTS_OK;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* public methods */
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
int ts2phc_slave_add(struct ts2phc_private *priv, const char *name)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
|
|
|
struct ts2phc_slave *slave;
|
|
|
|
|
|
|
|
/* Create each interface only once. */
|
2020-06-29 03:02:38 +08:00
|
|
|
STAILQ_FOREACH(slave, &priv->slaves, list) {
|
2019-07-08 07:47:48 +08:00
|
|
|
if (0 == strcmp(name, slave->name)) {
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
slave = ts2phc_slave_create(priv, name);
|
2019-07-08 07:47:48 +08:00
|
|
|
if (!slave) {
|
|
|
|
pr_err("failed to create slave");
|
|
|
|
return -1;
|
|
|
|
}
|
2020-06-29 03:02:38 +08:00
|
|
|
STAILQ_INSERT_TAIL(&priv->slaves, slave, list);
|
|
|
|
priv->n_slaves++;
|
2019-07-08 07:47:48 +08:00
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
int ts2phc_slave_arm(struct ts2phc_private *priv)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
|
|
|
struct ptp_extts_request extts;
|
|
|
|
struct ts2phc_slave *slave;
|
|
|
|
int err;
|
|
|
|
|
|
|
|
memset(&extts, 0, sizeof(extts));
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
STAILQ_FOREACH(slave, &priv->slaves, list) {
|
2019-07-08 07:47:48 +08:00
|
|
|
extts.index = slave->pin_desc.chan;
|
|
|
|
extts.flags = slave->polarity | PTP_ENABLE_FEATURE;
|
2020-07-28 07:51:28 +08:00
|
|
|
err = ioctl(CLOCKID_TO_FD(slave->clock->clkid),
|
|
|
|
PTP_EXTTS_REQUEST2, &extts);
|
2019-07-08 07:47:48 +08:00
|
|
|
if (err < 0) {
|
|
|
|
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
int ts2phc_slaves_init(struct ts2phc_private *priv)
|
|
|
|
{
|
|
|
|
int err;
|
|
|
|
|
|
|
|
err = ts2phc_slave_array_create(priv);
|
|
|
|
if (err)
|
|
|
|
return err;
|
|
|
|
|
|
|
|
return ts2phc_slave_arm(priv);
|
|
|
|
}
|
|
|
|
|
|
|
|
void ts2phc_slave_cleanup(struct ts2phc_private *priv)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
|
|
|
struct ts2phc_slave *slave;
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
ts2phc_slave_array_destroy(priv);
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
while ((slave = STAILQ_FIRST(&priv->slaves))) {
|
|
|
|
STAILQ_REMOVE_HEAD(&priv->slaves, list);
|
2019-07-08 07:47:48 +08:00
|
|
|
ts2phc_slave_destroy(slave);
|
2020-06-29 03:02:38 +08:00
|
|
|
priv->n_slaves--;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-06-29 03:02:38 +08:00
|
|
|
int ts2phc_slave_poll(struct ts2phc_private *priv)
|
2019-07-08 07:47:48 +08:00
|
|
|
{
|
2020-06-29 03:02:38 +08:00
|
|
|
struct ts2phc_slave_array *polling_array = priv->polling_array;
|
2020-08-01 22:45:00 +08:00
|
|
|
int all_slaves_have_events = 0;
|
|
|
|
int ignore_any = 0;
|
2019-07-08 07:47:48 +08:00
|
|
|
unsigned int i;
|
2020-08-01 22:45:00 +08:00
|
|
|
int cnt;
|
|
|
|
|
|
|
|
for (i = 0; i < priv->n_slaves; i++)
|
|
|
|
polling_array->collected_events[i] = 0;
|
|
|
|
|
|
|
|
while (!all_slaves_have_events) {
|
|
|
|
struct ts2phc_slave *slave;
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
cnt = poll(polling_array->pfd, priv->n_slaves, 2000);
|
|
|
|
if (cnt < 0) {
|
|
|
|
if (EINTR == errno) {
|
|
|
|
return 0;
|
|
|
|
} else {
|
|
|
|
pr_emerg("poll failed");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
} else if (!cnt) {
|
|
|
|
pr_debug("poll returns zero, no events");
|
2019-07-08 07:47:48 +08:00
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
for (i = 0; i < priv->n_slaves; i++) {
|
|
|
|
if (polling_array->pfd[i].revents & (POLLIN|POLLPRI)) {
|
|
|
|
enum extts_result result;
|
|
|
|
|
|
|
|
slave = polling_array->slave[i];
|
|
|
|
|
|
|
|
result = ts2phc_slave_event(priv, slave);
|
|
|
|
if (result == EXTTS_ERROR)
|
|
|
|
return -EIO;
|
|
|
|
if (result == EXTTS_IGNORE)
|
|
|
|
ignore_any = 1;
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Collect the events anyway, even if we'll
|
|
|
|
* ignore this master edge anyway. We don't
|
|
|
|
* want slave events from different edges
|
|
|
|
* to pile up and mix.
|
|
|
|
*/
|
|
|
|
polling_array->collected_events[i]++;
|
|
|
|
}
|
|
|
|
}
|
2019-07-08 07:47:48 +08:00
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
all_slaves_have_events = true;
|
2020-07-28 07:51:28 +08:00
|
|
|
|
2020-08-01 22:45:00 +08:00
|
|
|
for (i = 0; i < priv->n_slaves; i++) {
|
|
|
|
if (!polling_array->collected_events[i]) {
|
|
|
|
all_slaves_have_events = false;
|
|
|
|
break;
|
|
|
|
}
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|
|
|
|
}
|
2020-08-01 22:45:00 +08:00
|
|
|
|
|
|
|
if (ignore_any)
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
return 1;
|
2019-07-08 07:47:48 +08:00
|
|
|
}
|