[Linuxptp-devel] [PATCH v4 05/11] tlv: Encode and decode alternate time offset indicator TLVs.

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

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

diff --git a/tlv.c b/tlv.c
index 2e421ed..212414f 100644
--- a/tlv.c
+++ b/tlv.c
@@ -77,6 +77,22 @@ static uint16_t flip16(void *p)
 	return v;
 }
 
+static void host2net32_unaligned(void *p)
+{
+	int32_t v;
+	memcpy(&v, p, sizeof(v));
+	v = htonl(v);
+	memcpy(p, &v, sizeof(v));
+}
+
+static void net2host32_unaligned(void *p)
+{
+	int32_t v;
+	memcpy(&v, p, sizeof(v));
+	v = ntohl(v);
+	memcpy(p, &v, sizeof(v));
+}
+
 static int64_t host2net64_unaligned(void *p)
 {
 	int64_t v;
@@ -111,6 +127,43 @@ static bool tlv_array_invalid(struct TLV *tlv, size_t base_size, size_t item_siz
 	return (tlv->length == expected_length) ? false : true;
 }
 
+static int alttime_offset_post_recv(struct tlv_extra *extra)
+{
+	struct TLV *tlv = extra->tlv;
+	struct alternate_time_offset_indicator_tlv *atoi =
+		(struct alternate_time_offset_indicator_tlv *) tlv;
+
+	if (tlv->length < sizeof(struct alternate_time_offset_indicator_tlv) +
+	    atoi->displayName.length - sizeof(struct TLV)) {
+		return -EBADMSG;
+	}
+
+	NTOHS(atoi->type);
+	NTOHS(atoi->length);
+	/* Message alignment broken by design. */
+	net2host32_unaligned(&atoi->currentOffset);
+	net2host32_unaligned(&atoi->jumpSeconds);
+	flip16(&atoi->timeOfNextJump.seconds_msb);
+	net2host32_unaligned(&atoi->timeOfNextJump.seconds_lsb);
+
+	return 0;
+}
+
+static void alttime_offset_pre_send(struct tlv_extra *extra)
+{
+	struct alternate_time_offset_indicator_tlv *atoi;
+
+	atoi = (struct alternate_time_offset_indicator_tlv *) extra->tlv;
+
+	HTONS(atoi->type);
+	HTONS(atoi->length);
+	/* Message alignment broken by design. */
+	host2net32_unaligned(&atoi->currentOffset);
+	host2net32_unaligned(&atoi->jumpSeconds);
+	flip16(&atoi->timeOfNextJump.seconds_msb);
+	host2net32_unaligned(&atoi->timeOfNextJump.seconds_lsb);
+}
+
 static int mgt_post_recv(struct management_tlv *m, uint16_t data_len,
 			 struct tlv_extra *extra)
 {
@@ -1035,6 +1088,8 @@ int tlv_post_recv(struct tlv_extra *extra)
 		}
 		break;
 	case TLV_ALTERNATE_TIME_OFFSET_INDICATOR:
+		result = alttime_offset_post_recv(extra);
+		break;
 	case TLV_AUTHENTICATION_2008:
 	case TLV_AUTHENTICATION_CHALLENGE:
 	case TLV_SECURITY_ASSOCIATION_UPDATE:
@@ -1098,7 +1153,10 @@ void tlv_pre_send(struct TLV *tlv, struct tlv_extra *extra)
 		unicast_negotiation_pre_send(tlv);
 		break;
 	case TLV_PATH_TRACE:
+		break;
 	case TLV_ALTERNATE_TIME_OFFSET_INDICATOR:
+		alttime_offset_pre_send(extra);
+		break;
 	case TLV_AUTHENTICATION_2008:
 	case TLV_AUTHENTICATION_CHALLENGE:
 	case TLV_SECURITY_ASSOCIATION_UPDATE:
diff --git a/tlv.h b/tlv.h
index 3dbce4f..a1a7dbc 100644
--- a/tlv.h
+++ b/tlv.h
@@ -175,6 +175,32 @@ struct grant_unicast_xmit_tlv {
 	uint8_t         flags;
 } PACKED;
 
+struct alternate_time_offset_indicator_tlv {
+	Enumeration16   type;
+	UInteger16      length;
+	UInteger8       keyField;
+	/* Message alignment broken by design. */
+	Integer32       currentOffset;
+	Integer32       jumpSeconds;
+	struct {
+		uint16_t   seconds_msb; /* 16 bits + */
+		uint32_t   seconds_lsb; /* 32 bits = 48 bits*/
+	} PACKED timeOfNextJump;
+	struct PTPText  displayName;
+} PACKED;
+
+struct alternate_time_offset_properties {
+	UInteger8       keyField;
+	/* Message alignment broken by design. */
+	Integer32       currentOffset;
+	Integer32       jumpSeconds;
+	struct {
+		uint16_t   seconds_msb; /* 16 bits + */
+		uint32_t   seconds_lsb; /* 32 bits = 48 bits*/
+	} PACKED timeOfNextJump;
+	uint8_t pad;
+} PACKED;
+
 struct management_tlv {
 	Enumeration16 type;
 	UInteger16    length;
-- 
2.30.2