From 4a8877f9046210291e83d2a939527b2505808c8c Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Fri, 23 Feb 2018 22:43:24 -0800
Subject: [PATCH] msg: Introduce method for appending multiple TLVs on
 transmit.

In order to support multiple TLVs, the transmit code must be able to
append one TLV after another.  This patch adds a method that checks
whether there is room, allocates the TLV descriptor, sets the buffer
pointer, and appends the descriptor to the list.

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

diff --git a/msg.c b/msg.c
index 345de7e..9434d42 100644
--- a/msg.c
+++ b/msg.c
@@ -110,6 +110,68 @@ static int hdr_pre_send(struct ptp_header *m)
 	return 0;
 }
 
+static uint8_t *msg_suffix(struct ptp_message *m)
+{
+	switch (msg_type(m)) {
+	case SYNC:
+		return NULL;
+	case DELAY_REQ:
+		return NULL;
+	case PDELAY_REQ:
+		return NULL;
+	case PDELAY_RESP:
+		return NULL;
+	case FOLLOW_UP:
+		return m->follow_up.suffix;
+	case DELAY_RESP:
+		return m->delay_resp.suffix;
+	case PDELAY_RESP_FOLLOW_UP:
+		return m->pdelay_resp_fup.suffix;
+	case ANNOUNCE:
+		return m->announce.suffix;
+	case SIGNALING:
+		return m->signaling.suffix;
+	case MANAGEMENT:
+		return m->management.suffix;
+	}
+	return NULL;
+}
+
+static struct tlv_extra *msg_tlv_prepare(struct ptp_message *msg, int length)
+{
+	struct tlv_extra *extra, *tmp;
+	uint8_t *ptr;
+
+	/* Make sure this message type admits appended TLVs. */
+	ptr = msg_suffix(msg);
+	if (!ptr) {
+		pr_err("TLV on %s not allowed", msg_type_string(msg_type(msg)));
+		return NULL;
+	}
+	tmp = TAILQ_LAST(&msg->tlv_list, tlv_list);
+	if (tmp) {
+		ptr = (uint8_t *) tmp->tlv;
+		ptr += tmp->tlv->length;
+	}
+
+	/* Check that the message buffer has enough room for the new TLV. */
+	if ((unsigned long)(ptr + length) >
+	    (unsigned long)(&msg->tail_room)) {
+		pr_debug("cannot fit TLV of length %d into message", length);
+		return NULL;
+	}
+
+	/* Allocate a TLV descriptor and setup the pointer. */
+	extra = tlv_extra_alloc();
+	if (!extra) {
+		pr_err("failed to allocate TLV descriptor");
+		return NULL;
+	}
+	extra->tlv = (struct TLV *) ptr;
+
+	return extra;
+}
+
 static void port_id_post_recv(struct PortIdentity *pid)
 {
 	pid->portNumber = ntohs(pid->portNumber);
@@ -395,6 +457,18 @@ int msg_pre_send(struct ptp_message *m)
 	return 0;
 }
 
+struct tlv_extra *msg_tlv_append(struct ptp_message *msg, int length)
+{
+	struct tlv_extra *extra;
+
+	extra = msg_tlv_prepare(msg, length);
+	if (extra) {
+		msg->header.messageLength += length;
+		msg_tlv_attach(msg, extra);
+	}
+	return extra;
+}
+
 void msg_tlv_attach(struct ptp_message *msg, struct tlv_extra *extra)
 {
 	TAILQ_INSERT_TAIL(&msg->tlv_list, extra, list);
diff --git a/msg.h b/msg.h
index 7d7b64e..7005481 100644
--- a/msg.h
+++ b/msg.h
@@ -258,6 +258,22 @@ static inline Boolean field_is_set(struct ptp_message *m, int index, Octet bit)
 	return m->header.flagField[index] & bit ? TRUE : FALSE;
 }
 
+/**
+ * Append a new TLV onto a message for transmission.
+ *
+ * This is a high level API designed for the transmit path.  The
+ * function allocates a new descriptor, initializes its .tlv field,
+ * and ensures that the TLV will fit into the message buffer.  This
+ * function increments the message length field by 'length' before
+ * returning.
+ *
+ * @param msg     A message obtained using msg_allocate().  At a mininum,
+ *                the message type and length fields must set by the caller.
+ * @param length  The length of the TLV to append.
+ * @return        A pointer to a TLV descriptor on success or NULL otherwise.
+ */
+struct tlv_extra *msg_tlv_append(struct ptp_message *msg, int length);
+
 /**
  * Place a TLV descriptor into a message's list of TLVs.
  *