Introduce the ts2phc program.
Some PTP Hardware Clocks have input pins that can generate time stamps on the edges of external signals. This functionality can be used in various ways. For example, one can synchronize a PHC device to a global time source by taking a Pulse Per Second signal from the source into the PHC. This patch adds support for synchronizing one or more PHC slaves to a given master clock. The implementation follows a modular design that allows adding different kinds of master clocks in the future. This patch starts off with a single "generic" PPS master, meaning a PPS signal that lacks and time or date information. The generic master assumes that the Linux system time is approximately correct (by NTP or RTC for example) in order to calculate the time of the incoming PPS edges. Signed-off-by: Richard Cochran <richardcochran@gmail.com> Signed-off-by: Balint Ferencz <fernya@gmail.com>master
Richard Cochran
parent
2f0bfb2837
commit
1bdc9143aa
|
|
@ -9,3 +9,4 @@
|
|||
/phc_ctl
|
||||
/snmp4lptp
|
||||
/timemaster
|
||||
/ts2phc
|
||||
|
|
|
|||
14
config.c
14
config.c
|
|
@ -19,6 +19,7 @@
|
|||
#include <ctype.h>
|
||||
#include <float.h>
|
||||
#include <limits.h>
|
||||
#include <linux/ptp_clock.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
|
@ -169,6 +170,13 @@ static struct config_enum delay_mech_enu[] = {
|
|||
{ NULL, 0 },
|
||||
};
|
||||
|
||||
static struct config_enum extts_polarity_enu[] = {
|
||||
{ "rising", PTP_RISING_EDGE },
|
||||
{ "falling", PTP_FALLING_EDGE },
|
||||
{ "both", PTP_RISING_EDGE | PTP_FALLING_EDGE },
|
||||
{ NULL, 0 },
|
||||
};
|
||||
|
||||
static struct config_enum hwts_filter_enu[] = {
|
||||
{ "normal", HWTS_FILTER_NORMAL },
|
||||
{ "check", HWTS_FILTER_CHECK },
|
||||
|
|
@ -297,6 +305,12 @@ struct config_item config_tab[] = {
|
|||
GLOB_ITEM_INT("timeSource", INTERNAL_OSCILLATOR, 0x10, 0xfe),
|
||||
GLOB_ITEM_ENU("time_stamping", TS_HARDWARE, timestamping_enu),
|
||||
PORT_ITEM_INT("transportSpecific", 0, 0, 0x0F),
|
||||
PORT_ITEM_INT("ts2phc.channel", 0, 0, INT_MAX),
|
||||
PORT_ITEM_INT("ts2phc.extts_correction", 0, INT_MIN, INT_MAX),
|
||||
PORT_ITEM_ENU("ts2phc.extts_polarity", PTP_RISING_EDGE, extts_polarity_enu),
|
||||
PORT_ITEM_INT("ts2phc.master", 0, 0, 1),
|
||||
PORT_ITEM_INT("ts2phc.pin_index", 0, 0, INT_MAX),
|
||||
GLOB_ITEM_INT("ts2phc.pulsewidth", 500000000, 1000000, 999000000),
|
||||
PORT_ITEM_ENU("tsproc_mode", TSPROC_FILTER, tsproc_enu),
|
||||
GLOB_ITEM_INT("twoStepFlag", 1, 0, 1),
|
||||
GLOB_ITEM_INT("tx_timestamp_timeout", 1, 1, INT_MAX),
|
||||
|
|
|
|||
|
|
@ -0,0 +1,18 @@
|
|||
#
|
||||
# This example uses a PPS signal from a GPS receiver as an input to
|
||||
# the SDP0 pin of an Intel i210 card. The pulse from the receiver has
|
||||
# a width of 100 milliseconds.
|
||||
#
|
||||
# Important! The polarity is set to "both" because the i210 always
|
||||
# time stamps both the rising and the falling edges of the input
|
||||
# signal.
|
||||
#
|
||||
[global]
|
||||
use_syslog 0
|
||||
verbose 1
|
||||
logging_level 6
|
||||
ts2phc.pulsewidth 100000000
|
||||
[eth6]
|
||||
ts2phc.channel 0
|
||||
ts2phc.extts_polarity both
|
||||
ts2phc.pin_index 0
|
||||
8
makefile
8
makefile
|
|
@ -22,10 +22,11 @@ CC = $(CROSS_COMPILE)gcc
|
|||
VER = -DVER=$(version)
|
||||
CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
|
||||
LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
|
||||
PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster
|
||||
PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster ts2phc
|
||||
FILTERS = filter.o mave.o mmedian.o
|
||||
SERVOS = linreg.o ntpshm.o nullf.o pi.o servo.o
|
||||
TRANSP = raw.o transport.o udp.o udp6.o uds.o
|
||||
TS2PHC = ts2phc.o ts2phc_generic_master.o ts2phc_master.o ts2phc_slave.o
|
||||
OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
|
||||
e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o msg.o phc.o port.o \
|
||||
port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) sk.o \
|
||||
|
|
@ -33,7 +34,7 @@ OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o designated_fsm.o \
|
|||
unicast_fsm.o unicast_service.o util.o version.o
|
||||
|
||||
OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
|
||||
sysoff.o timemaster.o
|
||||
sysoff.o timemaster.o $(TS2PHC)
|
||||
SRC = $(OBJECTS:.o=.c)
|
||||
DEPEND = $(OBJECTS:.o=.d)
|
||||
srcdir := $(dir $(lastword $(MAKEFILE_LIST)))
|
||||
|
|
@ -66,6 +67,9 @@ phc_ctl: phc_ctl.o phc.o sk.o util.o clockadj.o sysoff.o print.o version.o
|
|||
|
||||
timemaster: phc.o print.o rtnl.o sk.o timemaster.o util.o version.o
|
||||
|
||||
ts2phc: config.o clockadj.o hash.o interface.o phc.o print.o $(SERVOS) sk.o \
|
||||
$(TS2PHC) util.o version.o
|
||||
|
||||
version.o: .version version.sh $(filter-out version.d,$(DEPEND))
|
||||
|
||||
.version: force
|
||||
|
|
|
|||
|
|
@ -0,0 +1,200 @@
|
|||
/**
|
||||
* @file ts2phc.c
|
||||
* @brief Utility program to synchronize the PHC clock to external events
|
||||
* @note Copyright (C) 2013 Balint Ferencz <fernya@sch.bme.hu>
|
||||
* @note Based on the phc2sys utility
|
||||
* @note Copyright (C) 2012 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "config.h"
|
||||
#include "interface.h"
|
||||
#include "print.h"
|
||||
#include "ts2phc_master.h"
|
||||
#include "ts2phc_slave.h"
|
||||
#include "version.h"
|
||||
|
||||
struct interface {
|
||||
STAILQ_ENTRY(interface) list;
|
||||
};
|
||||
|
||||
static void ts2phc_cleanup(struct config *cfg, struct ts2phc_master *master)
|
||||
{
|
||||
ts2phc_slave_cleanup();
|
||||
if (master) {
|
||||
ts2phc_master_destroy(master);
|
||||
}
|
||||
if (cfg) {
|
||||
config_destroy(cfg);
|
||||
}
|
||||
}
|
||||
|
||||
static void usage(char *progname)
|
||||
{
|
||||
fprintf(stderr,
|
||||
"\n"
|
||||
"usage: %s [options]\n\n"
|
||||
" -c [dev|name] phc slave clock (like /dev/ptp0 or eth0)\n"
|
||||
" (may be specified multiple times)\n"
|
||||
" -f [file] read configuration from 'file'\n"
|
||||
" -h prints this message and exits\n"
|
||||
" -l [num] set the logging level to 'num'\n"
|
||||
" -m print messages to stdout\n"
|
||||
" -q do not print messages to the syslog\n"
|
||||
" -s [dev|name] source of the PPS signal\n"
|
||||
" may take any of the following forms:\n"
|
||||
" generic - an external 1-PPS without ToD information\n"
|
||||
" -v prints the software version and exits\n"
|
||||
"\n",
|
||||
progname);
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
int c, err = 0, have_slave = 0, index, print_level;
|
||||
struct ts2phc_master *master = NULL;
|
||||
enum ts2phc_master_type pps_type;
|
||||
char *config = NULL, *progname;
|
||||
const char *pps_source = NULL;
|
||||
struct config *cfg = NULL;
|
||||
struct interface *iface;
|
||||
struct option *opts;
|
||||
|
||||
handle_term_signals();
|
||||
|
||||
cfg = config_create();
|
||||
if (!cfg) {
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
|
||||
opts = config_long_options(cfg);
|
||||
|
||||
/* Process the command line arguments. */
|
||||
progname = strrchr(argv[0], '/');
|
||||
progname = progname ? 1 + progname : argv[0];
|
||||
while (EOF != (c = getopt_long(argc, argv, "c:f:hi:l:mqs:v", opts, &index))) {
|
||||
switch (c) {
|
||||
case 0:
|
||||
if (config_parse_option(cfg, opts[index].name, optarg)) {
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
break;
|
||||
case 'c':
|
||||
if (!config_create_interface(optarg, cfg)) {
|
||||
fprintf(stderr, "failed to add slave\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
have_slave = 1;
|
||||
break;
|
||||
case 'f':
|
||||
config = optarg;
|
||||
break;
|
||||
case 'l':
|
||||
if (get_arg_val_i(c, optarg, &print_level,
|
||||
PRINT_LEVEL_MIN, PRINT_LEVEL_MAX)) {
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
config_set_int(cfg, "logging_level", print_level);
|
||||
print_set_level(print_level);
|
||||
break;
|
||||
case 'm':
|
||||
config_set_int(cfg, "verbose", 1);
|
||||
print_set_verbose(1);
|
||||
break;
|
||||
case 'q':
|
||||
config_set_int(cfg, "use_syslog", 0);
|
||||
print_set_syslog(0);
|
||||
break;
|
||||
case 's':
|
||||
if (pps_source) {
|
||||
fprintf(stderr, "too many PPS sources\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
pps_source = optarg;
|
||||
break;
|
||||
case 'v':
|
||||
ts2phc_cleanup(cfg, master);
|
||||
version_show(stdout);
|
||||
return 0;
|
||||
case 'h':
|
||||
ts2phc_cleanup(cfg, master);
|
||||
usage(progname);
|
||||
return -1;
|
||||
case '?':
|
||||
default:
|
||||
ts2phc_cleanup(cfg, master);
|
||||
usage(progname);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
if (config && (c = config_read(config, cfg))) {
|
||||
fprintf(stderr, "failed to read config\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
print_set_progname(progname);
|
||||
print_set_tag(config_get_string(cfg, NULL, "message_tag"));
|
||||
print_set_verbose(config_get_int(cfg, NULL, "verbose"));
|
||||
print_set_syslog(config_get_int(cfg, NULL, "use_syslog"));
|
||||
print_set_level(config_get_int(cfg, NULL, "logging_level"));
|
||||
|
||||
STAILQ_FOREACH(iface, &cfg->interfaces, list) {
|
||||
if (1 == config_get_int(cfg, interface_name(iface), "ts2phc.master")) {
|
||||
if (pps_source) {
|
||||
fprintf(stderr, "too many PPS sources\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
pps_source = interface_name(iface);
|
||||
} else {
|
||||
if (ts2phc_slave_add(cfg, interface_name(iface))) {
|
||||
fprintf(stderr, "failed to add slave\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
have_slave = 1;
|
||||
}
|
||||
}
|
||||
if (!have_slave) {
|
||||
fprintf(stderr, "no slave clocks specified\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
usage(progname);
|
||||
return -1;
|
||||
}
|
||||
if (!pps_source) {
|
||||
fprintf(stderr, "no PPS source specified\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
usage(progname);
|
||||
return -1;
|
||||
}
|
||||
if (ts2phc_slave_arm()) {
|
||||
fprintf(stderr, "failed to arm slaves\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
|
||||
pps_type = TS2PHC_MASTER_GENERIC;
|
||||
master = ts2phc_master_create(cfg, pps_source, pps_type);
|
||||
if (!master) {
|
||||
fprintf(stderr, "failed to create master\n");
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (is_running()) {
|
||||
err = ts2phc_slave_poll(master);
|
||||
if (err) {
|
||||
pr_err("poll failed");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
ts2phc_cleanup(cfg, master);
|
||||
return err;
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/**
|
||||
* @file ts2phc_generic_master.c
|
||||
* @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "missing.h"
|
||||
#include "print.h"
|
||||
#include "ts2phc_generic_master.h"
|
||||
#include "ts2phc_master_private.h"
|
||||
#include "util.h"
|
||||
|
||||
struct ts2phc_generic_master {
|
||||
struct ts2phc_master master;
|
||||
};
|
||||
|
||||
static void ts2phc_generic_master_destroy(struct ts2phc_master *master)
|
||||
{
|
||||
struct ts2phc_generic_master *s =
|
||||
container_of(master, struct ts2phc_generic_master, master);
|
||||
free(s);
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the time on the PPS source device at which the most recent
|
||||
* PPS event was generated. This implementation assumes that the
|
||||
* system time is approximately correct.
|
||||
*/
|
||||
static int ts2phc_generic_master_getppstime(struct ts2phc_master *m,
|
||||
struct timespec *ts)
|
||||
{
|
||||
struct timex ntx;
|
||||
int code;
|
||||
|
||||
memset(&ntx, 0, sizeof(ntx));
|
||||
ntx.modes = ADJ_NANO;
|
||||
code = adjtimex(&ntx);
|
||||
if (code == -1) {
|
||||
pr_err("adjtimex failed: %m");
|
||||
return -1;
|
||||
}
|
||||
ts->tv_sec = ntx.time.tv_sec + ntx.tai;
|
||||
ts->tv_nsec = ntx.time.tv_usec;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
|
||||
const char *dev)
|
||||
{
|
||||
struct ts2phc_generic_master *master;
|
||||
|
||||
master = calloc(1, sizeof(*master));
|
||||
if (!master) {
|
||||
return NULL;
|
||||
}
|
||||
master->master.destroy = ts2phc_generic_master_destroy;
|
||||
master->master.getppstime = ts2phc_generic_master_getppstime;
|
||||
|
||||
return &master->master;
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
/**
|
||||
* @file ts2phc_generic_master.h
|
||||
* @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#ifndef HAVE_TS2PHC_GENERIC_MASTER_H
|
||||
#define HAVE_TS2PHC_GENERIC_MASTER_H
|
||||
|
||||
#include "ts2phc_master.h"
|
||||
|
||||
struct ts2phc_master *ts2phc_generic_master_create(struct config *cfg,
|
||||
const char *dev);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,34 @@
|
|||
/**
|
||||
* @file ts2phc_master.c
|
||||
* @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#include "ts2phc_generic_master.h"
|
||||
#include "ts2phc_master_private.h"
|
||||
|
||||
struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
|
||||
enum ts2phc_master_type type)
|
||||
{
|
||||
struct ts2phc_master *master = NULL;
|
||||
|
||||
switch (type) {
|
||||
case TS2PHC_MASTER_GENERIC:
|
||||
master = ts2phc_generic_master_create(cfg, dev);
|
||||
break;
|
||||
case TS2PHC_MASTER_NMEA:
|
||||
break;
|
||||
case TS2PHC_MASTER_PHC:
|
||||
break;
|
||||
}
|
||||
return master;
|
||||
}
|
||||
|
||||
void ts2phc_master_destroy(struct ts2phc_master *master)
|
||||
{
|
||||
master->destroy(master);
|
||||
}
|
||||
|
||||
int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts)
|
||||
{
|
||||
return master->getppstime(master, ts);
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
/**
|
||||
* @file ts2phc_master.h
|
||||
* @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#ifndef HAVE_TS2PHC_MASTER_H
|
||||
#define HAVE_TS2PHC_MASTER_H
|
||||
|
||||
#include <time.h>
|
||||
|
||||
struct config;
|
||||
|
||||
/**
|
||||
* Opaque type
|
||||
*/
|
||||
struct ts2phc_master;
|
||||
|
||||
/**
|
||||
* Defines the available PPS master clocks.
|
||||
*/
|
||||
enum ts2phc_master_type {
|
||||
TS2PHC_MASTER_GENERIC,
|
||||
TS2PHC_MASTER_NMEA,
|
||||
TS2PHC_MASTER_PHC,
|
||||
};
|
||||
|
||||
/**
|
||||
* Create a new instance of a PPS master clock.
|
||||
* @param cfg Pointer to a valid configuration.
|
||||
* @param dev Name of the master clock or NULL.
|
||||
* @param type The type of the clock to create.
|
||||
* @return A pointer to a new PPS master clock on success, NULL otherwise.
|
||||
*/
|
||||
struct ts2phc_master *ts2phc_master_create(struct config *cfg, const char *dev,
|
||||
enum ts2phc_master_type type);
|
||||
|
||||
/**
|
||||
* Destroy an instance of a PPS master clock.
|
||||
* @param master Pointer to a master obtained via @ref ts2phc_master_create().
|
||||
*/
|
||||
void ts2phc_master_destroy(struct ts2phc_master *master);
|
||||
|
||||
/**
|
||||
* Returns the time on the PPS source device at which the most recent
|
||||
* PPS event was generated.
|
||||
* @param master Pointer to a master obtained via @ref ts2phc_master_create().
|
||||
* @param ts Buffer to hold the time of the last PPS event.
|
||||
* @return Zero if the reported time is valid, non-zero otherwise.
|
||||
*/
|
||||
int ts2phc_master_getppstime(struct ts2phc_master *master, struct timespec *ts);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
/**
|
||||
* @file ts2phc_master_private.h
|
||||
* @note Copyright (C) 2019 Richard Cochran <richardcochran@gmail.com>
|
||||
* @note SPDX-License-Identifier: GPL-2.0+
|
||||
*/
|
||||
#ifndef HAVE_TS2PHC_MASTER_PRIVATE_H
|
||||
#define HAVE_TS2PHC_MASTER_PRIVATE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "contain.h"
|
||||
#include "ts2phc_master.h"
|
||||
|
||||
struct ts2phc_master {
|
||||
void (*destroy)(struct ts2phc_master *ts2phc_master);
|
||||
int (*getppstime)(struct ts2phc_master *master, struct timespec *ts);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,426 @@
|
|||
/**
|
||||
* @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>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/queue.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "config.h"
|
||||
#include "clockadj.h"
|
||||
#include "missing.h"
|
||||
#include "phc.h"
|
||||
#include "print.h"
|
||||
#include "servo.h"
|
||||
#include "ts2phc_master.h"
|
||||
#include "ts2phc_slave.h"
|
||||
#include "util.h"
|
||||
|
||||
#define NS_PER_SEC 1000000000LL
|
||||
#define SAMPLE_WEIGHT 1.0
|
||||
#define SERVO_SYNC_INTERVAL 1.0
|
||||
|
||||
struct ts2phc_slave {
|
||||
char *name;
|
||||
STAILQ_ENTRY(ts2phc_slave) list;
|
||||
struct ptp_pin_desc pin_desc;
|
||||
enum servo_state state;
|
||||
unsigned int polarity;
|
||||
int32_t correction;
|
||||
uint32_t ignore_lower;
|
||||
uint32_t ignore_upper;
|
||||
struct servo *servo;
|
||||
clockid_t clk;
|
||||
int no_adj;
|
||||
int fd;
|
||||
};
|
||||
|
||||
struct ts2phc_slave_array {
|
||||
struct ts2phc_slave **slave;
|
||||
struct pollfd *pfd;
|
||||
} polling_array;
|
||||
|
||||
struct ts2phc_source_timestamp {
|
||||
struct timespec ts;
|
||||
int valid;
|
||||
};
|
||||
|
||||
enum extts_result {
|
||||
EXTTS_ERROR = -1,
|
||||
EXTTS_OK = 0,
|
||||
EXTTS_IGNORE = 1,
|
||||
};
|
||||
|
||||
static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
|
||||
struct ts2phc_source_timestamp ts,
|
||||
int64_t *offset,
|
||||
uint64_t *local_ts);
|
||||
|
||||
static STAILQ_HEAD(slave_ifaces_head, ts2phc_slave) ts2phc_slaves =
|
||||
STAILQ_HEAD_INITIALIZER(ts2phc_slaves);
|
||||
|
||||
static unsigned int ts2phc_n_slaves;
|
||||
|
||||
static int ts2phc_slave_array_create(void)
|
||||
{
|
||||
struct ts2phc_slave *slave;
|
||||
unsigned int i;
|
||||
|
||||
if (polling_array.slave) {
|
||||
return 0;
|
||||
}
|
||||
polling_array.slave = malloc(ts2phc_n_slaves * sizeof(*polling_array.slave));
|
||||
if (!polling_array.slave) {
|
||||
pr_err("low memory");
|
||||
return -1;
|
||||
}
|
||||
polling_array.pfd = malloc(ts2phc_n_slaves * sizeof(*polling_array.pfd));
|
||||
if (!polling_array.pfd) {
|
||||
pr_err("low memory");
|
||||
free(polling_array.slave);
|
||||
polling_array.slave = NULL;
|
||||
return -1;
|
||||
}
|
||||
i = 0;
|
||||
STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
|
||||
polling_array.slave[i] = slave;
|
||||
i++;
|
||||
}
|
||||
for (i = 0; i < ts2phc_n_slaves; i++) {
|
||||
polling_array.pfd[i].events = POLLIN | POLLPRI;
|
||||
polling_array.pfd[i].fd = polling_array.slave[i]->fd;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void ts2phc_slave_array_destroy(void)
|
||||
{
|
||||
free(polling_array.slave);
|
||||
free(polling_array.pfd);
|
||||
polling_array.slave = NULL;
|
||||
polling_array.pfd = NULL;
|
||||
}
|
||||
|
||||
static int ts2phc_slave_clear_fifo(struct ts2phc_slave *slave)
|
||||
{
|
||||
struct pollfd pfd = {
|
||||
.events = POLLIN | POLLPRI,
|
||||
.fd = slave->fd,
|
||||
};
|
||||
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;
|
||||
}
|
||||
|
||||
static struct ts2phc_slave *ts2phc_slave_create(struct config *cfg, const char *device)
|
||||
{
|
||||
enum servo_type servo = config_get_int(cfg, NULL, "clock_servo");
|
||||
int err, fadj, junk, max_adj, pulsewidth;
|
||||
struct ptp_extts_request extts;
|
||||
struct ts2phc_slave *slave;
|
||||
|
||||
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;
|
||||
}
|
||||
slave->pin_desc.index = config_get_int(cfg, device, "ts2phc.pin_index");
|
||||
slave->pin_desc.func = PTP_PF_EXTTS;
|
||||
slave->pin_desc.chan = config_get_int(cfg, device, "ts2phc.channel");
|
||||
slave->polarity = config_get_int(cfg, device, "ts2phc.extts_polarity");
|
||||
slave->correction = config_get_int(cfg, device, "ts2phc.extts_correction");
|
||||
|
||||
pulsewidth = config_get_int(cfg, device, "ts2phc.pulsewidth");
|
||||
pulsewidth /= 2;
|
||||
slave->ignore_upper = 1000000000 - pulsewidth;
|
||||
slave->ignore_lower = pulsewidth;
|
||||
|
||||
slave->clk = posix_clock_open(device, &junk);
|
||||
if (slave->clk == CLOCK_INVALID) {
|
||||
pr_err("failed to open clock");
|
||||
goto no_posix_clock;
|
||||
}
|
||||
slave->no_adj = config_get_int(cfg, NULL, "free_running");
|
||||
slave->fd = CLOCKID_TO_FD(slave->clk);
|
||||
|
||||
pr_debug("PHC slave %s has ptp index %d", device, junk);
|
||||
|
||||
fadj = (int) clockadj_get_freq(slave->clk);
|
||||
/* Due to a bug in older kernels, the reading may silently fail
|
||||
and return 0. Set the frequency back to make sure fadj is
|
||||
the actual frequency of the clock. */
|
||||
clockadj_set_freq(slave->clk, fadj);
|
||||
|
||||
max_adj = phc_max_adj(slave->clk);
|
||||
|
||||
slave->servo = servo_create(cfg, servo, -fadj, max_adj, 0);
|
||||
if (!slave->servo) {
|
||||
pr_err("failed to create servo");
|
||||
goto no_servo;
|
||||
}
|
||||
servo_sync_interval(slave->servo, SERVO_SYNC_INTERVAL);
|
||||
|
||||
if (phc_number_pins(slave->clk) > 0) {
|
||||
err = phc_pin_setfunc(slave->clk, &slave->pin_desc);
|
||||
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;
|
||||
if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
|
||||
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
||||
}
|
||||
if (ts2phc_slave_clear_fifo(slave)) {
|
||||
goto no_ext_ts;
|
||||
}
|
||||
|
||||
return slave;
|
||||
no_ext_ts:
|
||||
no_pin_func:
|
||||
servo_destroy(slave->servo);
|
||||
no_servo:
|
||||
close(slave->fd);
|
||||
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;
|
||||
if (ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts)) {
|
||||
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
||||
}
|
||||
posix_clock_close(slave->clk);
|
||||
free(slave->name);
|
||||
free(slave);
|
||||
}
|
||||
|
||||
static int ts2phc_slave_event(struct ts2phc_slave *slave,
|
||||
struct ts2phc_source_timestamp source_ts)
|
||||
{
|
||||
enum extts_result result;
|
||||
uint64_t extts_ts;
|
||||
int64_t offset;
|
||||
double adj;
|
||||
|
||||
result = ts2phc_slave_offset(slave, source_ts, &offset, &extts_ts);
|
||||
switch (result) {
|
||||
case EXTTS_ERROR:
|
||||
return -1;
|
||||
case EXTTS_OK:
|
||||
break;
|
||||
case EXTTS_IGNORE:
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (slave->no_adj) {
|
||||
pr_info("%s master offset %10" PRId64, slave->name, offset);
|
||||
return 0;
|
||||
}
|
||||
|
||||
adj = servo_sample(slave->servo, offset, extts_ts,
|
||||
SAMPLE_WEIGHT, &slave->state);
|
||||
|
||||
pr_debug("%s master offset %10" PRId64 " s%d freq %+7.0f",
|
||||
slave->name, offset, slave->state, adj);
|
||||
|
||||
switch (slave->state) {
|
||||
case SERVO_UNLOCKED:
|
||||
break;
|
||||
case SERVO_JUMP:
|
||||
clockadj_set_freq(slave->clk, -adj);
|
||||
clockadj_step(slave->clk, -offset);
|
||||
break;
|
||||
case SERVO_LOCKED:
|
||||
case SERVO_LOCKED_STABLE:
|
||||
clockadj_set_freq(slave->clk, -adj);
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static enum extts_result ts2phc_slave_offset(struct ts2phc_slave *slave,
|
||||
struct ts2phc_source_timestamp src,
|
||||
int64_t *offset,
|
||||
uint64_t *local_ts)
|
||||
{
|
||||
struct timespec source_ts = src.ts;
|
||||
struct ptp_extts_event event;
|
||||
uint64_t event_ns, source_ns;
|
||||
int cnt;
|
||||
|
||||
cnt = read(slave->fd, &event, sizeof(event));
|
||||
if (cnt != sizeof(event)) {
|
||||
pr_err("read extts event failed: %m");
|
||||
return EXTTS_ERROR;
|
||||
}
|
||||
if (event.index != slave->pin_desc.chan) {
|
||||
pr_err("extts on unexpected channel");
|
||||
return EXTTS_ERROR;
|
||||
}
|
||||
event_ns = event.t.sec * NS_PER_SEC;
|
||||
event_ns += event.t.nsec;
|
||||
|
||||
if (slave->polarity == (PTP_RISING_EDGE | PTP_FALLING_EDGE) &&
|
||||
source_ts.tv_nsec > slave->ignore_lower &&
|
||||
source_ts.tv_nsec < slave->ignore_upper) {
|
||||
|
||||
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);
|
||||
|
||||
return EXTTS_IGNORE;
|
||||
}
|
||||
if (source_ts.tv_nsec > 500000000) {
|
||||
source_ts.tv_sec++;
|
||||
}
|
||||
source_ns = source_ts.tv_sec * NS_PER_SEC;
|
||||
*offset = event_ns + slave->correction - source_ns;
|
||||
*local_ts = event_ns + slave->correction;
|
||||
|
||||
pr_debug("%s extts index %u at %lld.%09u corr %d src %" PRIi64
|
||||
".%ld diff %" PRId64,
|
||||
slave->name, event.index, event.t.sec, event.t.nsec,
|
||||
slave->correction,
|
||||
(int64_t) source_ts.tv_sec, source_ts.tv_nsec, *offset);
|
||||
|
||||
return EXTTS_OK;
|
||||
}
|
||||
|
||||
/* public methods */
|
||||
|
||||
int ts2phc_slave_add(struct config *cfg, const char *name)
|
||||
{
|
||||
struct ts2phc_slave *slave;
|
||||
|
||||
/* Create each interface only once. */
|
||||
STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
|
||||
if (0 == strcmp(name, slave->name)) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
slave = ts2phc_slave_create(cfg, name);
|
||||
if (!slave) {
|
||||
pr_err("failed to create slave");
|
||||
return -1;
|
||||
}
|
||||
STAILQ_INSERT_TAIL(&ts2phc_slaves, slave, list);
|
||||
ts2phc_n_slaves++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ts2phc_slave_arm(void)
|
||||
{
|
||||
struct ptp_extts_request extts;
|
||||
struct ts2phc_slave *slave;
|
||||
int err;
|
||||
|
||||
memset(&extts, 0, sizeof(extts));
|
||||
|
||||
STAILQ_FOREACH(slave, &ts2phc_slaves, list) {
|
||||
extts.index = slave->pin_desc.chan;
|
||||
extts.flags = slave->polarity | PTP_ENABLE_FEATURE;
|
||||
err = ioctl(slave->fd, PTP_EXTTS_REQUEST2, &extts);
|
||||
if (err < 0) {
|
||||
pr_err(PTP_EXTTS_REQUEST_FAILED);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ts2phc_slave_cleanup(void)
|
||||
{
|
||||
struct ts2phc_slave *slave;
|
||||
|
||||
ts2phc_slave_array_destroy();
|
||||
|
||||
while ((slave = STAILQ_FIRST(&ts2phc_slaves))) {
|
||||
STAILQ_REMOVE_HEAD(&ts2phc_slaves, list);
|
||||
ts2phc_slave_destroy(slave);
|
||||
ts2phc_n_slaves--;
|
||||
}
|
||||
}
|
||||
|
||||
int ts2phc_slave_poll(struct ts2phc_master *master)
|
||||
{
|
||||
struct ts2phc_source_timestamp source_ts;
|
||||
unsigned int i;
|
||||
int cnt;
|
||||
|
||||
if (ts2phc_slave_array_create()) {
|
||||
return -1;
|
||||
}
|
||||
cnt = poll(polling_array.pfd, ts2phc_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");
|
||||
return 0;
|
||||
}
|
||||
|
||||
source_ts.valid = ts2phc_master_getppstime(master, &source_ts.ts);
|
||||
|
||||
for (i = 0; i < ts2phc_n_slaves; i++) {
|
||||
if (polling_array.pfd[i].revents & (POLLIN|POLLPRI)) {
|
||||
ts2phc_slave_event(polling_array.slave[i], source_ts);
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
/**
|
||||
* @file ts2phc_slave.h
|
||||
* @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+
|
||||
*/
|
||||
#ifndef HAVE_TS2PHC_SLAVE_H
|
||||
#define HAVE_TS2PHC_SLAVE_H
|
||||
|
||||
#include "ts2phc_master.h"
|
||||
|
||||
int ts2phc_slave_add(struct config *cfg, const char *name);
|
||||
|
||||
int ts2phc_slave_arm(void);
|
||||
|
||||
void ts2phc_slave_cleanup(void);
|
||||
|
||||
int ts2phc_slave_poll(struct ts2phc_master *master);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue