[Linuxptp-devel] [PATCH RFC V1 18/18] pmc: Implement the NetSync Monitor query.

[Richard C. <ric...@gm...>]

This patch adds a new "NSM" command to the pmc 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 <ric...@gm...>
---
 pmc.c        |  28 +++++-
 pmc_common.c | 324 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 pmc_common.h |  20 ++++
 3 files changed, 363 insertions(+), 9 deletions(-)

diff --git a/pmc.c b/pmc.c
index a15e800..c75b590 100644
--- a/pmc.c
+++ b/pmc.c
@@ -36,6 +36,7 @@
 #include "version.h"
 
 #define BAD_ACTION   -1
+#define NSM_ACTION   -2
 #define BAD_ID       -1
 #define AMBIGUOUS_ID -2
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
@@ -117,8 +118,6 @@ static const char *action_string[] = {
 	"ACKNOWLEDGE",
 };
 
-#define IFMT "\n\t\t"
-
 static char *text2str(struct PTPText *text)
 {
 	static struct static_ptp_text s;
@@ -561,6 +560,8 @@ static int parse_action(char *s)
 		return COMMAND;
 	else if (0 == strncasecmp(s, "COMMAND", len))
 		return COMMAND;
+	else if (0 == strncasecmp(s, "NSM", len))
+		return NSM_ACTION;
 	return BAD_ACTION;
 }
 
@@ -613,6 +614,11 @@ static void print_help(FILE *fp)
 	fprintf(fp, "\tTARGET [portIdentity]\n");
 	fprintf(fp, "\tTARGET *\n");
 	fprintf(fp, "\n");
+	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 do_command(char *str)
@@ -632,6 +638,9 @@ static int do_command(char *str)
 		return parse_target(id_str);
 
 	action = parse_action(action_str);
+	if (action == NSM_ACTION) {
+		return pmc_nsm_request(pmc, id_str);
+	}
 	id = parse_id(id_str);
 
 	if (action == BAD_ACTION || id == BAD_ID)
@@ -688,7 +697,7 @@ int main(int argc, char *argv[])
 	UInteger8 boundary_hops = 1, domain_number = 0, transport_specific = 0;
 	struct ptp_message *msg;
 	struct config *cfg;
-#define N_FD 2
+#define N_FD 3
 	struct pollfd pollfd[N_FD];
 
 	handle_term_signals();
@@ -794,6 +803,7 @@ int main(int argc, char *argv[])
 
 	pollfd[0].fd = batch_mode ? -1 : STDIN_FILENO;
 	pollfd[1].fd = pmc_get_transport_fd(pmc);
+	pollfd[2].fd = pmc_get_transport_event_fd(pmc);
 
 	while (is_running()) {
 		if (batch_mode && !command) {
@@ -808,6 +818,7 @@ int main(int argc, char *argv[])
 
 		pollfd[0].events = 0;
 		pollfd[1].events = POLLIN | POLLPRI;
+		pollfd[2].events = POLLIN | POLLPRI;
 
 		if (!batch_mode && !command)
 			pollfd[0].events |= POLLIN | POLLPRI;
@@ -856,7 +867,16 @@ int main(int argc, char *argv[])
 		if (pollfd[1].revents & (POLLIN|POLLPRI)) {
 			msg = pmc_recv(pmc);
 			if (msg) {
-				pmc_show(msg, stdout);
+				if (!pmc_handle_nsm(pmc, msg, stdout)) {
+					pmc_show(msg, stdout);
+				}
+				msg_put(msg);
+			}
+		}
+		if (pollfd[2].revents & (POLLIN|POLLPRI)) {
+			msg = pmc_recv_event(pmc);
+			if (msg) {
+				pmc_handle_nsm(pmc, msg, stdout);
 				msg_put(msg);
 			}
 		}
diff --git a/pmc_common.c b/pmc_common.c
index da25c21..6366f76 100644
--- a/pmc_common.c
+++ b/pmc_common.c
@@ -17,9 +17,13 @@
  * with this program; if not, write to the Free Software Foundation, Inc.,
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
+#include <arpa/inet.h>
 #include <errno.h>
-#include <string.h>
+#include <netinet/in.h>
+#include <stdint.h>
 #include <stdlib.h>
+#include <string.h>
+#include <sys/socket.h>
 
 #include "print.h"
 #include "tlv.h"
@@ -60,8 +64,75 @@ struct pmc {
 	struct fdarray fdarray;
 	struct tsproc *tsproc;
 	int zero_length_gets;
+
+	/* NetSync Monitor */
+	struct ptp_message *nsm_delay_req;
+	struct ptp_message *nsm_delay_resp;
+	struct ptp_message *nsm_sync;
+	struct ptp_message *nsm_fup;
+	enum timestamp_type timestamping;
+	Integer64 asymmetry;
 };
 
+static int nsm_complete(struct pmc *pmc)
+{
+	if (!pmc->nsm_sync) {
+		return 0;
+	}
+	if (one_step(pmc->nsm_sync)) {
+		return pmc->nsm_delay_resp ? 1 : 0;
+	}
+	return (pmc->nsm_delay_resp && pmc->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_reset(struct pmc *pmc)
+{
+	if (pmc->nsm_delay_req) {
+		msg_put(pmc->nsm_delay_req);
+	}
+	if (pmc->nsm_delay_resp) {
+		msg_put(pmc->nsm_delay_resp);
+	}
+	if (pmc->nsm_sync) {
+		msg_put(pmc->nsm_sync);
+	}
+	if (pmc->nsm_fup) {
+		msg_put(pmc->nsm_fup);
+	}
+	pmc->nsm_delay_req = NULL;
+	pmc->nsm_delay_resp = NULL;
+	pmc->nsm_sync = NULL;
+	pmc->nsm_fup = NULL;
+}
+
 struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
 		       const char *iface_name, UInteger8 boundary_hops,
 		       UInteger8 domain_number, UInteger8 transport_specific,
@@ -107,6 +178,10 @@ struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
 	}
 	pmc->zero_length_gets = zero_datalen ? 1 : 0;
 
+	pmc->timestamping = config_get_int(cfg, NULL, "time_stamping");
+	pmc->asymmetry = config_get_int(cfg, iface_name, "delayAsymmetry");
+	pmc->asymmetry <<= 16;
+
 	return pmc;
 
 failed:
@@ -120,6 +195,7 @@ failed:
 
 void pmc_destroy(struct pmc *pmc)
 {
+	nsm_reset(pmc);
 	tsproc_destroy(pmc->tsproc);
 	transport_close(pmc->transport, &pmc->fdarray);
 	transport_destroy(pmc->transport);
@@ -238,6 +314,228 @@ int pmc_get_transport_event_fd(struct pmc *pmc)
 	return pmc->fdarray.fd[FD_EVENT];
 }
 
+int pmc_handle_nsm(struct pmc *pmc, 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 (!pmc->nsm_delay_req) {
+		return 0;
+	}
+	if (msg->header.sequenceId !=
+	    ntohs(pmc->nsm_delay_req->header.sequenceId)) {
+		return 0;
+	}
+	if (!(msg->header.flagField[0] & UNICAST)) {
+		return 0;
+	}
+
+	switch (msg_type(msg)) {
+	case SYNC:
+		if (!pmc->nsm_sync) {
+			pmc->nsm_sync = msg;
+			msg_get(msg);
+		}
+		break;
+	case FOLLOW_UP:
+		if (!pmc->nsm_fup) {
+			pmc->nsm_fup = msg;
+			msg_get(msg);
+		}
+		break;
+	case DELAY_RESP:
+		if (!pmc->nsm_delay_resp) {
+			pmc->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 0;
+	}
+
+	if (!nsm_complete(pmc)) {
+		return 1;
+	}
+
+	head = (struct nsm_resp_tlv_head *) pmc->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(pmc->tsproc, pmc->nsm_sync, pmc->nsm_fup,
+				    pmc->nsm_delay_req, pmc->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(pmc);
+
+	return 1;
+}
+
+int pmc_nsm_request(struct pmc *pmc, char *target)
+{
+	enum transport_type type = transport_type(pmc->transport);
+	unsigned char mac[MAC_LEN];
+	struct in_addr ipv4_addr;
+	struct ptp_message *msg;
+	int cnt, err, pdulen;
+	struct address dst;
+	struct TLV *tlv;
+
+	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;
+	}
+	pdulen = sizeof(struct delay_req_msg) + sizeof(*tlv);
+	tlv = (struct TLV *) msg->delay_req.suffix;
+	tlv->type = TLV_PTPMON_REQ;
+	tlv->length = 0;
+	msg->tlv_count = 1;
+	msg->hwts.type = pmc->timestamping;
+
+	msg->header.tsmt               = DELAY_REQ | pmc->transport_specific;
+	msg->header.ver                = PTP_VERSION;
+	msg->header.messageLength      = pdulen;
+	msg->header.domainNumber       = pmc->domain_number;
+	msg->header.correction         = -pmc->asymmetry;
+	msg->header.sourcePortIdentity = pmc->port_identity;
+	msg->header.sequenceId         = pmc->sequence_id++;
+	msg->header.control            = CTL_DELAY_REQ;
+	msg->header.logMessageInterval = 0x7f;
+
+	msg->address = dst;
+	msg->header.flagField[0] |= UNICAST;
+
+	err = msg_pre_send(msg);
+	if (err) {
+		pr_err("msg_pre_send failed");
+		goto out;
+	}
+	cnt = transport_sendto(pmc->transport, &pmc->fdarray, 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(pmc);
+	pmc->nsm_delay_req = msg;
+	return 0;
+out:
+	msg_put(msg);
+	return err;
+}
+
 int pmc_send_get_action(struct pmc *pmc, int id)
 {
 	int datalen, pdulen;
@@ -301,18 +599,24 @@ int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize)
 	return 0;
 }
 
-struct ptp_message *pmc_recv(struct pmc *pmc)
+static struct ptp_message *pmc_recv_internal(struct pmc *pmc, int event)
 {
 	struct ptp_message *msg;
-	int cnt, err;
+	int cnt, err, fd;
 
 	msg = msg_allocate();
 	if (!msg) {
 		pr_err("low memory");
 		return NULL;
 	}
-	msg->hwts.type = TS_SOFTWARE;
-	cnt = transport_recv(pmc->transport, pmc_get_transport_fd(pmc), msg);
+	if (event) {
+		msg->hwts.type = pmc->timestamping;
+		fd = pmc_get_transport_event_fd(pmc);
+	} else {
+		msg->hwts.type = TS_SOFTWARE;
+		fd = pmc_get_transport_fd(pmc);
+	}
+	cnt = transport_recv(pmc->transport, fd, msg);
 	if (cnt <= 0) {
 		pr_err("recv message failed");
 		goto failed;
@@ -339,6 +643,16 @@ failed:
 	return NULL;
 }
 
+struct ptp_message *pmc_recv(struct pmc *pmc)
+{
+	return pmc_recv_internal(pmc, 0);
+}
+
+struct ptp_message *pmc_recv_event(struct pmc *pmc)
+{
+	return pmc_recv_internal(pmc, 1);
+}
+
 int pmc_target(struct pmc *pmc, struct PortIdentity *pid)
 {
 	pmc->target = *pid;
diff --git a/pmc_common.h b/pmc_common.h
index d5ccb29..f951d34 100644
--- a/pmc_common.h
+++ b/pmc_common.h
@@ -25,6 +25,8 @@
 #include "msg.h"
 #include "transport.h"
 
+#define IFMT "\n\t\t"
+
 struct pmc;
 
 struct pmc *pmc_create(struct config *cfg, enum transport_type transport_type,
@@ -37,11 +39,29 @@ void pmc_destroy(struct pmc *pmc);
 int pmc_get_transport_fd(struct pmc *pmc);
 int pmc_get_transport_event_fd(struct pmc *pmc);
 
+/**
+ * Handle a NetSync Monitor message.
+ * @param pmc  A pointer obtained with pmc_create().
+ * @param msg  A message which may or may not be a NetSync Monitor message.
+ * @param fp   An open file for logging.
+ * @return     One (1) if the message was handled, zero (0) otherwise.
+ */
+int pmc_handle_nsm(struct pmc *pmc, struct ptp_message *msg, FILE *fp);
+
+/**
+ * Sends a NetSync Monitor delay request to a given node.
+ * @param pmc     A pointer obtained with pmc_create().
+ * @param target  The address of the node to measure.
+ * @return        Zero on success, non-zero otherwise.
+ */
+int pmc_nsm_request(struct pmc *pmc, char *target);
+
 int pmc_send_get_action(struct pmc *pmc, int id);
 
 int pmc_send_set_action(struct pmc *pmc, int id, void *data, int datasize);
 
 struct ptp_message *pmc_recv(struct pmc *pmc);
+struct ptp_message *pmc_recv_event(struct pmc *pmc);
 
 int pmc_target(struct pmc *pmc, struct PortIdentity *pid);
 void pmc_target_port(struct pmc *pmc, UInteger16 portNumber);
-- 
2.11.0