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);