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.

4 files changed, 157 insertions, 2 deletions

diff --git a/src/libsystemd-network/lldp-tlv.c b/src/libsystemd-network/lldp-tlv.c
index b967f34a99..b3f0bfb1e9 100644
--- a/src/libsystemd-network/lldp-tlv.c
+++ b/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);
diff --git a/src/libsystemd-network/lldp-tlv.h b/src/libsystemd-network/lldp-tlv.h
index 5af06b40ca..2d2c776be6 100644
--- a/src/libsystemd-network/lldp-tlv.h
+++ b/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);
diff --git a/src/libsystemd-network/lldp.h b/src/libsystemd-network/lldp.h
index 5e4b283e26..19e5cc5f41 100644
--- a/src/libsystemd-network/lldp.h
+++ b/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,
+};
diff --git a/src/libsystemd-network/sd-lldp.c b/src/libsystemd-network/sd-lldp.c
index a343370e96..7aa405a655 100644
--- a/src/libsystemd-network/sd-lldp.c
+++ b/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;