linuxptp/nsm.c

/**
 * @file nsm.c
 * @brief NSM client program
 * @note Copyright (C) 2018 Richard Cochran <richardcochran@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 = syn->hwts.ts;
	t3 = 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_device(iface->name, iface->ts_label);
		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 -EPROTO:
			pr_debug("ignoring message");
			break;
		}
		goto failed;
	}
	if (msg_sots_missing(msg)) {
		pr_err("received %s without timestamp",
		       msg_type_string(msg_type(msg)));
		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, TRANS_EVENT, 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;
}