msg: Populate the TLV list on receive.

This patch changes the receive message parsing code to place each TLV
into the list.  A method is introduced that allows attaching TLVs to
the end of the list.

In addition, msg.last_tlv is converted into a pointer to the last item
in the list.  Because of this change, the transmit code that uses this
field now allocates a TLV before using it.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>

clock.c

@@ -340,9 +340,17 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 	struct subscribe_events_np *sen;
 	struct management_tlv *tlv;
 	struct time_status_np *tsn;
+	struct tlv_extra *extra;
 	struct PTPText *text;
 	int datalen = 0;
 
+	extra = tlv_extra_alloc();
+	if (!extra) {
+		pr_err("failed to allocate TLV descriptor");
+		return 0;
+	}
+	extra->tlv = (struct TLV *) rsp->management.suffix;
+
 	tlv = (struct management_tlv *) rsp->management.suffix;
 	tlv->type = TLV_MANAGEMENT;
 	tlv->id = id;
@@ -448,7 +456,7 @@ static int clock_management_fill_response(struct clock *c, struct port *p,
 	}
 	tlv->length = sizeof(tlv->id) + datalen;
 	rsp->header.messageLength += sizeof(*tlv) + datalen;
-	rsp->tlv_count = 1;
+	msg_tlv_attach(rsp, extra);
 
 	/* The caller can respond to this message. */
 	return 1;

msg.c

@@ -120,33 +120,43 @@ static void port_id_pre_send(struct PortIdentity *pid)
 	pid->portNumber = htons(pid->portNumber);
 }
 
-static int suffix_post_recv(uint8_t *ptr, int len, struct tlv_extra *last)
+static int suffix_post_recv(struct ptp_message *msg, uint8_t *ptr, int len)
 {
-	int cnt, err;
-	struct TLV *tlv;
+	struct tlv_extra *extra;
+	int err;
 
 	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) {
+	while (len > sizeof(struct TLV)) {
+		extra = tlv_extra_alloc();
+		if (!extra) {
+			pr_err("failed to allocate TLV descriptor");
+			return -ENOMEM;
+		}
+		extra->tlv = (struct TLV *) ptr;
+		extra->tlv->type = ntohs(extra->tlv->type);
+		extra->tlv->length = ntohs(extra->tlv->length);
+		if (extra->tlv->length % 2) {
+			tlv_extra_recycle(extra);
 			return -EBADMSG;
 		}
 		len -= sizeof(struct TLV);
 		ptr += sizeof(struct TLV);
-		if (tlv->length > len) {
+		if (extra->tlv->length > len) {
+			tlv_extra_recycle(extra);
 			return -EBADMSG;
 		}
-		len -= tlv->length;
-		ptr += tlv->length;
-		err = tlv_post_recv(tlv, len > sizeof(struct TLV) ? NULL : last);
-		if (err)
+		len -= extra->tlv->length;
+		ptr += extra->tlv->length;
+		err = tlv_post_recv(extra);
+		if (err) {
+			tlv_extra_recycle(extra);
 			return err;
 		}
-	return cnt;
+		msg_tlv_attach(msg, extra);
+	}
+	return 0;
 }
 
 static void suffix_pre_send(uint8_t *ptr, int cnt, struct tlv_extra *last)
@@ -324,9 +334,9 @@ int msg_post_recv(struct ptp_message *m, int cnt)
 	if (msg_sots_missing(m))
 		return -ETIME;
 
-	m->tlv_count = suffix_post_recv(suffix, cnt - pdulen, &m->last_tlv);
-	if (m->tlv_count < 0)
-		return m->tlv_count;
+	err = suffix_post_recv(m, suffix, cnt - pdulen);
+	if (err)
+		return err;
 
 	return 0;
 }
@@ -381,10 +391,17 @@ int msg_pre_send(struct ptp_message *m)
 	default:
 		return -1;
 	}
-	suffix_pre_send(suffix, m->tlv_count, &m->last_tlv);
+	suffix_pre_send(suffix, m->tlv_count, m->last_tlv);
 	return 0;
 }
 
+void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra)
+{
+	TAILQ_INSERT_TAIL(&msg->tlv_list, extra, list);
+	msg->tlv_count++;
+	msg->last_tlv = extra;
+}
+
 const char *msg_type_string(int type)
 {
 	switch (type) {

msg.h

@@ -233,7 +233,7 @@ struct ptp_message {
 	 * the layout of the TLV makes it difficult to access the data
 	 * directly from the message's buffer.
 	 */
-	struct tlv_extra last_tlv;
+	struct tlv_extra *last_tlv;
 };
 
 /**
@@ -258,6 +258,14 @@ static inline Boolean field_is_set(struct ptp_message *m, int index, Octet bit)
 	return m->header.flagField[index] & bit ? TRUE : FALSE;
 }
 
+/**
+ * Place a TLV descriptor into a message's list of TLVs.
+ *
+ * @param msg     A message obtained using msg_allocate().
+ * @param extra   The TLV to be added to the list.
+ */
+void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra);
+
 /**
  * Obtain the transportSpecific field from a message.
  * @param m  Message to test.

pmc.c

@@ -194,8 +194,10 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 	struct time_status_np *tsn;
 	struct grandmaster_settings_np *gsn;
 	struct mgmt_clock_description *cd;
+	struct tlv_extra *extra;
 	struct portDS *p;
 	struct port_ds_np *pnp;
+
 	if (msg_type(msg) != MANAGEMENT) {
 		return;
 	}
@@ -209,6 +211,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 	if (msg->tlv_count != 1) {
 		goto out;
 	}
+	extra = TAILQ_FIRST(&msg->tlv_list);
 	tlv = (struct TLV *) msg->management.suffix;
 	if (tlv->type == TLV_MANAGEMENT) {
 		fprintf(fp, "MANAGEMENT ");
@@ -226,7 +229,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 	}
 	switch (mgt->id) {
 	case TLV_CLOCK_DESCRIPTION:
-		cd = &msg->last_tlv.cd;
+		cd = &extra->cd;
 		fprintf(fp, "CLOCK_DESCRIPTION "
 			IFMT "clockType             0x%hx"
 			IFMT "physicalLayerProtocol %s"
@@ -252,7 +255,7 @@ static void pmc_show(struct ptp_message *msg, FILE *fp)
 	case TLV_USER_DESCRIPTION:
 		fprintf(fp, "USER_DESCRIPTION "
 			IFMT "userDescription  %s",
-			text2str(msg->last_tlv.cd.userDescription));
+			text2str(extra->cd.userDescription));
 		break;
 	case TLV_DEFAULT_DATA_SET:
 		dds = (struct defaultDS *) mgt->data;

pmc_common.c

@@ -228,6 +228,8 @@ int pmc_send_get_action(struct pmc *pmc, int id)
 	int datalen, pdulen;
 	struct ptp_message *msg;
 	struct management_tlv *mgt;
+	struct tlv_extra *extra;
+
 	msg = pmc_message(pmc, GET);
 	if (!msg) {
 		return -1;
@@ -239,14 +241,21 @@ int pmc_send_get_action(struct pmc *pmc, int id)
 	mgt->id = id;
 	pdulen = msg->header.messageLength + sizeof(*mgt) + datalen;
 	msg->header.messageLength = pdulen;
-	msg->tlv_count = 1;
+
+	extra = tlv_extra_alloc();
+	if (!extra) {
+		pr_err("failed to allocate TLV descriptor");
+		return -ENOMEM;
+	}
+	extra->tlv = (struct TLV *) msg->management.suffix;
+	msg_tlv_attach(msg, extra);
 
 	if (id == TLV_CLOCK_DESCRIPTION && !pmc->zero_length_gets) {
 		/*
 		 * Make sure the tlv_extra pointers dereferenced in
 		 * mgt_pre_send() do point to something.
 		 */
-		struct mgmt_clock_description *cd = &msg->last_tlv.cd;
+		struct mgmt_clock_description *cd = &extra->cd;
 		uint8_t *buf = mgt->data;
 		cd->clockType = (UInteger16 *) buf;
 		buf += sizeof(*cd->clockType);

port.c

@@ -711,11 +711,19 @@ static int port_management_fill_response(struct port *target,
 	struct port_properties_np *ppn;
 	struct management_tlv *tlv;
 	struct port_ds_np *pdsnp;
+	struct tlv_extra *extra;
 	struct portDS *pds;
 	uint16_t u16;
 	uint8_t *buf;
 	int datalen;
 
+	extra = tlv_extra_alloc();
+	if (!extra) {
+		pr_err("failed to allocate TLV descriptor");
+		return 0;
+	}
+	extra->tlv = (struct TLV *) rsp->management.suffix;
+
 	tlv = (struct management_tlv *) rsp->management.suffix;
 	tlv->type = TLV_MANAGEMENT;
 	tlv->id = id;
@@ -725,7 +733,7 @@ static int port_management_fill_response(struct port *target,
 		datalen = 0;
 		break;
 	case TLV_CLOCK_DESCRIPTION:
-		cd = &rsp->last_tlv.cd;
+		cd = &extra->cd;
 		buf = tlv->data;
 		cd->clockType = (UInteger16 *) buf;
 		buf += sizeof(*cd->clockType);
@@ -867,7 +875,7 @@ static int port_management_fill_response(struct port *target,
 	}
 	tlv->length = sizeof(tlv->id) + datalen;
 	rsp->header.messageLength += sizeof(*tlv) + datalen;
-	rsp->tlv_count = 1;
+	msg_tlv_attach(rsp, extra);
 
 	/* The caller can respond to this message. */
 	return 1;

tlv.c

@@ -444,15 +444,13 @@ void tlv_extra_recycle(struct tlv_extra *extra)
 	TAILQ_INSERT_HEAD(&tlv_pool, extra, list);
 }
 
-int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra)
+int tlv_post_recv(struct tlv_extra *extra)
 {
 	int result = 0;
 	struct management_tlv *mgt;
 	struct management_error_status *mes;
+	struct TLV *tlv = extra->tlv;
 	struct path_trace_tlv *ptt;
-	struct tlv_extra dummy_extra;
-	if (!extra)
-		extra = &dummy_extra;
 
 	switch (tlv->type) {
 	case TLV_MANAGEMENT:

tlv.h

@@ -256,12 +256,10 @@ void tlv_extra_recycle(struct tlv_extra *extra);
 
 /**
  * Converts recognized value sub-fields into host byte order.
- * @param tlv Pointer to a Type Length Value field.
- * @param extra Additional struct where data from tlv will be saved,
- * can be NULL.
+ * @param extra  TLV descriptor pointing to the protocol data.
  * @return Zero if successful, otherwise non-zero
  */
-int tlv_post_recv(struct TLV *tlv, struct tlv_extra *extra);
+int tlv_post_recv(struct tlv_extra *extra);
 
 /**
  * Converts recognized value sub-fields into network byte order.