src/libsystemd-network/lldp-tlv.c

ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani/***
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  This file is part of systemd.
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  Copyright (C) 2014 Tom Gundersen
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  Copyright (C) 2014 Susant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  systemd is free software; you can redistribute it and/or modify it
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  under the terms of the GNU Lesser General Public License as published by
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  the Free Software Foundation; either version 2.1 of the License, or
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  (at your option) any later version.
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  systemd is distributed in the hope that it will be useful, but
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  WITHOUT ANY WARRANTY; without even the implied warranty of
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  Lesser General Public License for more details.
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  You should have received a copy of the GNU Lesser General Public License
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani  along with systemd; If not, see <http://www.gnu.org/licenses/>.
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani***/
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani#include <arpa/inet.h>
cf0fbc49e67b55f8d346fc94de28c90113505297Thomas Hindoe Paaboel Andersen#include <net/ethernet.h>
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
b5efdb8af40ea759a1ea584c1bc44ecc81dd00ceLennart Poettering#include "alloc-util.h"
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani#include "lldp-tlv.h"
b5efdb8af40ea759a1ea584c1bc44ecc81dd00ceLennart Poettering#include "macro.h"
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_section_new(tlv_section **ret) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        tlv_section *s;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        s = new0(tlv_section, 1);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!s)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return -ENOMEM;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *ret = s;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahanivoid tlv_section_free(tlv_section *m) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!m)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        free(m);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_new(tlv_packet **ret) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        tlv_packet *m;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m = new0(tlv_packet, 1);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!m)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return -ENOMEM;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        LIST_HEAD_INIT(m->sections);
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        m->n_ref = 1;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *ret = m;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
176c355b43e616b61552566303ad59d5fd910333Beniamino Galvanitlv_packet *sd_lldp_packet_ref(tlv_packet *m) {
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        if (!m)
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani                return NULL;
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        assert(m->n_ref > 0);
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        m->n_ref++;
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        return m;
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani}
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
176c355b43e616b61552566303ad59d5fd910333Beniamino Galvanitlv_packet *sd_lldp_packet_unref(tlv_packet *m) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        tlv_section *s, *n;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!m)
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani                return NULL;
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        assert(m->n_ref > 0);
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        m->n_ref--;
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        if (m->n_ref > 0)
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani                return m;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        LIST_FOREACH_SAFE(section, s, n, m->sections)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                tlv_section_free(s);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        free(m);
2212d76d08f3bc34c683aed1a6736325b841625cBeniamino Galvani        return NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint8_t *p;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(data, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(data_length, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (m->length + data_length > ETHER_MAX_LEN)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return -ENOMEM;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        p = m->pdu + m->length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(p, data, data_length);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->length += data_length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint16_t type;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        type = htons(data);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint32_t type;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        type = htonl(data);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return tlv_packet_append_bytes(m, data, size);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container_pos = m->pdu + m->length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return tlv_packet_append_u16(m, type << 9);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint lldp_tlv_packet_close_container(tlv_packet *m) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint16_t type;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m->container_pos, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(&type, m->container_pos, sizeof(uint16_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(m->container_pos, &type, sizeof(uint16_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahanistatic inline int tlv_packet_read_internal(tlv_section *m, void **data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m->read_pos, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *data = m->read_pos;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
a7f7d1bde43fc825c49afea3f946f5b4b3d563e0Harald Hoyer        void *val = NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        r = tlv_packet_read_internal(m->container,  &val);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(data, val, sizeof(uint8_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container->read_pos ++;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint16_t t;
a7f7d1bde43fc825c49afea3f946f5b4b3d563e0Harald Hoyer        void *val = NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        r = tlv_packet_read_internal(m->container, &val);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(&t, val, sizeof(uint16_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *data = ntohs(t);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container->read_pos += 2;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint32_t t;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        void *val;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        r = tlv_packet_read_internal(m->container, &val);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(&t, val, sizeof(uint32_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *data = ntohl(t);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container->read_pos += 4;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
a7f7d1bde43fc825c49afea3f946f5b4b3d563e0Harald Hoyer        void *val = NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        r = tlv_packet_read_internal(m->container, &val);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *data = (char *) val;
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani        *data_length = m->container->data + m->container->length - m->container->read_pos;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani        m->container->read_pos += *data_length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
a7f7d1bde43fc825c49afea3f946f5b4b3d563e0Harald Hoyer        void *val = NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        r = tlv_packet_read_internal(m->container, &val);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        *data = (uint8_t *) val;
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani        *data_length = m->container->data + m->container->length - m->container->read_pos;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani        m->container->read_pos += *data_length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani/* parse raw TLV packet */
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        tlv_section *section, *tail;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint16_t t, l;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        uint8_t *p;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        int r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(size, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        p = m->pdu;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
a8eaaee72a2f06e0fb64fb71de3b71ecba31dafbJan Engelhardt        /* extract Ethernet header */
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        memcpy(&m->mac, p, ETH_ALEN);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        p += sizeof(struct ether_header);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        for (l = 0; l <= size; ) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                r = tlv_section_new(&section);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                if (r < 0)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                        return r;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                memcpy(&t, p, sizeof(uint16_t));
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                section->type = ntohs(t) >> 9;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                section->length = ntohs(t) & 0x01ff;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                        tlv_section_free(section);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                        break;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                }
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                p += 2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                if (section->type == LLDP_TYPE_PRIVATE &&
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                    section->length >= LLDP_OUI_LEN + 1) {
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        section->oui = p;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        p += LLDP_OUI_LEN;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        section->subtype = *p++;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        section->length -= LLDP_OUI_LEN + 1;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        l += LLDP_OUI_LEN + 1;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                }
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                section->data = p;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                LIST_FIND_TAIL(section, m->sections, tail);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                LIST_INSERT_AFTER(section, m->sections, tail, section);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                p += section->length;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                l += (section->length + 2);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        }
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        tlv_section *s;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(type != LLDP_TYPE_PRIVATE, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        LIST_FOREACH(section, s, m->sections)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                if (s->type == type)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                        break;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!s)
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return -1;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container = s;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container->read_pos = s->data;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        if (!m->container->read_pos) {
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani                m->container = NULL;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani                return -1;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        }
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint lldp_tlv_packet_enter_container_oui(tlv_packet *m, const uint8_t *oui, uint8_t subtype) {
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        tlv_section *s;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(m, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(oui, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        LIST_FOREACH(section, s, m->sections) {
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                if (s->type == LLDP_TYPE_PRIVATE &&
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                    s->oui &&
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                    s->subtype == subtype &&
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                    !memcmp(s->oui, oui, LLDP_OUI_LEN))
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                        break;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        }
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (!s)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                return -1;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        m->container = s;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        m->container->read_pos = s->data;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (!m->container->read_pos) {
29eca2ff897dfbac65ef401515effb055da65339Beniamino Galvani                m->container = NULL;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                return -1;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        }
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        return 0;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahaniint lldp_tlv_packet_exit_container(tlv_packet *m) {
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        assert_return(m, -EINVAL);
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        m->container = 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani        return 0;
ad1ad5c8e36ea795034fcdac660b15d7c141d55bSusant Sahani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvanistatic int lldp_tlv_packet_read_u16_tlv(tlv_packet *tlv, uint16_t type, uint16_t *value) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        assert_return(tlv, -EINVAL);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        r = lldp_tlv_packet_enter_container(tlv, type);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        r = tlv_packet_read_u16(tlv, value);
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani}
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvanistatic int lldp_tlv_packet_read_string_tlv(tlv_packet *tlv, uint16_t type, char **data, uint16_t *length) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        char *s;
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        assert_return(tlv, -EINVAL);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        r = lldp_tlv_packet_enter_container(tlv, type);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        if (r < 0)
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani                return r;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        r = tlv_packet_read_string(tlv, &s, length);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        if (r < 0)
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani                goto out;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        *data = (char *) s;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani out:
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani}
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_chassis_id(tlv_packet *tlv,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                   uint8_t *type,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                   uint8_t **data,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                   uint16_t *length) {
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        uint8_t subtype;
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        assert_return(tlv, -EINVAL);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_CHASSIS_ID);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        r = tlv_packet_read_u8(tlv, &subtype);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                goto out;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        switch (subtype) {
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                r = tlv_packet_read_bytes(tlv, data, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                        goto out;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                break;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        default:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                r = -EOPNOTSUPP;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                break;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        }
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        *type = subtype;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller out:
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_port_id(tlv_packet *tlv,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                uint8_t *type,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                uint8_t **data,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                uint16_t *length) {
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        uint8_t subtype;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        char *s;
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        assert_return(tlv, -EINVAL);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_ID);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        r = tlv_packet_read_u8(tlv, &subtype);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                goto out;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        switch (subtype) {
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_PORT_SUBTYPE_PORT_COMPONENT:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_PORT_SUBTYPE_INTERFACE_ALIAS:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                r = tlv_packet_read_string(tlv, &s, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                        goto out;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                *data = (uint8_t *) s;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                break;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        case LLDP_PORT_SUBTYPE_MAC_ADDRESS:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                r = tlv_packet_read_bytes(tlv, data, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                        goto out;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                break;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        default:
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                r = -EOPNOTSUPP;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                break;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        }
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani        *type = subtype;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller out:
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_ttl(tlv_packet *tlv, uint16_t *ttl) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_TTL, ttl);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_system_name(tlv_packet *tlv,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                    char **data,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                    uint16_t *length) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_NAME, data, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_system_description(tlv_packet *tlv,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                           char **data,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                           uint16_t *length) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_DESCRIPTION, data, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_port_description(tlv_packet *tlv,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                         char **data,
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani                                         uint16_t *length) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_PORT_DESCRIPTION, data, length);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvaniint sd_lldp_packet_read_system_capability(tlv_packet *tlv, uint16_t *data) {
11e8357164694d8307dea5bcae017c98c5f125abBeniamino Galvani        return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_SYSTEM_CAPABILITIES, data);
b57003ecc3fe92c8074cbc72ee2b7d9cda742271Beniamino Galvani}
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint sd_lldp_packet_read_port_vlan_id(tlv_packet *tlv, uint16_t *id) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(tlv, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = tlv_packet_read_u16(tlv, id);
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(tlv, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = tlv_packet_read_u8(tlv, flags);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r >= 0)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                r = tlv_packet_read_u16(tlv, id);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint sd_lldp_packet_read_vlan_name(tlv_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        uint8_t len = 0;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(tlv, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = tlv_packet_read_u16(tlv, vlan_id);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r >= 0)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                r = tlv_packet_read_u8(tlv, &len);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r >= 0)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                r = tlv_packet_read_string(tlv, name, length);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r >= 0 && len < *length)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                *length = len;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint sd_lldp_packet_read_management_vid(tlv_packet *tlv, uint16_t *id) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(tlv, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = tlv_packet_read_u16(tlv, id);
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvaniint sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id) {
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        int r, r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        assert_return(tlv, -EINVAL);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r < 0)
9ac8e913246d603c612b6083beac5a5ef7e307b2Thomas Haller                return r;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        r = tlv_packet_read_u8(tlv, status);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani        if (r >= 0)
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani                r = tlv_packet_read_u32(tlv, id);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
564cabd46c7c1532ad4d562bf8332eaed49ac201Beniamino Galvani        return r < 0 ? r : r2;
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani}
d8c89d6198651e34786bc78df26f313eede9918bBeniamino Galvani
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvaniint sd_lldp_packet_get_destination_type(tlv_packet *tlv, int *dest) {
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        assert_return(tlv, -EINVAL);
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        assert_return(dest, -EINVAL);
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        /* 802.1AB-2009, Table 7-1 */
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_BRIDGE, ETH_ALEN))
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE;
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_NON_TPMR_BRIDGE, ETH_ALEN))
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE;
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_CUSTOMER_BRIDGE, ETH_ALEN))
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE;
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        else
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani                return -EINVAL;
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani        return 0;
4fc6de5df31c81ae35e82ed91d0a2ee515edad41Beniamino Galvani}