tlv: Introduce the NetSync Monitor TLVs.

This patch adds support for packing and unpacking the NSM TLVs.  In
addition, it introduces macros to make the ntoh/htoh boilerplate easier
to read.  The idea is to reduce the number of monstrous muti-line
assignments like:

	pds->grandmasterClockQuality.offsetScaledLogVariance =
		htons(pds->grandmasterClockQuality.offsetScaledLogVariance);

Signed-off-by: Richard Cochran <richardcochran@gmail.com>

tlv.c

@@ -25,6 +25,11 @@
 #include "tlv.h"
 #include "msg.h"
 
+#define HTONS(x) (x) = htons(x)
+#define HTONL(x) (x) = htonl(x)
+#define NTOHS(x) (x) = ntohs(x)
+#define NTOHL(x) (x) = ntohl(x)
+
 #define TLV_LENGTH_INVALID(tlv, type) \
 	(tlv->length < sizeof(struct type) - sizeof(struct TLV))
 
@@ -56,6 +61,24 @@ static uint16_t flip16(uint16_t *p)
 	return v;
 }
 
+static int64_t host2net64_unaligned(int64_t *p)
+{
+	int64_t v;
+	memcpy(&v, p, sizeof(v));
+	v = host2net64(v);
+	memcpy(p, &v, sizeof(v));
+	return v;
+}
+
+static int64_t net2host64_unaligned(int64_t *p)
+{
+	int64_t v;
+	memcpy(&v, p, sizeof(v));
+	v = net2host64(v);
+	memcpy(p, &v, sizeof(v));
+	return v;
+}
+
 static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 			 struct tlv_extra *extra)
 {
@@ -371,6 +394,111 @@ static void mgt_pre_send(struct management_tlv *m, struct tlv_extra *extra)
 	}
 }
 
+static int nsm_resp_post_recv(struct tlv_extra *extra)
+{
+	struct nsm_resp_tlv_head *head;
+	struct TLV *tlv = extra->tlv;
+	struct timePropertiesDS *tp;
+	struct PortAddress *paddr;
+	struct currentDS *cds;
+	struct parentDS *pds;
+	unsigned char *ptr;
+	uint16_t expected;
+
+	if (tlv->length < sizeof(*head) + sizeof(*extra->foot)
+	    - sizeof(head->type) - sizeof(head->length)) {
+		return -EBADMSG;
+	}
+	head = (struct nsm_resp_tlv_head *) tlv;
+	paddr = &head->parent_addr;
+	NTOHS(paddr->networkProtocol);
+	NTOHS(paddr->addressLength);
+
+	switch (paddr->networkProtocol) {
+	case TRANS_UDP_IPV4:
+		expected = 4;
+		break;
+	case TRANS_UDP_IPV6:
+		expected = 16;
+		break;
+	case TRANS_IEEE_802_3:
+		expected = 6;
+		break;
+	default:
+		return -EBADMSG;
+	}
+	if (paddr->addressLength != expected) {
+		return -EBADMSG;
+	}
+	if (tlv->length != sizeof(*head) + sizeof(*extra->foot) +
+	    paddr->addressLength - sizeof(head->type) - sizeof(head->length)) {
+		return -EBADMSG;
+	}
+
+	ptr = (unsigned char *) tlv;
+	ptr += sizeof(*head) + paddr->addressLength;
+	extra->foot = (struct nsm_resp_tlv_foot *) ptr;
+
+	pds = &extra->foot->parent;
+	cds = &extra->foot->current;
+	tp = &extra->foot->timeprop;
+
+	/*
+	 * At this point the alignment only 2 bytes worst case.
+	 * So we need to be careful with the 64 bit words.
+	 */
+	NTOHS(pds->parentPortIdentity.portNumber);
+	NTOHS(pds->observedParentOffsetScaledLogVariance);
+	NTOHL(pds->observedParentClockPhaseChangeRate);
+	NTOHS(pds->grandmasterClockQuality.offsetScaledLogVariance);
+
+	NTOHS(cds->stepsRemoved);
+	net2host64_unaligned(&cds->offsetFromMaster);
+	net2host64_unaligned(&cds->meanPathDelay);
+
+	NTOHS(tp->currentUtcOffset);
+
+	NTOHL(extra->foot->lastsync.seconds_lsb);
+	NTOHS(extra->foot->lastsync.seconds_msb);
+	NTOHL(extra->foot->lastsync.nanoseconds);
+
+	return 0;
+}
+
+static void nsm_resp_pre_send(struct tlv_extra *extra)
+{
+	struct nsm_resp_tlv_head *head;
+	struct timePropertiesDS *tp;
+	struct PortAddress *paddr;
+	struct currentDS *cds;
+	struct parentDS *pds;
+
+	head = (struct nsm_resp_tlv_head *) extra->tlv;
+	paddr = &head->parent_addr;
+
+	pds = &extra->foot->parent;
+	cds = &extra->foot->current;
+	tp = &extra->foot->timeprop;
+
+	NTOHS(paddr->networkProtocol);
+	NTOHS(paddr->addressLength);
+
+	HTONS(pds->parentPortIdentity.portNumber);
+	HTONS(pds->observedParentOffsetScaledLogVariance);
+	HTONL(pds->observedParentClockPhaseChangeRate);
+	HTONS(pds->grandmasterClockQuality.offsetScaledLogVariance);
+
+	HTONS(cds->stepsRemoved);
+	host2net64_unaligned(&cds->offsetFromMaster);
+	host2net64_unaligned(&cds->meanPathDelay);
+
+	HTONS(tp->currentUtcOffset);
+
+	HTONL(extra->foot->lastsync.seconds_lsb);
+	HTONS(extra->foot->lastsync.seconds_msb);
+	HTONL(extra->foot->lastsync.nanoseconds);
+}
+
 static int org_post_recv(struct organization_tlv *org)
 {
 	struct follow_up_info_tlv *f;
@@ -489,6 +617,11 @@ int tlv_post_recv(struct tlv_extra *extra)
 	case TLV_AUTHENTICATION_CHALLENGE:
 	case TLV_SECURITY_ASSOCIATION_UPDATE:
 	case TLV_CUM_FREQ_SCALE_FACTOR_OFFSET:
+	case TLV_PTPMON_REQ:
+		break;
+	case TLV_PTPMON_RESP:
+		result = nsm_resp_post_recv(extra);
+		break;
 	default:
 		break;
 	}
@@ -527,6 +660,11 @@ void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra)
 	case TLV_AUTHENTICATION_CHALLENGE:
 	case TLV_SECURITY_ASSOCIATION_UPDATE:
 	case TLV_CUM_FREQ_SCALE_FACTOR_OFFSET:
+	case TLV_PTPMON_REQ:
+		break;
+	case TLV_PTPMON_RESP:
+		nsm_resp_pre_send(extra);
+		break;
 	default:
 		break;
 	}

tlv.h

@@ -39,6 +39,8 @@
 #define TLV_AUTHENTICATION_CHALLENGE			0x2001
 #define TLV_SECURITY_ASSOCIATION_UPDATE			0x2002
 #define TLV_CUM_FREQ_SCALE_FACTOR_OFFSET		0x2003
+#define TLV_PTPMON_REQ					0x21FE
+#define TLV_PTPMON_RESP					0x21FF
 
 enum management_action {
 	GET,
@@ -134,6 +136,21 @@ struct management_error_status {
 	Octet         data[0];
 } PACKED;
 
+struct nsm_resp_tlv_head {
+	Enumeration16           type;
+	UInteger16              length;
+	uint8_t                 port_state;
+	uint8_t                 reserved;
+	struct PortAddress      parent_addr;
+} PACKED;
+
+struct nsm_resp_tlv_foot {
+	struct parentDS         parent;
+	struct currentDS        current;
+	struct timePropertiesDS timeprop;
+	struct Timestamp        lastsync;
+} PACKED;
+
 /* Organizationally Unique Identifiers */
 #define IEEE_802_1_COMMITTEE 0x00, 0x80, 0xC2
 extern uint8_t ieee8021_id[3];
@@ -234,6 +251,7 @@ struct tlv_extra {
 	struct TLV *tlv;
 	union {
 		struct mgmt_clock_description cd;
+		struct nsm_resp_tlv_foot *foot;
 	};
 };