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.

1 files changed, 138 insertions, 1 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);