From f20cf6225a75f3d109c1ef0fadbfa78ff03e1d52 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Sat, 28 Jul 2012 18:54:26 +0200
Subject: [PATCH] Convert TLV type and length to host byte order on receive.

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

diff --git a/msg.c b/msg.c
index 24bfe26..6ded539 100644
--- a/msg.c
+++ b/msg.c
@@ -142,6 +142,32 @@ static void port_id_pre_send(struct PortIdentity *pid)
 	pid->portNumber = htons(pid->portNumber);
 }
 
+static int suffix_post_recv(uint8_t *ptr, int len)
+{
+	int cnt;
+	struct TLV *tlv;
+
+	if (!ptr)
+		return 0;
+
+	for (cnt = 0; len > sizeof(struct TLV); cnt++) {
+		tlv = (struct TLV *) ptr;
+		tlv->type = ntohs(tlv->type);
+		tlv->length = ntohs(tlv->length);
+		if (tlv->length % 2) {
+			break;
+		}
+		len -= sizeof(struct TLV);
+		ptr += sizeof(struct TLV);
+		if (tlv->length > len) {
+			break;
+		}
+		len -= tlv->length;
+		ptr += tlv->length;
+	}
+	return cnt;
+}
+
 static void timestamp_post_recv(struct ptp_message *m, struct Timestamp *ts)
 {
 	uint32_t lsb = ntohl(ts->seconds_lsb);
@@ -193,6 +219,7 @@ void msg_get(struct ptp_message *m)
 int msg_post_recv(struct ptp_message *m, int cnt)
 {
 	int pdulen, type;
+	uint8_t *suffix = NULL;
 
 	if (cnt < sizeof(struct ptp_header))
 		return -1;
@@ -254,21 +281,28 @@ int msg_post_recv(struct ptp_message *m, int cnt)
 		break;
 	case FOLLOW_UP:
 		timestamp_post_recv(m, &m->follow_up.preciseOriginTimestamp);
+		suffix = m->follow_up.suffix;
 		break;
 	case DELAY_RESP:
 		timestamp_post_recv(m, &m->delay_resp.receiveTimestamp);
+		suffix = m->delay_resp.suffix;
 		break;
 	case PDELAY_RESP_FOLLOW_UP:
 		timestamp_post_recv(m, &m->pdelay_resp_fup.responseOriginTimestamp);
 		port_id_post_recv(&m->pdelay_resp_fup.requestingPortIdentity);
+		suffix = m->pdelay_resp_fup.suffix;
 		break;
 	case ANNOUNCE:
 		clock_gettime(CLOCK_MONOTONIC, &m->ts.host);
 		timestamp_post_recv(m, &m->announce.originTimestamp);
 		announce_post_recv(&m->announce);
+		suffix = m->announce.suffix;
 		break;
 	case SIGNALING:
+		suffix = m->signaling.suffix;
+		break;
 	case MANAGEMENT:
+		suffix = m->management.suffix;
 		break;
 	}
 
@@ -277,6 +311,8 @@ int msg_post_recv(struct ptp_message *m, int cnt)
 		return -1;
 	}
 
+	m->tlv_count = suffix_post_recv(suffix, cnt - pdulen);
+
 	return 0;
 }
 
diff --git a/msg.h b/msg.h
index d737ee2..8147383 100644
--- a/msg.h
+++ b/msg.h
@@ -198,6 +198,10 @@ struct ptp_message {
 	 * SO_TIMESTAMPING socket option.
 	 */
 	struct hw_timestamp hwts;
+	/**
+	 * Contains the number of TLVs in the suffix.
+	 */
+	int tlv_count;
 };
 
 /**