lldp: add support for organizationally specific TLVs

LLDP TLVs of type 127 are used to carry organizationally specific
information and include additional fields to specify the OUI and
subtype.

Add support for parsing such fields and functions to access the most
common IEEE 802.1 specific TLVs.

src/libsystemd-network/lldp-tlv.c

@@ -277,7 +277,7 @@ int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
 
         p = m->pdu;
 
-        /* extract ethernet herader */
+        /* extract ethernet header */
         memcpy(&m->mac, p, ETH_ALEN);
         p += sizeof(struct ether_header);
 
@@ -297,6 +297,17 @@ int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
                 }
 
                 p += 2;
+
+                if (section->type == LLDP_TYPE_PRIVATE &&
+                    section->length >= LLDP_OUI_LEN + 1) {
+                        section->oui = p;
+                        p += LLDP_OUI_LEN;
+                        section->subtype = *p++;
+
+                        section->length -= LLDP_OUI_LEN + 1;
+                        l += LLDP_OUI_LEN + 1;
+                }
+
                 section->data = p;
 
                 LIST_FIND_TAIL(section, m->sections, tail);
@@ -313,6 +324,7 @@ int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
         tlv_section *s;
 
         assert_return(m, -EINVAL);
+        assert_return(type != LLDP_TYPE_PRIVATE, -EINVAL);
 
         LIST_FOREACH(section, s, m->sections)
                 if (s->type == type)
@@ -331,6 +343,34 @@ int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
         return 0;
 }
 
+int lldp_tlv_packet_enter_container_oui(tlv_packet *m, const uint8_t *oui, uint8_t subtype) {
+        tlv_section *s;
+
+        assert_return(m, -EINVAL);
+        assert_return(oui, -EINVAL);
+
+        LIST_FOREACH(section, s, m->sections) {
+                if (s->type == LLDP_TYPE_PRIVATE &&
+                    s->oui &&
+                    s->subtype == subtype &&
+                    !memcmp(s->oui, oui, LLDP_OUI_LEN))
+                        break;
+        }
+
+        if (!s)
+                return -1;
+
+        m->container = s;
+
+        m->container->read_pos = s->data;
+        if (!m->container->read_pos) {
+                m->container = 0;
+                return -1;
+        }
+
+        return 0;
+}
+
 int lldp_tlv_packet_exit_container(tlv_packet *m) {
         assert_return(m, -EINVAL);
 
@@ -495,6 +535,103 @@ int sd_lldp_packet_read_system_capability(tlv_packet *tlv, uint16_t *data) {
         return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_SYSTEM_CAPABILITIES, data);
 }
 
+int sd_lldp_packet_read_port_vlan_id(tlv_packet *tlv, uint16_t *id) {
+        int r;
+
+        assert_return(tlv, -EINVAL);
+
+        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID);
+        if (r < 0)
+                goto out;
+
+        r = tlv_packet_read_u16(tlv, id);
+
+        (void) lldp_tlv_packet_exit_container(tlv);
+
+ out:
+        return r;
+}
+
+int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id) {
+        int r;
+
+        assert_return(tlv, -EINVAL);
+
+        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID);
+        if (r < 0)
+                goto out;
+
+        r = tlv_packet_read_u8(tlv, flags);
+        if (r >= 0)
+                r = tlv_packet_read_u16(tlv, id);
+
+        (void) lldp_tlv_packet_exit_container(tlv);
+
+ out:
+        return r;
+}
+
+int sd_lldp_packet_read_vlan_name(tlv_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length) {
+        int r;
+        uint8_t len = 0;
+
+        assert_return(tlv, -EINVAL);
+
+        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME);
+        if (r < 0)
+                goto out;
+
+        r = tlv_packet_read_u16(tlv, vlan_id);
+        if (r >= 0)
+                r = tlv_packet_read_u8(tlv, &len);
+        if (r >= 0)
+                r = tlv_packet_read_string(tlv, name, length);
+
+        if (r >= 0 && len < *length)
+                *length = len;
+
+        (void) lldp_tlv_packet_exit_container(tlv);
+
+ out:
+        return r;
+}
+
+int sd_lldp_packet_read_management_vid(tlv_packet *tlv, uint16_t *id) {
+        int r;
+
+        assert_return(tlv, -EINVAL);
+
+        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID);
+        if (r < 0)
+                goto out;
+
+        r = tlv_packet_read_u16(tlv, id);
+
+        (void) lldp_tlv_packet_exit_container(tlv);
+
+ out:
+        return r;
+}
+
+int sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id) {
+        int r;
+
+        assert_return(tlv, -EINVAL);
+
+        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION);
+        if (r < 0)
+                goto out;
+
+        r = tlv_packet_read_u8(tlv, status);
+        if (r >= 0)
+                r = tlv_packet_read_u32(tlv, id);
+
+        (void) lldp_tlv_packet_exit_container(tlv);
+
+ out:
+        return r;
+}
+
 int sd_lldp_packet_get_destination_type(tlv_packet *tlv, int *dest) {
         assert_return(tlv, -EINVAL);
         assert_return(dest, -EINVAL);

src/libsystemd-network/lldp-tlv.h

@@ -33,9 +33,13 @@
 typedef struct tlv_packet tlv_packet;
 typedef struct tlv_section tlv_section;
 
+#define LLDP_OUI_LEN 3
+
 struct tlv_section {
         uint16_t type;
         uint16_t length;
+        uint8_t *oui;
+        uint8_t subtype;
 
         uint8_t *read_pos;
         uint8_t *data;
@@ -83,6 +87,7 @@ int tlv_packet_append_u32(tlv_packet *m, uint32_t data);
 int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size);
 
 int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type);
+int lldp_tlv_packet_enter_container_oui(tlv_packet *m, const uint8_t *oui, uint8_t subtype);
 int lldp_tlv_packet_exit_container(tlv_packet *m);
 
 int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length);

src/libsystemd-network/lldp.h

@@ -113,3 +113,16 @@ typedef enum LLDPMedCapability {
         LLDP_MED_CAPABILITY_MAX,
         LLDP_MED_CAPABILITY_INVALID        = -1,
 } LLDPMedCapability;
+
+#define LLDP_OUI_802_1 (uint8_t[]) { 0x00, 0x80, 0xc2 }
+#define LLDP_OUI_802_3 (uint8_t[]) { 0x00, 0x12, 0x0f }
+
+enum {
+        LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID            = 1,
+        LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID   = 2,
+        LLDP_OUI_SUBTYPE_802_1_VLAN_NAME               = 3,
+        LLDP_OUI_SUBTYPE_802_1_PROTOCOL_IDENTITY       = 4,
+        LLDP_OUI_SUBTYPE_802_1_VID_USAGE_DIGEST        = 5,
+        LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID          = 6,
+        LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION        = 7,
+};

src/libsystemd-network/sd-lldp.c

@@ -199,7 +199,7 @@ int lldp_handle_packet(tlv_packet *tlv, uint16_t length) {
                         goto out;
                 }
 
-                /* skip type and lengh encoding */
+                /* skip type and length encoding */
                 p += 2;
                 q = p;
 

src/systemd/sd-lldp.h

@@ -59,6 +59,13 @@ int sd_lldp_packet_read_system_description(sd_lldp_packet *tlv, char **data, uin
 int sd_lldp_packet_read_system_capability(sd_lldp_packet *tlv, uint16_t *data);
 int sd_lldp_packet_read_port_description(sd_lldp_packet *tlv, char **data, uint16_t *length);
 
+/* IEEE 802.1 organizationally specific TLVs */
+int sd_lldp_packet_read_port_vlan_id(sd_lldp_packet *tlv, uint16_t *id);
+int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id);
+int sd_lldp_packet_read_vlan_name(sd_lldp_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length);
+int sd_lldp_packet_read_management_vid(sd_lldp_packet *tlv, uint16_t *id);
+int sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id);
+
 sd_lldp_packet *sd_lldp_packet_ref(sd_lldp_packet *tlv);
 sd_lldp_packet *sd_lldp_packet_unref(sd_lldp_packet *tlv);