From 4059e2ef7f277b962dc4a93bd2df9445361c7cca Mon Sep 17 00:00:00 2001
From: Valery Ivanov <ivalery111@gmail.com>
Date: Fri, 7 May 2021 19:28:25 +0300
Subject: feat(functions): add libnet_build_lldp_chassis function

---
 include/libnet/libnet-functions.h | 15 +++++++
 src/libnet_build_lldp.c           | 83 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 98 insertions(+)
 create mode 100644 src/libnet_build_lldp.c

diff --git a/include/libnet/libnet-functions.h b/include/libnet/libnet-functions.h
index 357c363..d8b5b67 100644
--- a/include/libnet/libnet-functions.h
+++ b/include/libnet/libnet-functions.h
@@ -731,6 +731,21 @@ libnet_build_cdp(uint8_t version, uint8_t ttl, uint16_t sum, uint16_t type,
 uint16_t value_s, const uint8_t *value, const uint8_t* payload, uint32_t payload_s,
 libnet_t *l, libnet_ptag_t ptag);
 
+/**
+ * Builds a LLDP Chassis ID TLV. The Chassis ID TLV is the _first_ mandatory TLV in the LLDPDU.
+ * @param subtype Chassis ID Subtype
+ * @param value the Chassis ID string
+ * @param value_s length of value argument
+ * @param l pointer to a libnet context
+ * @param ptag protocol tag to modify an existing header, 0 to build a new one
+ * @return protocol tag value on success
+ * @retval -1 on error
+ */
+LIBNET_API
+libnet_ptag_t libnet_build_lldp_chassis(const uint8_t subtype,
+const uint8_t *const value, const uint8_t value_s,
+libnet_t *l, libnet_ptag_t ptag);
+
 /**
  * Builds an IP version 4 RFC 792 Internet Control Message Protocol (ICMP)
  * echo request/reply header
diff --git a/src/libnet_build_lldp.c b/src/libnet_build_lldp.c
new file mode 100644
index 0000000..14f011c
--- /dev/null
+++ b/src/libnet_build_lldp.c
@@ -0,0 +1,83 @@
+#include "common.h"
+
+LIBNET_API
+libnet_ptag_t libnet_build_lldp_chassis(const uint8_t subtype,
+const uint8_t *const value,const uint8_t value_s, libnet_t *l,
+libnet_ptag_t ptag)
+{
+  assert(value != NULL && "Chassis ID string cannot be a NULL");
+  assert((value_s != 0) && "Chassis ID string length must be greater or equal to 1");
+  assert(LIBNET_LLDP_TLV_HDR_SIZE == 0x02 && "TLV header size must be a 2 bytes");
+  assert(LIBNET_LLDP_SUBTYPE_SIZE == 0x01 && "Subtype field size must be 1 byte");
+
+  uint32_t n, h;
+  libnet_pblock_t *p;
+  struct libnet_lldp_hdr hdr;
+  memset(&hdr, 0, sizeof(struct libnet_lldp_hdr));
+
+  if (l == NULL)
+  {
+    return (-1);
+  }
+
+  if(value == NULL)
+  {
+    snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
+    "%s(): Chassis ID string is NULL", __func__);
+    return (-1);
+  }
+
+  if(value_s == 0)
+  {
+      snprintf(l->err_buf, LIBNET_ERRBUF_SIZE,
+      "%s(): Incorrect Chassis ID string length", __func__);
+      return (-1);
+  }
+
+  /* size of memory block */
+  n = h =  LIBNET_LLDP_TLV_HDR_SIZE + /* TLV Header size */
+           LIBNET_LLDP_SUBTYPE_SIZE + /* Chassis ID subtype size */
+           value_s;                   /* Chassis ID string length */
+
+  LIBNET_LLDP_TLV_SET_TYPE(hdr.tlv_info, LIBNET_LLDP_CHASSIS_ID);
+  LIBNET_LLDP_TLV_SET_LEN(hdr.tlv_info, value_s + LIBNET_LLDP_SUBTYPE_SIZE);
+
+  /*
+   *  Find the existing protocol block if a ptag is specified, or create
+   *  a new one.
+   */
+  p = libnet_pblock_probe(l, ptag, n, LIBNET_PBLOCK_LLDP_CHASSIS_H);
+  if (p == NULL)
+  {
+    return (-1);
+  }
+
+  const uint16_t type_and_len = htons(hdr.tlv_info);
+  if (libnet_pblock_append(l, p, &type_and_len, sizeof(type_and_len)) == -1)
+  {
+    goto bad;
+  }
+
+  if (libnet_pblock_append(l, p, &subtype, sizeof(subtype)) == -1)
+  {
+    goto bad;
+  }
+
+  if (libnet_pblock_append(l, p, value, value_s) == -1)
+  {
+    goto bad;
+  }
+
+  return (ptag ? ptag
+               : libnet_pblock_update(l, p, h, LIBNET_PBLOCK_LLDP_CHASSIS_H));
+bad:
+  libnet_pblock_delete(l, p);
+  return (-1);
+}
+
+/**
+ * Local Variables:
+ *  indent-tabs-mode: nil
+ *  c-file-style: "stroustrup"
+ * End:
+ */
-- 
cgit v1.2.1