From 226bd355af222dbbbf8aa14fb282abddf2297716 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Thu, 9 Aug 2012 20:33:47 +0200
Subject: [PATCH] Convert the follow up info tlv to and from network byte
 order.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 msg.c |  4 ++--
 msg.h | 10 ++++++++++
 tlv.c | 61 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 tlv.h | 28 +++++++++++++++++++++++++++
 4 files changed, 101 insertions(+), 2 deletions(-)

diff --git a/msg.c b/msg.c
index 0082a90..d24fbc0 100644
--- a/msg.c
+++ b/msg.c
@@ -76,12 +76,12 @@ static void announce_post_recv(struct announce_msg *m)
 	m->stepsRemoved = ntohs(m->stepsRemoved);
 }
 
-static int64_t host2net64(int64_t val)
+int64_t host2net64(int64_t val)
 {
 	return __cpu_to_be64(val);
 }
 
-static int64_t net2host64(int64_t val)
+int64_t net2host64(int64_t val)
 {
 	return __be64_to_cpu(val);
 }
diff --git a/msg.h b/msg.h
index 44c6134..b01f086 100644
--- a/msg.h
+++ b/msg.h
@@ -317,4 +317,14 @@ static inline Boolean one_step(struct ptp_message *m)
 	return !field_is_set(m, 0, TWO_STEP);
 }
 
+/**
+ * Convert a 64 bit word into network byte order.
+ */
+int64_t host2net64(int64_t val);
+
+/**
+ * Convert a 64 bit word into host byte order.
+ */
+int64_t net2host64(int64_t val);
+
 #endif
diff --git a/tlv.c b/tlv.c
index bde00d6..3e84184 100644
--- a/tlv.c
+++ b/tlv.c
@@ -17,9 +17,66 @@
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  */
 #include <arpa/inet.h>
+#include <string.h>
 
 #include "tlv.h"
 
+uint8_t ieee8021_id[3] = { IEEE_802_1_COMMITTEE };
+
+static void scaled_ns_n2h(ScaledNs *sns)
+{
+	sns->nanoseconds_msb = ntohs(sns->nanoseconds_msb);
+	sns->nanoseconds_lsb = net2host64(sns->nanoseconds_msb);
+	sns->fractional_nanoseconds = ntohs(sns->fractional_nanoseconds);
+}
+
+static void scaled_ns_h2n(ScaledNs *sns)
+{
+	sns->nanoseconds_msb = htons(sns->nanoseconds_msb);
+	sns->nanoseconds_lsb = host2net64(sns->nanoseconds_msb);
+	sns->fractional_nanoseconds = htons(sns->fractional_nanoseconds);
+}
+
+static void org_post_recv(struct organization_tlv *org)
+{
+	struct follow_up_info_tlv *f;
+
+	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return;
+		}
+		switch (org->subtype[2]) {
+		case 1:
+			f = (struct follow_up_info_tlv *) org;
+			f->cumulativeScaledRateOffset = ntohl(f->cumulativeScaledRateOffset);
+			f->gmTimeBaseIndicator = ntohs(f->gmTimeBaseIndicator);
+			scaled_ns_n2h(&f->lastGmPhaseChange);
+			f->scaledLastGmPhaseChange = ntohl(f->scaledLastGmPhaseChange);
+			break;
+		}
+	}
+}
+
+static void org_pre_send(struct organization_tlv *org)
+{
+	struct follow_up_info_tlv *f;
+
+	if (0 == memcmp(org->id, ieee8021_id, sizeof(ieee8021_id))) {
+		if (org->subtype[0] || org->subtype[1]) {
+			return;
+		}
+		switch (org->subtype[2]) {
+		case 1:
+			f = (struct follow_up_info_tlv *) org;
+			f->cumulativeScaledRateOffset = htonl(f->cumulativeScaledRateOffset);
+			f->gmTimeBaseIndicator = htons(f->gmTimeBaseIndicator);
+			scaled_ns_h2n(&f->lastGmPhaseChange);
+			f->scaledLastGmPhaseChange = htonl(f->scaledLastGmPhaseChange);
+			break;
+		}
+	}
+}
+
 void tlv_post_recv(struct TLV *tlv)
 {
 	struct management_tlv *mgt;
@@ -37,6 +94,8 @@ void tlv_post_recv(struct TLV *tlv)
 		mes->id = ntohs(mes->id);
 		break;
 	case TLV_ORGANIZATION_EXTENSION:
+		org_post_recv((struct organization_tlv *) tlv);
+		break;
 	case TLV_REQUEST_UNICAST_TRANSMISSION:
 	case TLV_GRANT_UNICAST_TRANSMISSION:
 	case TLV_CANCEL_UNICAST_TRANSMISSION:
@@ -74,6 +133,8 @@ void tlv_pre_send(struct TLV *tlv)
 		mes->id = htons(mes->id);
 		break;
 	case TLV_ORGANIZATION_EXTENSION:
+		org_pre_send((struct organization_tlv *) tlv);
+		break;
 	case TLV_REQUEST_UNICAST_TRANSMISSION:
 	case TLV_GRANT_UNICAST_TRANSMISSION:
 	case TLV_CANCEL_UNICAST_TRANSMISSION:
diff --git a/tlv.h b/tlv.h
index b6088d2..375b821 100644
--- a/tlv.h
+++ b/tlv.h
@@ -123,6 +123,17 @@ struct management_error_status {
 	Octet         data[0];
 } PACKED;
 
+/* Organizationally Unique Identifiers */
+#define IEEE_802_1_COMMITTEE 0x00, 0x80, 0xC2
+extern uint8_t ieee8021_id[3];
+
+struct organization_tlv {
+	Enumeration16 type;
+	UInteger16    length;
+	Octet         id[3];
+	Octet         subtype[3];
+} PACKED;
+
 #define PATH_TRACE_MAX \
 	((sizeof(struct message_data) - sizeof(struct announce_msg) - sizeof(struct TLV)) / \
 	 sizeof(struct ClockIdentity))
@@ -138,6 +149,23 @@ static inline unsigned int path_length(struct path_trace_tlv *p)
 	return p->length / sizeof(struct ClockIdentity);
 }
 
+typedef struct Integer96 {
+	uint16_t nanoseconds_msb;
+	uint64_t nanoseconds_lsb;
+	uint16_t fractional_nanoseconds;
+} PACKED ScaledNs;
+
+struct follow_up_info_tlv {
+	Enumeration16 type;
+	UInteger16    length;
+	Octet         id[3];
+	Octet         subtype[3];
+	UInteger32    cumulativeScaledRateOffset;
+	UInteger16    gmTimeBaseIndicator;
+	ScaledNs      lastGmPhaseChange;
+	Integer32     scaledLastGmPhaseChange;
+} PACKED;
+
 /**
  * Converts recognized value sub-fields into host byte order.
  * @param tlv Pointer to a Type Length Value field.