From d3a519e26d2edd351bf4e6e16e24badaaf5f3646 Mon Sep 17 00:00:00 2001 From: Richard Cochran Date: Mon, 13 Apr 2020 18:57:16 -0700 Subject: [PATCH] Introduce a module for slave event monitoring. This patch adds a new module for slave event monitoring with its own configuration option, a UDS address. If the option is enabled, then the monitor will send events to the configured address. The default setting produces an inactive monitor that does nothing. Signed-off-by: Richard Cochran --- config.c | 1 + makefile | 6 +- monitor.c | 171 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ monitor.h | 22 +++++++ ptp4l.8 | 6 ++ 5 files changed, 203 insertions(+), 3 deletions(-) create mode 100644 monitor.c create mode 100644 monitor.h diff --git a/config.c b/config.c index 269183c..d3446b4 100644 --- a/config.c +++ b/config.c @@ -297,6 +297,7 @@ struct config_item config_tab[] = { GLOB_ITEM_INT("sanity_freq_limit", 200000000, 0, INT_MAX), GLOB_ITEM_INT("servo_num_offset_values", 10, 0, INT_MAX), GLOB_ITEM_INT("servo_offset_threshold", 0, 0, INT_MAX), + GLOB_ITEM_STR("slave_event_monitor", ""), GLOB_ITEM_INT("slaveOnly", 0, 0, 1), GLOB_ITEM_INT("socket_priority", 0, 0, 15), GLOB_ITEM_DBL("step_threshold", 0.0, 0.0, DBL_MAX), diff --git a/makefile b/makefile index acc94f7..27c4d78 100644 --- a/makefile +++ b/makefile @@ -29,9 +29,9 @@ TRANSP = raw.o transport.o udp.o udp6.o uds.o TS2PHC = ts2phc.o lstab.o nmea.o serial.o sock.o ts2phc_generic_master.o \ ts2phc_master.o ts2phc_phc_master.o ts2phc_nmea_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 \ - stats.o tc.o $(TRANSP) telecom.o tlv.o tsproc.o unicast_client.o \ + e2e_tc.o fault.o $(FILTERS) fsm.o hash.o interface.o monitor.o msg.o phc.o \ + port.o port_signaling.o pqueue.o print.o ptp4l.o p2p_tc.o rtnl.o $(SERVOS) \ + sk.o stats.o tc.o $(TRANSP) telecom.o tlv.o tsproc.o unicast_client.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 \ diff --git a/monitor.c b/monitor.c new file mode 100644 index 0000000..5706e5d --- /dev/null +++ b/monitor.c @@ -0,0 +1,171 @@ +/** + * @file monitor.c + * @note Copyright (C) 2020 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#include +#include + +#include "address.h" +#include "monitor.h" +#include "print.h" + +#define RECORDS_PER_MESSAGE 1 + +struct monitor_message { + struct ptp_message *msg; + int records_per_msg; + int count; +}; + +struct monitor { + struct port *dst_port; + struct slave_rx_sync_timing_data_tlv *sync_tlv; + struct monitor_message sync; +}; + +static bool monitor_active(struct monitor *monitor) +{ + return monitor->dst_port ? true : false; +} + +static int monitor_forward(struct port *port, struct ptp_message *msg) +{ + int err, pdulen = msg->header.messageLength; + + if (msg_pre_send(msg)) { + return -1; + } + err = port_forward_to(port, msg); + if (err) { + pr_debug("failed to send signaling message to slave event monitor: %s", + strerror(-err)); + } + if (msg_post_recv(msg, pdulen)) { + return -1; + } + msg->header.sequenceId++; + + return 0; +} + +static struct tlv_extra *monitor_init_message(struct monitor_message *mm, + struct port *destination, + uint16_t tlv_type, + size_t tlv_size, + struct address address) +{ + struct ptp_message *msg; + struct tlv_extra *extra; + + msg = port_signaling_construct(destination, &wildcard_pid); + if (!msg) { + return NULL; + } + extra = msg_tlv_append(msg, tlv_size); + if (!extra) { + msg_put(msg); + return NULL; + } + extra->tlv->type = tlv_type; + extra->tlv->length = tlv_size - sizeof(extra->tlv->type) - + sizeof(extra->tlv->length); + + mm->msg = msg; + mm->msg->address = address; + mm->records_per_msg = RECORDS_PER_MESSAGE; + mm->count = 0; + + return extra; +} + +static int monitor_init_sync(struct monitor *monitor, struct address address) +{ + const size_t tlv_size = sizeof(struct slave_rx_sync_timing_data_tlv) + + sizeof(struct slave_rx_sync_timing_record) * RECORDS_PER_MESSAGE; + struct tlv_extra *extra; + + extra = monitor_init_message(&monitor->sync, monitor->dst_port, + TLV_SLAVE_RX_SYNC_TIMING_DATA, tlv_size, + address); + if (!extra) { + return -1; + } + monitor->sync_tlv = (struct slave_rx_sync_timing_data_tlv *) extra->tlv; + + return 0; +} + +struct monitor *monitor_create(struct config *config, struct port *dst) +{ + struct monitor *monitor; + struct address address; + struct sockaddr_un sa; + const char *path; + + monitor = calloc(1, sizeof(*monitor)); + if (!monitor) { + return NULL; + } + path = config_get_string(config, NULL, "slave_event_monitor"); + if (!path || !path[0]) { + /* Return an inactive monitor. */ + return monitor; + } + memset(&sa, 0, sizeof(sa)); + sa.sun_family = AF_LOCAL; + snprintf(sa.sun_path, sizeof(sa.sun_path) - 1, "%s", path); + address.sun = sa; + address.len = sizeof(sa); + + monitor->dst_port = dst; + + if (monitor_init_sync(monitor, address)) { + free(monitor); + return NULL; + } + + return monitor; +} + +void monitor_destroy(struct monitor *monitor) +{ + if (monitor->sync.msg) { + msg_put(monitor->sync.msg); + } + free(monitor); +} + +int monitor_sync(struct monitor *monitor, struct PortIdentity source_pid, + uint16_t seqid, tmv_t t1, tmv_t corr, tmv_t t2) +{ + struct slave_rx_sync_timing_record *record; + struct ptp_message *msg; + + if (!monitor_active(monitor)) { + return 0; + } + + msg = monitor->sync.msg; + + if (!pid_eq(&monitor->sync_tlv->sourcePortIdentity, &source_pid)) { + /* There was a change in remote master. Drop stale records. */ + memcpy(&monitor->sync_tlv->sourcePortIdentity, &source_pid, + sizeof(monitor->sync_tlv->sourcePortIdentity)); + monitor->sync.count = 0; + } + + record = monitor->sync_tlv->record + monitor->sync.count; + record->sequenceId = seqid; + record->syncOriginTimestamp = tmv_to_Timestamp(t1); + record->totalCorrectionField = tmv_to_TimeInterval(corr); + record->scaledCumulativeRateOffset = 0; + record->syncEventIngressTimestamp = tmv_to_Timestamp(t2); + + monitor->sync.count++; + if (monitor->sync.count == monitor->sync.records_per_msg) { + monitor->sync.count = 0; + return monitor_forward(monitor->dst_port, msg); + } + return 0; +} diff --git a/monitor.h b/monitor.h new file mode 100644 index 0000000..e2a6ca3 --- /dev/null +++ b/monitor.h @@ -0,0 +1,22 @@ +/** + * @file monitor.h + * @note Copyright (C) 2020 Richard Cochran + * @note SPDX-License-Identifier: GPL-2.0+ + */ +#ifndef HAVE_MONITOR_H +#define HAVE_MONITOR_H + +#include "config.h" +#include "port.h" +#include "tmv.h" + +struct monitor; + +struct monitor *monitor_create(struct config *config, struct port *dst); + +void monitor_destroy(struct monitor *monitor); + +int monitor_sync(struct monitor *monitor, struct PortIdentity source_pid, + uint16_t seqid, tmv_t t1, tmv_t corr, tmv_t t2); + +#endif diff --git a/ptp4l.8 b/ptp4l.8 index 24ce10d..b179b81 100644 --- a/ptp4l.8 +++ b/ptp4l.8 @@ -764,6 +764,12 @@ to the SERVO_LOCKED_STABLE state. The transition occurs once the last 'servo_num_offset_values' offsets are all below the threshold value. The default value of offset_threshold is 0 (disabled). .TP +.B slave_event_monitor +Specifies the address of a UNIX domain socket for slave event +monitoring. A local client bound to this address will receive +SLAVE_RX_SYNC_TIMING_DATA and SLAVE_DELAY_TIMING_DATA_NP TLVs. +The default is the empty string (disabled). +.TP .B write_phase_mode This option enables using the "write phase" feature of a PTP Hardware Clock. If supported by the device, this mode uses the hardware's