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 <richardcoch...@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
@@ -259,6 +259,22 @@ static inline Boolean field_is_set(struct ptp_message *m,
int index, Octet bit)
}
/**
+ * 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.
*
* @param msg A message obtained using msg_allocate().
--
2.11.0
------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel
