daemon: add basic custom TLV send & receive

Signed-off-by: Alexandru Ardelean <ardeleanalex@gmail.com>

diff --git a/src/daemon/lldpd.c b/src/daemon/lldpd.c
index 8ba2071017f5cb2a136ef3db3cc0480e1e55d02f..782e9232e1cbc5ac75982b45b6e92c7daaafd3d6 100644 (file)
--- a/src/daemon/lldpd.c
+++ b/src/daemon/lldpd.c
@@ -169,6 +169,7 @@ lldpd_alloc_hardware(struct lldpd *cfg, char *name, int index)
        TAILQ_INIT(&hardware->h_lport.p_ppvids);
        TAILQ_INIT(&hardware->h_lport.p_pids);
 #endif
+       TAILQ_INIT(&hardware->h_lport.p_custom_list);
 
        levent_hardware_init(hardware);
        return hardware;

diff --git a/src/daemon/protocols/lldp.c b/src/daemon/protocols/lldp.c
index 18055995cf4f004d7d4a5e2091cee333ae1e0a03..c4a30a41b84f6da599beb86a17b18b333fa77abd 100644 (file)
--- a/src/daemon/protocols/lldp.c
+++ b/src/daemon/protocols/lldp.c
@@ -84,6 +84,7 @@ static int _lldp_send(struct lldpd *global,
        int i;
        const u_int8_t med[] = LLDP_TLV_ORG_MED;
 #endif
+       struct lldpd_custom *custom;
        port = &hardware->h_lport;
        chassis = port->p_chassis;
        length = hardware->h_mtu;
@@ -431,6 +432,16 @@ static int _lldp_send(struct lldpd *global,
        }
 #endif
 
+       TAILQ_FOREACH(custom, &port->p_custom_list, next) {
+               if (!(
+                     POKE_START_LLDP_TLV(LLDP_TLV_ORG) &&
+                     POKE_BYTES(custom->oui, sizeof(custom->oui)) &&
+                     POKE_UINT8(custom->subtype) &&
+                     POKE_BYTES(custom->oui_info, custom->oui_info_len) &&
+                     POKE_END_LLDP_TLV))
+                       goto toobig;
+       }
+
 end:
        /* END */
        if (!(
@@ -579,6 +590,7 @@ lldp_decode(struct lldpd *cfg, char *frame, int s,
        u_int8_t addr_str_length, addr_str_buffer[32];
        u_int8_t addr_family, addr_length, *addr_ptr, iface_subtype;
        u_int32_t iface_number, iface;
+       struct lldpd_custom *custom;
 
        log_debug("lldp", "receive LLDP PDU on %s",
            hardware->h_ifname);
@@ -598,6 +610,7 @@ lldp_decode(struct lldpd *cfg, char *frame, int s,
        TAILQ_INIT(&port->p_ppvids);
        TAILQ_INIT(&port->p_pids);
 #endif
+       TAILQ_INIT(&port->p_custom_list);
 
        length = s;
        pos = (u_int8_t*)frame;
@@ -1078,6 +1091,15 @@ lldp_decode(struct lldpd *cfg, char *frame, int s,
                                    orgid[0], orgid[1], orgid[2],
                                    hardware->h_ifname);
                                hardware->h_rx_unrecognized_cnt++;
+                               custom = (struct lldpd_custom*)calloc(1, sizeof(struct lldpd_custom));
+                               if (!custom)
+                                       return ENOMEM;
+                               custom->oui_info_len = tlv_size > 4 ? tlv_size - 4 : 0;
+                               memcpy(custom->oui, orgid, sizeof(custom->oui));
+                               custom->subtype = tlv_subtype;
+                               if (custom->oui_info_len > 0)
+                                       PEEK_BYTES(custom->oui_info, custom->oui_info_len);
+                               TAILQ_INSERT_TAIL(&port->p_custom_list, custom, next);
                        }
                        break;
                default:

diff --git a/src/lldp-const.h b/src/lldp-const.h
index 81a227a4377cce3429134b343a2c216a18314bf7..509b08fd5f7e2064cb76ac0bd49628d53007f76c 100644 (file)
--- a/src/lldp-const.h
+++ b/src/lldp-const.h
@@ -24,6 +24,8 @@
  */
 
 #define LLDP_TLV_ORG                   127
+#define LLDP_TLV_ORG_OUI_LEN           3
+#define LLDP_TLV_ORG_OUI_INFO_MAXLEN   507
 
 /* Chassis ID subtype */
 #define LLDP_CHASSISID_SUBTYPE_CHASSIS 1

diff --git a/src/lldpd-structs.c b/src/lldpd-structs.c
index e06428773be239ff6b4e50e8c5d39f25b8d7f08e..f713abdfa7595bd8052181e76111d0ffad5f689d 100644 (file)
--- a/src/lldpd-structs.c
+++ b/src/lldpd-structs.c
@@ -103,6 +103,19 @@ lldpd_pi_cleanup(struct lldpd_port *port)
 }
 #endif
 
+void 
+lldpd_custom_list_cleanup(struct lldpd_port *port)
+{
+       struct lldpd_custom *custom, *custom_next;
+       for (custom = TAILQ_FIRST(&port->p_custom_list);
+           custom != NULL;
+           custom = custom_next) {
+               custom_next = TAILQ_NEXT(custom, next);
+               free(custom);
+       }
+       TAILQ_INIT(&port->p_custom_list);
+}
+
 /* Cleanup a remote port. The before last argument, `expire` is a function that
  * should be called when a remote port is removed. If the last argument is 1,
  * all remote ports are removed.
@@ -171,6 +184,7 @@ lldpd_port_cleanup(struct lldpd_port *port, int all)
                        port->p_chassis->c_refcount--;
                        port->p_chassis = NULL;
                }
+               lldpd_custom_list_cleanup(port);
        }
 }
 

diff --git a/src/lldpd-structs.h b/src/lldpd-structs.h
index 43795de7943fc64b09890de509a96d673efd034a..8c91d2d1003f219306c9cef707951b97bbd1e346 100644 (file)
--- a/src/lldpd-structs.h
+++ b/src/lldpd-structs.h
@@ -214,6 +214,22 @@ MARSHAL_STR(lldpd_chassis, c_med_asset)
 #endif
 MARSHAL_END(lldpd_chassis);
 
+/* Custom TLV struct as defined on page 35 of IEEE 802.1AB-2005 */
+struct lldpd_custom {
+       TAILQ_ENTRY(lldpd_custom)       next;   /* Pointer to next custom TLV */
+
+       /* Organizationally Unique Identifier */
+       u_int8_t                oui[LLDP_TLV_ORG_OUI_LEN];
+       /* Organizationally Defined Subtype */
+       u_int8_t                subtype;
+       /* Organizationally Defined Information String; for now/simplicity static array */
+       u_int8_t                oui_info[LLDP_TLV_ORG_OUI_INFO_MAXLEN];
+       /* Organizationally Defined Information String length */
+       int                     oui_info_len;
+};
+MARSHAL_BEGIN(lldpd_custom)
+MARSHAL_TQE(lldpd_custom, next)
+MARSHAL_END(lldpd_custom);
 
 struct lldpd_port {
        TAILQ_ENTRY(lldpd_port)  p_entries;
@@ -253,6 +269,7 @@ struct lldpd_port {
        TAILQ_HEAD(, lldpd_ppvid) p_ppvids;
        TAILQ_HEAD(, lldpd_pi)    p_pids;
 #endif
+       TAILQ_HEAD(, lldpd_custom) p_custom_list;
 };
 MARSHAL_BEGIN(lldpd_port)
 MARSHAL_TQE(lldpd_port, p_entries)
@@ -270,6 +287,7 @@ MARSHAL_SUBTQ(lldpd_port, lldpd_vlan, p_vlans)
 MARSHAL_SUBTQ(lldpd_port, lldpd_ppvid, p_ppvids)
 MARSHAL_SUBTQ(lldpd_port, lldpd_pi, p_pids)
 #endif
+MARSHAL_SUBTQ(lldpd_port, lldpd_custom, p_custom_list)
 MARSHAL_END(lldpd_port);
 
 /* Used to modify some port related settings */
@@ -483,5 +501,6 @@ void         lldpd_ppvid_cleanup(struct lldpd_port *);
 void    lldpd_vlan_cleanup(struct lldpd_port *);
 void    lldpd_pi_cleanup(struct lldpd_port *);
 #endif
+void     lldpd_custom_list_cleanup(struct lldpd_port *);
 
 #endif