From 1d450540c75fda679a2948c0a8bc14d572326db8 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Sat, 31 Mar 2018 22:10:54 -0700
Subject: [PATCH] msg: Support unicast negotiation message formats.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 msg.c |  2 ++
 tlv.c | 88 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tlv.h | 36 ++++++++++++++++++++++++
 3 files changed, 126 insertions(+)

diff --git a/msg.c b/msg.c
index 090715b..6c20c97 100644
--- a/msg.c
+++ b/msg.c
@@ -417,6 +417,7 @@ int msg_post_recv(struct ptp_message *m, int cnt)
 		announce_post_recv(&m->announce);
 		break;
 	case SIGNALING:
+		port_id_post_recv(&m->signaling.targetPortIdentity);
 		break;
 	case MANAGEMENT:
 		port_id_post_recv(&m->management.targetPortIdentity);
@@ -467,6 +468,7 @@ int msg_pre_send(struct ptp_message *m)
 		announce_pre_send(&m->announce);
 		break;
 	case SIGNALING:
+		port_id_pre_send(&m->signaling.targetPortIdentity);
 		break;
 	case MANAGEMENT:
 		port_id_pre_send(&m->management.targetPortIdentity);
diff --git a/tlv.c b/tlv.c
index 7285af7..b647160 100644
--- a/tlv.c
+++ b/tlv.c
@@ -544,6 +544,91 @@ static void org_pre_send(struct organization_tlv *org)
 	}
 }
 
+static int unicast_message_type_valid(uint8_t message_type)
+{
+	message_type >>= 4;
+	switch (message_type) {
+	case ANNOUNCE:
+	case SYNC:
+	case DELAY_RESP:
+	case PDELAY_RESP:
+		return 1;
+	default:
+		return 0;
+	}
+}
+
+static int unicast_negotiation_post_recv(struct tlv_extra *extra)
+{
+	struct request_unicast_xmit_tlv *request;
+	struct ack_cancel_unicast_xmit_tlv *ack;
+	struct cancel_unicast_xmit_tlv *cancel;
+	struct grant_unicast_xmit_tlv *grant;
+	struct TLV *tlv = extra->tlv;
+
+	switch (tlv->type) {
+	case TLV_REQUEST_UNICAST_TRANSMISSION:
+		if (TLV_LENGTH_INVALID(tlv, request_unicast_xmit_tlv)) {
+			return -EBADMSG;
+		}
+		request = (struct request_unicast_xmit_tlv *) tlv;
+		if (!unicast_message_type_valid(request->message_type)) {
+			return -EBADMSG;
+		}
+		NTOHL(request->durationField);
+		break;
+	case TLV_GRANT_UNICAST_TRANSMISSION:
+		if (TLV_LENGTH_INVALID(tlv, grant_unicast_xmit_tlv)) {
+			return -EBADMSG;
+		}
+		grant = (struct grant_unicast_xmit_tlv *) tlv;
+		if (!unicast_message_type_valid(grant->message_type)) {
+			return -EBADMSG;
+		}
+		NTOHL(grant->durationField);
+		break;
+	case TLV_CANCEL_UNICAST_TRANSMISSION:
+		if (TLV_LENGTH_INVALID(tlv, cancel_unicast_xmit_tlv)) {
+			return -EBADMSG;
+		}
+		cancel = (struct cancel_unicast_xmit_tlv *) tlv;
+		if (!unicast_message_type_valid(cancel->message_type_flags)) {
+			return -EBADMSG;
+		}
+		break;
+	case TLV_ACKNOWLEDGE_CANCEL_UNICAST_TRANSMISSION:
+		if (TLV_LENGTH_INVALID(tlv, ack_cancel_unicast_xmit_tlv)) {
+			return -EBADMSG;
+		}
+		ack = (struct ack_cancel_unicast_xmit_tlv *) tlv;
+		if (!unicast_message_type_valid(ack->message_type_flags)) {
+			return -EBADMSG;
+		}
+		break;
+	}
+	return 0;
+}
+
+static void unicast_negotiation_pre_send(struct TLV *tlv)
+{
+	struct request_unicast_xmit_tlv *request;
+	struct grant_unicast_xmit_tlv *grant;
+
+	switch (tlv->type) {
+	case TLV_REQUEST_UNICAST_TRANSMISSION:
+		request = (struct request_unicast_xmit_tlv *) tlv;
+		HTONL(request->durationField);
+		break;
+	case TLV_GRANT_UNICAST_TRANSMISSION:
+		grant = (struct grant_unicast_xmit_tlv *) tlv;
+		HTONL(grant->durationField);
+		break;
+	case TLV_CANCEL_UNICAST_TRANSMISSION:
+	case TLV_ACKNOWLEDGE_CANCEL_UNICAST_TRANSMISSION:
+		break;
+	}
+}
+
 struct tlv_extra *tlv_extra_alloc(void)
 {
 	struct tlv_extra *extra = TAILQ_FIRST(&tlv_pool);
@@ -605,6 +690,7 @@ int tlv_post_recv(struct tlv_extra *extra)
 	case TLV_GRANT_UNICAST_TRANSMISSION:
 	case TLV_CANCEL_UNICAST_TRANSMISSION:
 	case TLV_ACKNOWLEDGE_CANCEL_UNICAST_TRANSMISSION:
+		result = unicast_negotiation_post_recv(extra);
 		break;
 	case TLV_PATH_TRACE:
 		ptt = (struct path_trace_tlv *) tlv;
@@ -654,6 +740,8 @@ void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra)
 	case TLV_GRANT_UNICAST_TRANSMISSION:
 	case TLV_CANCEL_UNICAST_TRANSMISSION:
 	case TLV_ACKNOWLEDGE_CANCEL_UNICAST_TRANSMISSION:
+		unicast_negotiation_pre_send(tlv);
+		break;
 	case TLV_PATH_TRACE:
 	case TLV_ALTERNATE_TIME_OFFSET_INDICATOR:
 	case TLV_AUTHENTICATION:
diff --git a/tlv.h b/tlv.h
index 4ec9173..958555c 100644
--- a/tlv.h
+++ b/tlv.h
@@ -115,6 +115,34 @@ enum management_action {
 #define TLV_NOT_SUPPORTED				0x0006
 #define TLV_GENERAL_ERROR				0xFFFE
 
+#define CANCEL_UNICAST_MAINTAIN_REQUEST	(1 << 0)
+#define CANCEL_UNICAST_MAINTAIN_GRANT	(1 << 1)
+#define GRANT_UNICAST_RENEWAL_INVITED	(1 << 0)
+
+struct ack_cancel_unicast_xmit_tlv {
+	Enumeration16   type;
+	UInteger16      length;
+	uint8_t         message_type_flags;
+	uint8_t         reserved;
+} PACKED;
+
+struct cancel_unicast_xmit_tlv {
+	Enumeration16   type;
+	UInteger16      length;
+	uint8_t         message_type_flags;
+	uint8_t         reserved;
+} PACKED;
+
+struct grant_unicast_xmit_tlv {
+	Enumeration16   type;
+	UInteger16      length;
+	uint8_t         message_type;
+	Integer8        logInterMessagePeriod;
+	UInteger32      durationField;
+	uint8_t         reserved;
+	uint8_t         flags;
+} PACKED;
+
 struct management_tlv {
 	Enumeration16 type;
 	UInteger16    length;
@@ -172,6 +200,14 @@ struct path_trace_tlv {
 	struct ClockIdentity cid[0];
 } PACKED;
 
+struct request_unicast_xmit_tlv {
+	Enumeration16   type;
+	UInteger16      length;
+	uint8_t         message_type;
+	Integer8        logInterMessagePeriod;
+	UInteger32      durationField;
+} PACKED;
+
 static inline unsigned int path_length(struct path_trace_tlv *p)
 {
 	return p->length / sizeof(struct ClockIdentity);