This patch adds a new "NSM" program. The new code handles only one
outstanding NSM command at a time. If and when all four event time
stamps have arrived, the code prints the instantaneous estimated
offset without any averaging or smoothing.
Signed-off-by: Richard Cochran <richardcoch...@gmail.com>
---
.gitignore | 1 +
makefile | 7 +-
nsm.c | 627 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
3 files changed, 633 insertions(+), 2 deletions(-)
create mode 100644 nsm.c
diff --git a/.gitignore b/.gitignore
index 68a4c3e..6d288ae 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,7 @@
/*.o
/.version
/hwstamp_ctl
+/nsm
/phc2sys
/pmc
/ptp4l
diff --git a/makefile b/makefile
index f898336..796235b 100644
--- a/makefile
+++ b/makefile
@@ -22,13 +22,13 @@ CC = $(CROSS_COMPILE)gcc
VER = -DVER=$(version)
CFLAGS = -Wall $(VER) $(incdefs) $(DEBUG) $(EXTRA_CFLAGS)
LDLIBS = -lm -lrt $(EXTRA_LDFLAGS)
-PRG = ptp4l pmc phc2sys hwstamp_ctl phc_ctl timemaster
+PRG = ptp4l hwstamp_ctl nsm phc2sys phc_ctl pmc timemaster
OBJ = bmc.o clock.o clockadj.o clockcheck.o config.o fault.o \
filter.o fsm.o hash.o linreg.o mave.o mmedian.o msg.o ntpshm.o nullf.o phc.o \
pi.o port.o print.o ptp4l.o raw.o rtnl.o servo.o sk.o stats.o tlv.o \
transport.o tsproc.o udp.o udp6.o uds.o util.o version.o
-OBJECTS = $(OBJ) hwstamp_ctl.o phc2sys.o phc_ctl.o pmc.o pmc_common.o \
+OBJECTS = $(OBJ) hwstamp_ctl.o nsm.o phc2sys.o phc_ctl.o pmc.o
pmc_common.o \
sysoff.o timemaster.o
SRC = $(OBJECTS:.o=.c)
DEPEND = $(OBJECTS:.o=.d)
@@ -46,6 +46,9 @@ all: $(PRG)
ptp4l: $(OBJ)
+nsm: config.o filter.o hash.o mave.o mmedian.o msg.o nsm.o print.o raw.o \
+ rtnl.o sk.o transport.o tlv.o tsproc.o udp.o udp6.o uds.o util.o version.o
+
pmc: config.o hash.o msg.o pmc.o pmc_common.o print.o raw.o sk.o tlv.o \
transport.o udp.o udp6.o uds.o util.o version.o
diff --git a/nsm.c b/nsm.c
new file mode 100644
index 0000000..972a4ae
--- /dev/null
+++ b/nsm.c
@@ -0,0 +1,627 @@
+/**
+ * @file nsm.c
+ * @brief NSM client program
+ * @note Copyright (C) 2018 Richard Cochran <richardcoch...@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include <errno.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+#include <arpa/inet.h>
+
+#include "config.h"
+#include "print.h"
+#include "rtnl.h"
+#include "util.h"
+#include "version.h"
+
+#define IFMT "\n\t\t"
+#define NSM_NFD 3
+
+struct nsm {
+ struct config *cfg;
+ struct fdarray fda;
+ struct transport *trp;
+ struct tsproc *tsproc;
+ struct ptp_message *nsm_delay_req;
+ struct ptp_message *nsm_delay_resp;
+ struct ptp_message *nsm_sync;
+ struct ptp_message *nsm_fup;
+ struct PortIdentity port_identity;
+ UInteger16 sequence_id;
+ const char *name;
+} the_nsm;
+
+static void nsm_help(FILE *fp);
+static int nsm_request(struct nsm *nsm, char *target);
+static void nsm_reset(struct nsm *nsm);
+
+static int nsm_command(struct nsm *nsm, const char *cmd)
+{
+ char action_str[10+1] = {0}, id_str[64+1] = {0};
+
+ if (0 == strncasecmp(cmd, "HELP", strlen(cmd))) {
+ nsm_help(stdout);
+ return 0;
+ }
+ if (2 != sscanf(cmd, " %10s %64s", action_str, id_str)) {
+ pr_err("bad command: %s", cmd);
+ return -1;
+ }
+ if (0 == strncasecmp(action_str, "NSM", strlen(action_str))) {
+ return nsm_request(nsm, id_str);
+ }
+ pr_err("bad command: %s", cmd);
+ return -1;
+}
+
+static int nsm_complete(struct nsm *nsm)
+{
+ if (!nsm->nsm_sync) {
+ return 0;
+ }
+ if (one_step(nsm->nsm_sync)) {
+ return nsm->nsm_delay_resp ? 1 : 0;
+ }
+ return (nsm->nsm_delay_resp && nsm->nsm_fup) ? 1 : 0;
+}
+
+static int64_t nsm_compute_offset(struct tsproc *tsp,
+ struct ptp_message *syn,
+ struct ptp_message *fup,
+ struct ptp_message *req,
+ struct ptp_message *resp)
+{
+ tmv_t c1, c2, c3, t1, t1c, t2, t3, t4, t4c, offset;
+
+ c1 = correction_to_tmv(syn->header.correction);
+ c2 = correction_to_tmv(fup->header.correction);
+ c3 = correction_to_tmv(resp->header.correction);
+
+ t1 = timestamp_to_tmv(fup->ts.pdu);
+ t2 = timespec_to_tmv(syn->hwts.ts);
+ t3 = timespec_to_tmv(req->hwts.ts);
+ t4 = timestamp_to_tmv(resp->ts.pdu);
+
+ t1c = tmv_add(t1, tmv_add(c1, c2));
+ t4c = tmv_sub(t4, c3);
+
+ tsproc_reset(tsp, 1);
+ tsproc_down_ts(tsp, t1c, t2);
+ tsproc_up_ts(tsp, t3, t4c);
+ tsproc_update_offset(tsp, &offset, NULL);
+
+ return tmv_to_nanoseconds(offset);
+}
+
+static void nsm_close(struct nsm *nsm)
+{
+ nsm_reset(nsm);
+ transport_close(nsm->trp, &nsm->fda);
+ transport_destroy(nsm->trp);
+ tsproc_destroy(nsm->tsproc);
+}
+
+static void nsm_handle_msg(struct nsm *nsm, struct ptp_message *msg, FILE *fp)
+{
+ struct nsm_resp_tlv_head *head;
+ struct nsm_resp_tlv_foot *foot;
+ struct timePropertiesDS *tp;
+ struct PortAddress *paddr;
+ struct currentDS cds;
+ struct parentDS *pds;
+ struct Timestamp ts;
+ unsigned char *ptr;
+ int64_t offset;
+
+ if (!nsm->nsm_delay_req) {
+ return;
+ }
+ if (msg->header.sequenceId !=
+ ntohs(nsm->nsm_delay_req->header.sequenceId)) {
+ return;
+ }
+ if (!(msg->header.flagField[0] & UNICAST)) {
+ return;
+ }
+
+ switch (msg_type(msg)) {
+ case SYNC:
+ if (!nsm->nsm_sync) {
+ nsm->nsm_sync = msg;
+ msg_get(msg);
+ }
+ break;
+ case FOLLOW_UP:
+ if (!nsm->nsm_fup) {
+ nsm->nsm_fup = msg;
+ msg_get(msg);
+ }
+ break;
+ case DELAY_RESP:
+ if (!nsm->nsm_delay_resp) {
+ nsm->nsm_delay_resp = msg;
+ msg_get(msg);
+ }
+ break;
+ case DELAY_REQ:
+ case PDELAY_REQ:
+ case PDELAY_RESP:
+ case PDELAY_RESP_FOLLOW_UP:
+ case ANNOUNCE:
+ case SIGNALING:
+ case MANAGEMENT:
+ return;
+ }
+
+ if (!nsm_complete(nsm)) {
+ return;
+ }
+
+ head = (struct nsm_resp_tlv_head *)
nsm->nsm_delay_resp->delay_resp.suffix;
+ paddr = &head->parent_addr;
+
+ ptr = (unsigned char *) head;
+ ptr += sizeof(*head) + paddr->addressLength;
+ foot = (struct nsm_resp_tlv_foot *) ptr;
+
+ pds = &foot->parent;
+ memcpy(&cds, &foot->current, sizeof(cds));
+ tp = &foot->timeprop;
+ memcpy(&ts, &foot->lastsync, sizeof(ts));
+
+ offset = nsm_compute_offset(nsm->tsproc, nsm->nsm_sync, nsm->nsm_fup,
+ nsm->nsm_delay_req, nsm->nsm_delay_resp);
+
+ fprintf(fp, "NSM MEASUREMENT COMPLETE"
+ IFMT "offset %" PRId64
+ IFMT "portState %s"
+ IFMT "parentPortAddress %hu %s\n",
+ offset,
+ ps_str[head->port_state],
+ head->parent_addr.networkProtocol,
+ portaddr2str(&head->parent_addr));
+ fprintf(fp, "\tparentDataset"
+ IFMT "parentPortIdentity %s"
+ IFMT "parentStats %hhu"
+ IFMT "observedParentOffsetScaledLogVariance 0x%04hx"
+ IFMT "observedParentClockPhaseChangeRate 0x%08x"
+ IFMT "grandmasterPriority1 %hhu"
+ IFMT "gm.ClockClass %hhu"
+ IFMT "gm.ClockAccuracy 0x%02hhx"
+ IFMT "gm.OffsetScaledLogVariance 0x%04hx"
+ IFMT "grandmasterPriority2 %hhu"
+ IFMT "grandmasterIdentity %s\n",
+ pid2str(&pds->parentPortIdentity),
+ pds->parentStats,
+ pds->observedParentOffsetScaledLogVariance,
+ pds->observedParentClockPhaseChangeRate,
+ pds->grandmasterPriority1,
+ pds->grandmasterClockQuality.clockClass,
+ pds->grandmasterClockQuality.clockAccuracy,
+ pds->grandmasterClockQuality.offsetScaledLogVariance,
+ pds->grandmasterPriority2,
+ cid2str(&pds->grandmasterIdentity));
+ fprintf(fp, "\tcurrentDataset"
+ IFMT "stepsRemoved %hd"
+ IFMT "offsetFromMaster %.1f"
+ IFMT "meanPathDelay %.1f\n",
+ cds.stepsRemoved, cds.offsetFromMaster / 65536.0,
+ cds.meanPathDelay / 65536.0);
+ fprintf(fp, "\ttimePropertiesDataset"
+ IFMT "currentUtcOffset %hd"
+ IFMT "leap61 %d"
+ IFMT "leap59 %d"
+ IFMT "currentUtcOffsetValid %d"
+ IFMT "ptpTimescale %d"
+ IFMT "timeTraceable %d"
+ IFMT "frequencyTraceable %d"
+ IFMT "timeSource 0x%02hhx\n",
+ tp->currentUtcOffset,
+ tp->flags & LEAP_61 ? 1 : 0,
+ tp->flags & LEAP_59 ? 1 : 0,
+ tp->flags & UTC_OFF_VALID ? 1 : 0,
+ tp->flags & PTP_TIMESCALE ? 1 : 0,
+ tp->flags & TIME_TRACEABLE ? 1 : 0,
+ tp->flags & FREQ_TRACEABLE ? 1 : 0,
+ tp->timeSource);
+ fprintf(fp, "\tlastSyncTimestamp %" PRId64 ".%09u\n",
+ ((uint64_t)ts.seconds_lsb) | (((uint64_t)ts.seconds_msb) << 32),
+ ts.nanoseconds);
+
+ fflush(fp);
+ nsm_reset(nsm);
+}
+
+static void nsm_help(FILE *fp)
+{
+ fprintf(fp, "\tSend a NetSync Monitor request to a specific port
address:\n");
+ fprintf(fp, "\n");
+ fprintf(fp, "\tNSM 111.222.333.444\n");
+ fprintf(fp, "\tNSM aa:bb:cc:dd:ee:ff\n");
+ fprintf(fp, "\n");
+}
+
+static int nsm_open(struct nsm *nsm, struct config *cfg)
+{
+ enum transport_type transport;
+ struct interface *iface;
+ const char *name;
+ int count = 0;
+
+ STAILQ_FOREACH(iface, &cfg->interfaces, list) {
+ rtnl_get_ts_label(iface);
+ if (iface->ts_label[0] == '\0') {
+ strncpy(iface->ts_label, iface->name, MAX_IFNAME_SIZE);
+ }
+ count++;
+ }
+ if (count != 1) {
+ pr_err("need exactly one interface");
+ return -1;
+ }
+ iface = STAILQ_FIRST(&cfg->interfaces);
+ nsm->name = name = iface->name;
+ nsm->cfg = cfg;
+
+ transport = config_get_int(cfg, name, "network_transport");
+
+ if (generate_clock_identity(&nsm->port_identity.clockIdentity, name)) {
+ pr_err("failed to generate a clock identity");
+ return -1;
+ }
+ nsm->port_identity.portNumber = 1;
+
+ nsm->tsproc = tsproc_create(TSPROC_RAW, FILTER_MOVING_AVERAGE, 10);
+ if (!nsm->tsproc) {
+ pr_err("failed to create time stamp processor");
+ goto no_tsproc;
+ }
+ nsm->trp = transport_create(cfg, transport);
+ if (!nsm->trp) {
+ pr_err("failed to create transport");
+ goto no_trans;
+ }
+ if (transport_open(nsm->trp, iface, &nsm->fda,
+ config_get_int(cfg, NULL, "time_stamping"))) {
+ pr_err("failed to open transport");
+ goto open_failed;
+ }
+ return 0;
+
+open_failed:
+ transport_destroy(nsm->trp);
+no_trans:
+ tsproc_destroy(nsm->tsproc);
+no_tsproc:
+ return -1;
+}
+
+static struct ptp_message *nsm_recv(struct nsm *nsm, int fd)
+{
+ struct ptp_message *msg;
+ int cnt, err;
+
+ msg = msg_allocate();
+ if (!msg) {
+ pr_err("low memory");
+ return NULL;
+ }
+ msg->hwts.type = config_get_int(nsm->cfg, NULL, "time_stamping");
+
+ cnt = transport_recv(nsm->trp, fd, msg);
+ if (cnt <= 0) {
+ pr_err("recv message failed");
+ goto failed;
+ }
+ err = msg_post_recv(msg, cnt);
+ if (err) {
+ switch (err) {
+ case -EBADMSG:
+ pr_err("bad message");
+ break;
+ case -ETIME:
+ pr_err("received %s without timestamp",
+ msg_type_string(msg_type(msg)));
+ break;
+ case -EPROTO:
+ pr_debug("ignoring message");
+ break;
+ }
+ goto failed;
+ }
+
+ return msg;
+failed:
+ msg_put(msg);
+ return NULL;
+}
+
+static int nsm_request(struct nsm *nsm, char *target)
+{
+ enum transport_type type = transport_type(nsm->trp);
+ UInteger8 transportSpecific;
+ unsigned char mac[MAC_LEN];
+ struct in_addr ipv4_addr;
+ struct ptp_message *msg;
+ struct tlv_extra *extra;
+ Integer64 asymmetry;
+ struct address dst;
+ int cnt, err;
+
+ memset(&dst, 0, sizeof(dst));
+
+ switch (type) {
+ case TRANS_UDS:
+ case TRANS_UDP_IPV6:
+ case TRANS_DEVICENET:
+ case TRANS_CONTROLNET:
+ case TRANS_PROFINET:
+ pr_err("sorry, NSM not support with this transport");
+ return -1;
+ case TRANS_UDP_IPV4:
+ if (!inet_aton(target, &ipv4_addr)) {
+ pr_err("bad IPv4 address");
+ return -1;
+ }
+ dst.sin.sin_family = AF_INET;
+ dst.sin.sin_addr = ipv4_addr;
+ dst.len = sizeof(dst.sin);
+ break;
+ case TRANS_IEEE_802_3:
+ if (str2mac(target, mac)) {
+ pr_err("bad Layer-2 address");
+ return -1;
+ }
+ dst.sll.sll_family = AF_PACKET;
+ dst.sll.sll_halen = MAC_LEN;
+ memcpy(&dst.sll.sll_addr, mac, MAC_LEN);
+ dst.len = sizeof(dst.sll);
+ break;
+ }
+
+ msg = msg_allocate();
+ if (!msg) {
+ return -1;
+ }
+
+ transportSpecific = config_get_int(nsm->cfg, nsm->name,
"transportSpecific");
+ transportSpecific <<= 4;
+
+ asymmetry = config_get_int(nsm->cfg, nsm->name, "delayAsymmetry");
+ asymmetry <<= 16;
+
+ msg->hwts.type = config_get_int(nsm->cfg, NULL, "time_stamping");
+
+ msg->header.tsmt = DELAY_REQ | transportSpecific;
+ msg->header.ver = PTP_VERSION;
+ msg->header.messageLength = sizeof(struct delay_req_msg);
+ msg->header.domainNumber = config_get_int(nsm->cfg, NULL,
"domainNumber");
+ msg->header.correction = -asymmetry;
+ msg->header.sourcePortIdentity = nsm->port_identity;
+ msg->header.sequenceId = nsm->sequence_id++;
+ msg->header.control = CTL_DELAY_REQ;
+ msg->header.logMessageInterval = 0x7f;
+
+ msg->address = dst;
+ msg->header.flagField[0] |= UNICAST;
+
+ extra = msg_tlv_append(msg, sizeof(struct TLV));
+ if (!extra) {
+ msg_put(msg);
+ return -ENOMEM;
+ }
+ extra->tlv->type = TLV_PTPMON_REQ;
+ extra->tlv->length = 0;
+
+ err = msg_pre_send(msg);
+ if (err) {
+ pr_err("msg_pre_send failed");
+ goto out;
+ }
+ cnt = transport_sendto(nsm->trp, &nsm->fda, 1, msg);
+ if (cnt <= 0) {
+ pr_err("transport_sendto failed");
+ err = -1;
+ goto out;
+ }
+ if (msg_sots_missing(msg)) {
+ pr_err("missing timestamp on transmitted delay request");
+ err = -1;
+ goto out;
+ }
+ nsm_reset(nsm);
+ nsm->nsm_delay_req = msg;
+ return 0;
+out:
+ msg_put(msg);
+ return err;
+}
+
+static void nsm_reset(struct nsm *nsm)
+{
+ if (nsm->nsm_delay_req) {
+ msg_put(nsm->nsm_delay_req);
+ }
+ if (nsm->nsm_delay_resp) {
+ msg_put(nsm->nsm_delay_resp);
+ }
+ if (nsm->nsm_sync) {
+ msg_put(nsm->nsm_sync);
+ }
+ if (nsm->nsm_fup) {
+ msg_put(nsm->nsm_fup);
+ }
+ nsm->nsm_delay_req = NULL;
+ nsm->nsm_delay_resp = NULL;
+ nsm->nsm_sync = NULL;
+ nsm->nsm_fup = NULL;
+}
+
+static void usage(char *progname)
+{
+ fprintf(stderr,
+ "\nusage: %s [options]\n\n"
+ " -f [file] read configuration from 'file'\n"
+ " -h prints this message and exits\n"
+ " -i [dev] interface device to use\n"
+ " -v prints the software version and exits\n"
+ "\n",
+ progname);
+}
+
+int main(int argc, char *argv[])
+{
+ char *cmd = NULL, *config = NULL, line[1024], *progname;
+ int c, cnt, err = 0, index, length, tmo = -1;
+ struct pollfd pollfd[NSM_NFD];
+ struct nsm *nsm = &the_nsm;
+ struct ptp_message *msg;
+ struct option *opts;
+ struct config *cfg;
+
+ if (handle_term_signals()) {
+ return -1;
+ }
+ cfg = config_create();
+ if (!cfg) {
+ return -1;
+ }
+ opts = config_long_options(cfg);
+ print_set_verbose(1);
+ print_set_syslog(0);
+
+ /* Process the command line arguments. */
+ progname = strrchr(argv[0], '/');
+ progname = progname ? 1+progname : argv[0];
+ while (EOF != (c = getopt_long(argc, argv, "f:hi:v", opts, &index))) {
+ switch (c) {
+ case 0:
+ if (config_parse_option(cfg, opts[index].name, optarg))
{
+ config_destroy(cfg);
+ return -1;
+ }
+ break;
+ case 'f':
+ config = optarg;
+ break;
+ case 'i':
+ if (!config_create_interface(optarg, cfg)) {
+ config_destroy(cfg);
+ return -1;
+ }
+ break;
+ case 'v':
+ version_show(stdout);
+ config_destroy(cfg);
+ return 0;
+ case 'h':
+ usage(progname);
+ config_destroy(cfg);
+ return 0;
+ case '?':
+ default:
+ usage(progname);
+ config_destroy(cfg);
+ return -1;
+ }
+ }
+
+ print_set_syslog(0);
+ print_set_verbose(1);
+
+ if (config && (err = config_read(config, cfg))) {
+ goto out;
+ }
+
+ print_set_progname(progname);
+ print_set_tag(config_get_string(cfg, NULL, "message_tag"));
+ print_set_level(config_get_int(cfg, NULL, "logging_level"));
+
+ err = nsm_open(nsm, cfg);
+ if (err) {
+ goto out;
+ }
+ pollfd[0].fd = nsm->fda.fd[0];
+ pollfd[1].fd = nsm->fda.fd[1];
+ pollfd[2].fd = STDIN_FILENO;
+ pollfd[0].events = POLLIN | POLLPRI;
+ pollfd[1].events = POLLIN | POLLPRI;
+ pollfd[2].events = POLLIN | POLLPRI;
+
+ while (is_running()) {
+ cnt = poll(pollfd, NSM_NFD, tmo);
+ if (cnt < 0) {
+ if (EINTR == errno) {
+ continue;
+ } else {
+ pr_emerg("poll failed");
+ err = -1;
+ break;
+ }
+ } else if (!cnt) {
+ break;
+ }
+ if (pollfd[2].revents & POLLHUP) {
+ if (tmo == -1) {
+ /* Wait a bit longer for outstanding replies. */
+ tmo = 100;
+ pollfd[2].fd = -1;
+ pollfd[2].events = 0;
+ } else {
+ break;
+ }
+ }
+ if (pollfd[2].revents & (POLLIN|POLLPRI)) {
+ if (!fgets(line, sizeof(line), stdin)) {
+ break;
+ }
+ length = strlen(line);
+ if (length < 2) {
+ continue;
+ }
+ line[length - 1] = 0;
+ cmd = line;
+ if (nsm_command(nsm, cmd)) {
+ pr_err("command failed");
+ }
+ }
+ if (pollfd[0].revents & (POLLIN|POLLPRI)) {
+ msg = nsm_recv(nsm, pollfd[0].fd);
+ if (msg) {
+ nsm_handle_msg(nsm, msg, stdout);
+ msg_put(msg);
+ }
+ }
+ if (pollfd[1].revents & (POLLIN|POLLPRI)) {
+ msg = nsm_recv(nsm, pollfd[1].fd);
+ if (msg) {
+ nsm_handle_msg(nsm, msg, stdout);
+ msg_put(msg);
+ }
+ }
+ }
+
+ nsm_close(nsm);
+out:
+ msg_cleanup();
+ config_destroy(cfg);
+ return err;
+}
--
2.11.0
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
