[Linuxptp-devel] [PATCH v4 02/11] tlv: Encode and decode power profile TLVs.

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

Signed-off-by: Richard Cochran <ric...@gm...>
---
 tlv.c | 36 ++++++++++++++++++++++++++++++++++++
 tlv.h | 24 ++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/tlv.c b/tlv.c
index 1c13460..29d0556 100644
--- a/tlv.c
+++ b/tlv.c
@@ -35,6 +35,7 @@
 	(tlv->length < sizeof(struct type) - sizeof(struct TLV))
 
 uint8_t ieee8021_id[3] = { IEEE_802_1_COMMITTEE };
+uint8_t ieeec37_238_id[3] = { IEEE_C37_238_PROFILE };
 
 static TAILQ_HEAD(tlv_pool, tlv_extra) tlv_pool =
 	TAILQ_HEAD_INITIALIZER(tlv_pool);
@@ -679,6 +680,7 @@ static void nsm_resp_pre_send(struct tlv_extra *extra)
 
 static int org_post_recv(struct organization_tlv *org)
 {
+	struct ieee_c37_238_2017_tlv *p;
 	struct follow_up_info_tlv *f;
 
 	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
@@ -701,6 +703,24 @@ static int org_post_recv(struct organization_tlv *org)
 				goto bad_length;
 		}
 	}
+	if (0 == memcmp(org->id, ieeec37_238_id, sizeof(ieeec37_238_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return 0;
+		}
+		switch (org->subtype[2]) {
+		case 1:
+		case 2:
+			/* Layout of 2011 and 2017 messages is compatible. */
+			if (org->length + sizeof(struct TLV) !=
+			    sizeof(struct ieee_c37_238_2017_tlv))
+				goto bad_length;
+			p = (struct ieee_c37_238_2017_tlv *) org;
+			NTOHS(p->grandmasterID);
+			NTOHL(p->reserved1);
+			NTOHL(p->totalTimeInaccuracy);
+			break;
+		}
+	}
 	return 0;
 bad_length:
 	return -EBADMSG;
@@ -708,6 +728,7 @@ bad_length:
 
 static void org_pre_send(struct organization_tlv *org)
 {
+	struct ieee_c37_238_2017_tlv *p;
 	struct follow_up_info_tlv *f;
 
 	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
@@ -724,6 +745,21 @@ static void org_pre_send(struct organization_tlv *org)
 			break;
 		}
 	}
+	if (0 == memcmp(org->id, ieeec37_238_id, sizeof(ieeec37_238_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return;
+		}
+		switch (org->subtype[2]) {
+		case 1:
+		case 2:
+			/* Layout of 2011 and 2017 messages is compatible. */
+			p = (struct ieee_c37_238_2017_tlv *) org;
+			HTONS(p->grandmasterID);
+			HTONL(p->reserved1);
+			HTONL(p->totalTimeInaccuracy);
+			break;
+		}
+	}
 }
 
 static int slave_delay_timing_data_post_revc(struct tlv_extra *extra)
diff --git a/tlv.h b/tlv.h
index 8966696..409c8d0 100644
--- a/tlv.h
+++ b/tlv.h
@@ -213,6 +213,8 @@ struct nsm_resp_tlv_foot {
 /* Organizationally Unique Identifiers */
 #define IEEE_802_1_COMMITTEE 0x00, 0x80, 0xC2
 extern uint8_t ieee8021_id[3];
+#define IEEE_C37_238_PROFILE 0x1C, 0x12, 0x9D
+extern uint8_t ieeec37_238_id[3];
 
 struct organization_tlv {
 	Enumeration16 type;
@@ -300,6 +302,28 @@ struct follow_up_info_tlv {
 	Integer32     scaledLastGmPhaseChange;
 } PACKED;
 
+struct ieee_c37_238_2011_tlv {
+	Enumeration16 type;
+	UInteger16    length;
+	Octet         id[3];
+	Octet         subtype[3];
+	UInteger16    grandmasterID;
+	UInteger32    grandmasterTimeInaccuracy;
+	UInteger32    networkTimeInaccuracy;
+	Octet         pad[2];
+} PACKED;
+
+struct ieee_c37_238_2017_tlv {
+	Enumeration16 type;
+	UInteger16    length;
+	Octet         id[3];
+	Octet         subtype[3];
+	UInteger16    grandmasterID;
+	UInteger32    reserved1;
+	UInteger32    totalTimeInaccuracy;
+	Octet         pad[2];
+} PACKED;
+
 struct msg_interval_req_tlv {
 	Enumeration16 type;
 	UInteger16    length;
-- 
2.30.2