/**

 * @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_unicast(msg)) {

		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;

	struct ptp_message *msg;

	struct tlv_extra *extra;

	Integer64 asymmetry;

	struct address dst;

	int cnt, err;

	if (str2addr(type, target, &dst)) {

		return -1;

	}

	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[])

{

	int batch_mode = 0, c, cnt, err = 0, index, length, tmo = -1;

	char *cmd = NULL, *config = NULL, line[1024], *progname;

	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;

	}

	if (optind < argc) {

		batch_mode = 1;

	}

	pollfd[0].fd = nsm->fda.fd[0];

	pollfd[1].fd = nsm->fda.fd[1];

	pollfd[2].fd = batch_mode ? -1 : STDIN_FILENO;

	pollfd[0].events = POLLIN | POLLPRI;

	pollfd[1].events = POLLIN | POLLPRI;

	pollfd[2].events = batch_mode ? 0 : POLLIN | POLLPRI;

	while (is_running()) {

		if (batch_mode) {

			if (optind < argc && !nsm->nsm_delay_req) {

				cmd = argv[optind++];

				if (nsm_command(nsm, cmd)) {

					pr_err("command failed");

					continue;

				}

			}

			/* Wait a bit for any outstanding replies. */

			tmo = 100;

		}

		cnt = poll(pollfd, NSM_NFD, tmo);

		if (cnt < 0) {

			if (EINTR == errno) {

				continue;

			} else {

				pr_emerg("poll failed");

				err = -1;

				break;

			}

		} else if (!cnt && optind < argc) {

			/* For batch mode. No response received from target node,

			 * continue with next command. */

			nsm_reset(nsm);

			continue;

		} 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;

}