[thirdparty/systemd.git] / src / libsystemd-network / lldp-tlv.c

/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/

/***
  This file is part of systemd.

  Copyright (C) 2014 Tom Gundersen
  Copyright (C) 2014 Susant Sahani

  systemd is free software; you can redistribute it and/or modify it
  under the terms of the GNU Lesser General Public License as published by
  the Free Software Foundation; either version 2.1 of the License, or
  (at your option) any later version.

  systemd is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public License
  along with systemd; If not, see <http://www.gnu.org/licenses/>.
***/

#include <net/ethernet.h>
#include <arpa/inet.h>

#include "alloc-util.h"
#include "lldp-tlv.h"
#include "macro.h"

int tlv_section_new(tlv_section **ret) {
        tlv_section *s;

        s = new0(tlv_section, 1);
        if (!s)
                return -ENOMEM;

        *ret = s;

        return 0;
}

void tlv_section_free(tlv_section *m) {

        if (!m)
                return;

        free(m);
}

int tlv_packet_new(tlv_packet **ret) {
        tlv_packet *m;

        m = new0(tlv_packet, 1);
        if (!m)
                return -ENOMEM;

        LIST_HEAD_INIT(m->sections);
        m->n_ref = 1;

        *ret = m;

        return 0;
}

tlv_packet *sd_lldp_packet_ref(tlv_packet *m) {

        if (!m)
                return NULL;

        assert(m->n_ref > 0);
        m->n_ref++;

        return m;
}

tlv_packet *sd_lldp_packet_unref(tlv_packet *m) {
        tlv_section *s, *n;

        if (!m)
                return NULL;

        assert(m->n_ref > 0);
        m->n_ref--;

        if (m->n_ref > 0)
                return m;

        LIST_FOREACH_SAFE(section, s, n, m->sections)
                tlv_section_free(s);

        free(m);
        return NULL;
}

int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
        uint8_t *p;

        assert_return(m, -EINVAL);
        assert_return(data, -EINVAL);
        assert_return(data_length, -EINVAL);

        if (m->length + data_length > ETHER_MAX_LEN)
                return -ENOMEM;

        p = m->pdu + m->length;
        memcpy(p, data, data_length);
        m->length += data_length;

        return 0;
}

int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {

        assert_return(m, -EINVAL);

        return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
}

int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
        uint16_t type;

        assert_return(m, -EINVAL);

        type = htons(data);

        return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
}

int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
        uint32_t type;

        assert_return(m, -EINVAL);

        type = htonl(data);

        return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
}

int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {

        assert_return(m, -EINVAL);

        return tlv_packet_append_bytes(m, data, size);
}

int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {

        assert_return(m, -EINVAL);

        m->container_pos = m->pdu + m->length;

        return tlv_packet_append_u16(m, type << 9);
}

int lldp_tlv_packet_close_container(tlv_packet *m) {
        uint16_t type;

        assert_return(m, -EINVAL);
        assert_return(m->container_pos, -EINVAL);

        memcpy(&type, m->container_pos, sizeof(uint16_t));

        type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
        memcpy(m->container_pos, &type, sizeof(uint16_t));

        return 0;
}

static inline int tlv_packet_read_internal(tlv_section *m, void **data) {

        assert_return(m->read_pos, -EINVAL);

        *data = m->read_pos;

        return 0;
}

int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
        void *val = NULL;
        int r;

        assert_return(m, -EINVAL);

        r = tlv_packet_read_internal(m->container,  &val);
        if (r < 0)
                return r;

        memcpy(data, val, sizeof(uint8_t));

        m->container->read_pos ++;

        return 0;
}

int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
        uint16_t t;
        void *val = NULL;
        int r;

        assert_return(m, -EINVAL);

        r = tlv_packet_read_internal(m->container, &val);
        if (r < 0)
                return r;

        memcpy(&t, val, sizeof(uint16_t));
        *data = ntohs(t);

        m->container->read_pos += 2;

        return 0;
}

int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
        uint32_t t;
        void *val;
        int r;

        assert_return(m, -EINVAL);

        r = tlv_packet_read_internal(m->container, &val);
        if (r < 0)
                return r;

        memcpy(&t, val, sizeof(uint32_t));
        *data = ntohl(t);

        m->container->read_pos += 4;

        return r;
}

int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
        void *val = NULL;
        int r;

        assert_return(m, -EINVAL);

        r = tlv_packet_read_internal(m->container, &val);
        if (r < 0)
                return r;

        *data = (char *) val;
        *data_length = m->container->data + m->container->length - m->container->read_pos;

        m->container->read_pos += *data_length;

        return 0;
}

int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
        void *val = NULL;
        int r;

        assert_return(m, -EINVAL);

        r = tlv_packet_read_internal(m->container, &val);
        if (r < 0)
                return r;

        *data = (uint8_t *) val;
        *data_length = m->container->data + m->container->length - m->container->read_pos;

        m->container->read_pos += *data_length;

        return 0;
}

/* parse raw TLV packet */
int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
        tlv_section *section, *tail;
        uint16_t t, l;
        uint8_t *p;
        int r;

        assert_return(m, -EINVAL);
        assert_return(size, -EINVAL);

        p = m->pdu;

        /* extract Ethernet header */
        memcpy(&m->mac, p, ETH_ALEN);
        p += sizeof(struct ether_header);

        for (l = 0; l <= size; ) {
                r = tlv_section_new(&section);
                if (r < 0)
                        return r;

                memcpy(&t, p, sizeof(uint16_t));

                section->type = ntohs(t) >> 9;
                section->length = ntohs(t) & 0x01ff;

                if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
                        tlv_section_free(section);
                        break;
                }

                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);
                LIST_INSERT_AFTER(section, m->sections, tail, section);

                p += section->length;
                l += (section->length + 2);
        }

        return 0;
}

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)
                        break;
        if (!s)
                return -1;

        m->container = s;

        m->container->read_pos = s->data;
        if (!m->container->read_pos) {
                m->container = NULL;
                return -1;
        }

        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 = NULL;
                return -1;
        }

        return 0;
}

int lldp_tlv_packet_exit_container(tlv_packet *m) {
        assert_return(m, -EINVAL);

        m->container = 0;

        return 0;
}

static int lldp_tlv_packet_read_u16_tlv(tlv_packet *tlv, uint16_t type, uint16_t *value) {
        int r, r2;

        assert_return(tlv, -EINVAL);

        r = lldp_tlv_packet_enter_container(tlv, type);
        if (r < 0)
                goto out;

        r = tlv_packet_read_u16(tlv, value);
        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

static int lldp_tlv_packet_read_string_tlv(tlv_packet *tlv, uint16_t type, char **data, uint16_t *length) {
        char *s;
        int r, r2;

        assert_return(tlv, -EINVAL);

        r = lldp_tlv_packet_enter_container(tlv, type);
        if (r < 0)
                return r;

        r = tlv_packet_read_string(tlv, &s, length);
        if (r < 0)
                goto out;

        *data = (char *) s;

 out:
        r2 = lldp_tlv_packet_exit_container(tlv);

        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_chassis_id(tlv_packet *tlv,
                                   uint8_t *type,
                                   uint8_t **data,
                                   uint16_t *length) {
        uint8_t subtype;
        int r, r2;

        assert_return(tlv, -EINVAL);

        r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_CHASSIS_ID);
        if (r < 0)
                goto out2;

        r = tlv_packet_read_u8(tlv, &subtype);
        if (r < 0)
                goto out1;

        switch (subtype) {
        case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:

                r = tlv_packet_read_bytes(tlv, data, length);
                if (r < 0)
                        goto out1;

                break;
        default:
                r = -EOPNOTSUPP;
                break;
        }

        *type = subtype;

 out1:
        r2 = lldp_tlv_packet_exit_container(tlv);

 out2:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_port_id(tlv_packet *tlv,
                                uint8_t *type,
                                uint8_t **data,
                                uint16_t *length) {
        uint8_t subtype;
        char *s;
        int r, r2;

        assert_return(tlv, -EINVAL);

        r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_ID);
        if (r < 0)
                goto out2;

        r = tlv_packet_read_u8(tlv, &subtype);
        if (r < 0)
                goto out1;

        switch (subtype) {
        case LLDP_PORT_SUBTYPE_PORT_COMPONENT:
        case LLDP_PORT_SUBTYPE_INTERFACE_ALIAS:
        case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
        case LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED:

                r = tlv_packet_read_string(tlv, &s, length);
                if (r < 0)
                        goto out1;

                *data = (uint8_t *) s;

                break;
        case LLDP_PORT_SUBTYPE_MAC_ADDRESS:

                r = tlv_packet_read_bytes(tlv, data, length);
                if (r < 0)
                        goto out1;

                break;
        default:
                r = -EOPNOTSUPP;
                break;
        }

        *type = subtype;

 out1:
        r2 = lldp_tlv_packet_exit_container(tlv);

 out2:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_ttl(tlv_packet *tlv, uint16_t *ttl) {
        return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_TTL, ttl);
}

int sd_lldp_packet_read_system_name(tlv_packet *tlv,
                                    char **data,
                                    uint16_t *length) {
        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_NAME, data, length);
}

int sd_lldp_packet_read_system_description(tlv_packet *tlv,
                                           char **data,
                                           uint16_t *length) {
        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_DESCRIPTION, data, length);
}

int sd_lldp_packet_read_port_description(tlv_packet *tlv,
                                         char **data,
                                         uint16_t *length) {
        return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_PORT_DESCRIPTION, data, length);
}

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, r2;

        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);
        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id) {
        int r, r2;

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

        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_vlan_name(tlv_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length) {
        int r, r2;
        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;

        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_management_vid(tlv_packet *tlv, uint16_t *id) {
        int r, r2;

        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);
        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id) {
        int r, r2;

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

        r2 = lldp_tlv_packet_exit_container(tlv);

 out:
        return r < 0 ? r : r2;
}

int sd_lldp_packet_get_destination_type(tlv_packet *tlv, int *dest) {
        assert_return(tlv, -EINVAL);
        assert_return(dest, -EINVAL);

        /* 802.1AB-2009, Table 7-1 */
        if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_BRIDGE, ETH_ALEN))
                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE;
        else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_NON_TPMR_BRIDGE, ETH_ALEN))
                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE;
        else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_CUSTOMER_BRIDGE, ETH_ALEN))
                *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE;
        else
                return -EINVAL;

        return 0;
}
Commit	Line	Data
ad1ad5c8 SS	1	/-- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil --/
	2
	3	/***
	4	This file is part of systemd.
	5
	6	Copyright (C) 2014 Tom Gundersen
	7	Copyright (C) 2014 Susant Sahani
	8
	9	systemd is free software; you can redistribute it and/or modify it
	10	under the terms of the GNU Lesser General Public License as published by
	11	the Free Software Foundation; either version 2.1 of the License, or
	12	(at your option) any later version.
	13
	14	systemd is distributed in the hope that it will be useful, but
	15	WITHOUT ANY WARRANTY; without even the implied warranty of
	16	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	17	Lesser General Public License for more details.
	18
	19	You should have received a copy of the GNU Lesser General Public License
	20	along with systemd; If not, see <http://www.gnu.org/licenses/>.
	21	***/
	22
	23	#include <net/ethernet.h>
	24	#include <arpa/inet.h>
	25
b5efdb8a	26	#include "alloc-util.h"
ad1ad5c8	27	#include "lldp-tlv.h"
b5efdb8a	28	#include "macro.h"
ad1ad5c8 SS	29
	30	int tlv_section_new(tlv_section **ret) {
	31	tlv_section *s;
	32
	33	s = new0(tlv_section, 1);
	34	if (!s)
	35	return -ENOMEM;
	36
	37	*ret = s;
	38
	39	return 0;
	40	}
	41
	42	void tlv_section_free(tlv_section *m) {
	43
	44	if (!m)
	45	return;
	46
	47	free(m);
	48	}
	49
	50	int tlv_packet_new(tlv_packet **ret) {
	51	tlv_packet *m;
	52
	53	m = new0(tlv_packet, 1);
	54	if (!m)
	55	return -ENOMEM;
	56
	57	LIST_HEAD_INIT(m->sections);
2212d76d	58	m->n_ref = 1;
ad1ad5c8 SS	59
	60	*ret = m;
	61
	62	return 0;
	63	}
	64
176c355b	65	tlv_packet sd_lldp_packet_ref(tlv_packet m) {
2212d76d BG	66
	67	if (!m)
	68	return NULL;
	69
	70	assert(m->n_ref > 0);
	71	m->n_ref++;
	72
	73	return m;
	74	}
	75
176c355b	76	tlv_packet sd_lldp_packet_unref(tlv_packet m) {
ad1ad5c8 SS	77	tlv_section s, n;
	78
	79	if (!m)
2212d76d BG	80	return NULL;
	81
	82	assert(m->n_ref > 0);
	83	m->n_ref--;
	84
	85	if (m->n_ref > 0)
	86	return m;
ad1ad5c8 SS	87
	88	LIST_FOREACH_SAFE(section, s, n, m->sections)
	89	tlv_section_free(s);
	90
	91	free(m);
2212d76d	92	return NULL;
ad1ad5c8 SS	93	}
	94
	95	int tlv_packet_append_bytes(tlv_packet m, const void data, size_t data_length) {
	96	uint8_t *p;
	97
	98	assert_return(m, -EINVAL);
	99	assert_return(data, -EINVAL);
	100	assert_return(data_length, -EINVAL);
	101
	102	if (m->length + data_length > ETHER_MAX_LEN)
	103	return -ENOMEM;
	104
	105	p = m->pdu + m->length;
	106	memcpy(p, data, data_length);
	107	m->length += data_length;
	108
	109	return 0;
	110	}
	111
	112	int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
	113
	114	assert_return(m, -EINVAL);
	115
	116	return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
	117	}
	118
	119	int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
	120	uint16_t type;
	121
	122	assert_return(m, -EINVAL);
	123
	124	type = htons(data);
	125
	126	return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
	127	}
	128
	129	int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
	130	uint32_t type;
	131
	132	assert_return(m, -EINVAL);
	133
	134	type = htonl(data);
	135
	136	return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
	137	}
	138
	139	int tlv_packet_append_string(tlv_packet m, char data, uint16_t size) {
	140
	141	assert_return(m, -EINVAL);
	142
	143	return tlv_packet_append_bytes(m, data, size);
	144	}
	145
	146	int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
	147
	148	assert_return(m, -EINVAL);
	149
	150	m->container_pos = m->pdu + m->length;
	151
	152	return tlv_packet_append_u16(m, type << 9);
	153	}
	154
	155	int lldp_tlv_packet_close_container(tlv_packet *m) {
	156	uint16_t type;
157
158	assert_return(m, -EINVAL);
159	assert_return(m->container_pos, -EINVAL);
160
161	memcpy(&type, m->container_pos, sizeof(uint16_t));
162
163	type \|= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
164	memcpy(m->container_pos, &type, sizeof(uint16_t));
165
166	return 0;
167	}
168
169	static inline int tlv_packet_read_internal(tlv_section m, void *data) {
170
171	assert_return(m->read_pos, -EINVAL);
172
173	*data = m->read_pos;
174
175	return 0;
176	}
177
178	int tlv_packet_read_u8(tlv_packet m, uint8_t data) {
a7f7d1bd	179	void *val = NULL;
ad1ad5c8 SS	180	int r;
	181
	182	assert_return(m, -EINVAL);
	183
	184	r = tlv_packet_read_internal(m->container, &val);
	185	if (r < 0)
	186	return r;
	187
	188	memcpy(data, val, sizeof(uint8_t));
	189
	190	m->container->read_pos ++;
	191
	192	return 0;
	193	}
	194
	195	int tlv_packet_read_u16(tlv_packet m, uint16_t data) {
	196	uint16_t t;
a7f7d1bd	197	void *val = NULL;
ad1ad5c8 SS	198	int r;
	199
	200	assert_return(m, -EINVAL);
	201
	202	r = tlv_packet_read_internal(m->container, &val);
	203	if (r < 0)
	204	return r;
	205
	206	memcpy(&t, val, sizeof(uint16_t));
	207	*data = ntohs(t);
	208
	209	m->container->read_pos += 2;
	210
	211	return 0;
	212	}
	213
	214	int tlv_packet_read_u32(tlv_packet m, uint32_t data) {
	215	uint32_t t;
	216	void *val;
	217	int r;
	218
	219	assert_return(m, -EINVAL);
	220
	221	r = tlv_packet_read_internal(m->container, &val);
	222	if (r < 0)
	223	return r;
	224
	225	memcpy(&t, val, sizeof(uint32_t));
	226	*data = ntohl(t);
	227
	228	m->container->read_pos += 4;
	229
	230	return r;
	231	}
	232
	233	int tlv_packet_read_string(tlv_packet m, char data, uint16_t data_length) {
a7f7d1bd	234	void *val = NULL;
ad1ad5c8 SS	235	int r;
	236
	237	assert_return(m, -EINVAL);
	238
	239	r = tlv_packet_read_internal(m->container, &val);
	240	if (r < 0)
	241	return r;
	242
	243	data = (char ) val;
29eca2ff	244	*data_length = m->container->data + m->container->length - m->container->read_pos;
ad1ad5c8	245
29eca2ff	246	m->container->read_pos += *data_length;
ad1ad5c8 SS	247
	248	return 0;
	249	}
	250
	251	int tlv_packet_read_bytes(tlv_packet m, uint8_t data, uint16_t data_length) {
a7f7d1bd	252	void *val = NULL;
ad1ad5c8 SS	253	int r;
	254
	255	assert_return(m, -EINVAL);
	256
	257	r = tlv_packet_read_internal(m->container, &val);
	258	if (r < 0)
	259	return r;
	260
	261	data = (uint8_t ) val;
29eca2ff	262	*data_length = m->container->data + m->container->length - m->container->read_pos;
ad1ad5c8	263
29eca2ff	264	m->container->read_pos += *data_length;
ad1ad5c8 SS	265
	266	return 0;
	267	}
	268
	269	/* parse raw TLV packet */
	270	int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
	271	tlv_section section, tail;
	272	uint16_t t, l;
	273	uint8_t *p;
	274	int r;
	275
	276	assert_return(m, -EINVAL);
	277	assert_return(size, -EINVAL);
	278
	279	p = m->pdu;
	280
a8eaaee7	281	/* extract Ethernet header */
ad1ad5c8 SS	282	memcpy(&m->mac, p, ETH_ALEN);
	283	p += sizeof(struct ether_header);
	284
	285	for (l = 0; l <= size; ) {
	286	r = tlv_section_new(&section);
	287	if (r < 0)
	288	return r;
	289
	290	memcpy(&t, p, sizeof(uint16_t));
	291
	292	section->type = ntohs(t) >> 9;
	293	section->length = ntohs(t) & 0x01ff;
	294
	295	if (section->type == LLDP_TYPE_END \|\| section->type >=_LLDP_TYPE_MAX) {
	296	tlv_section_free(section);
	297	break;
	298	}
	299
	300	p += 2;
d8c89d61 BG	301
	302	if (section->type == LLDP_TYPE_PRIVATE &&
	303	section->length >= LLDP_OUI_LEN + 1) {
	304	section->oui = p;
	305	p += LLDP_OUI_LEN;
	306	section->subtype = *p++;
	307
	308	section->length -= LLDP_OUI_LEN + 1;
	309	l += LLDP_OUI_LEN + 1;
	310	}
	311
ad1ad5c8 SS	312	section->data = p;
	313
	314	LIST_FIND_TAIL(section, m->sections, tail);
	315	LIST_INSERT_AFTER(section, m->sections, tail, section);
	316
	317	p += section->length;
	318	l += (section->length + 2);
	319	}
	320
	321	return 0;
	322	}
	323
	324	int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
	325	tlv_section *s;
	326
	327	assert_return(m, -EINVAL);
d8c89d61	328	assert_return(type != LLDP_TYPE_PRIVATE, -EINVAL);
ad1ad5c8 SS	329
	330	LIST_FOREACH(section, s, m->sections)
	331	if (s->type == type)
	332	break;
	333	if (!s)
	334	return -1;
	335
	336	m->container = s;
	337
	338	m->container->read_pos = s->data;
	339	if (!m->container->read_pos) {
29eca2ff	340	m->container = NULL;
ad1ad5c8 SS	341	return -1;
	342	}
	343
	344	return 0;
	345	}
	346
d8c89d61 BG	347	int lldp_tlv_packet_enter_container_oui(tlv_packet m, const uint8_t oui, uint8_t subtype) {
	348	tlv_section *s;
	349
	350	assert_return(m, -EINVAL);
	351	assert_return(oui, -EINVAL);
	352
	353	LIST_FOREACH(section, s, m->sections) {
	354	if (s->type == LLDP_TYPE_PRIVATE &&
	355	s->oui &&
	356	s->subtype == subtype &&
	357	!memcmp(s->oui, oui, LLDP_OUI_LEN))
	358	break;
	359	}
	360
	361	if (!s)
	362	return -1;
	363
	364	m->container = s;
	365
	366	m->container->read_pos = s->data;
	367	if (!m->container->read_pos) {
29eca2ff	368	m->container = NULL;
d8c89d61 BG	369	return -1;
	370	}
	371
	372	return 0;
	373	}
	374
ad1ad5c8 SS	375	int lldp_tlv_packet_exit_container(tlv_packet *m) {
	376	assert_return(m, -EINVAL);
	377
	378	m->container = 0;
	379
	380	return 0;
	381	}
b57003ec	382
11e83571	383	static int lldp_tlv_packet_read_u16_tlv(tlv_packet tlv, uint16_t type, uint16_t value) {
564cabd4	384	int r, r2;
11e83571 BG	385
	386	assert_return(tlv, -EINVAL);
	387
	388	r = lldp_tlv_packet_enter_container(tlv, type);
	389	if (r < 0)
	390	goto out;
	391
	392	r = tlv_packet_read_u16(tlv, value);
564cabd4	393	r2 = lldp_tlv_packet_exit_container(tlv);
11e83571 BG	394
11e83571 BG	395	out:
564cabd4	396	return r < 0 ? r : r2;
11e83571 BG	397	}
	398
	399	static int lldp_tlv_packet_read_string_tlv(tlv_packet tlv, uint16_t type, char data, uint16_t length) {
	400	char *s;
564cabd4	401	int r, r2;
11e83571 BG	402
	403	assert_return(tlv, -EINVAL);
	404
	405	r = lldp_tlv_packet_enter_container(tlv, type);
	406	if (r < 0)
	407	return r;
	408
	409	r = tlv_packet_read_string(tlv, &s, length);
	410	if (r < 0)
	411	goto out;
	412
	413	data = (char ) s;
	414
	415	out:
564cabd4	416	r2 = lldp_tlv_packet_exit_container(tlv);
11e83571	417
564cabd4	418	return r < 0 ? r : r2;
11e83571 BG	419	}
11e83571 BG	420
b57003ec BG	421	int sd_lldp_packet_read_chassis_id(tlv_packet *tlv,
	422	uint8_t *type,
	423	uint8_t **data,
	424	uint16_t *length) {
	425	uint8_t subtype;
564cabd4	426	int r, r2;
b57003ec BG	427
	428	assert_return(tlv, -EINVAL);
	429
	430	r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_CHASSIS_ID);
	431	if (r < 0)
	432	goto out2;
	433
	434	r = tlv_packet_read_u8(tlv, &subtype);
	435	if (r < 0)
	436	goto out1;
	437
	438	switch (subtype) {
	439	case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:
	440
	441	r = tlv_packet_read_bytes(tlv, data, length);
	442	if (r < 0)
	443	goto out1;
	444
	445	break;
	446	default:
	447	r = -EOPNOTSUPP;
	448	break;
	449	}
	450
	451	*type = subtype;
	452
	453	out1:
564cabd4	454	r2 = lldp_tlv_packet_exit_container(tlv);
b57003ec BG	455
b57003ec BG	456	out2:
564cabd4	457	return r < 0 ? r : r2;
b57003ec BG	458	}
	459
	460	int sd_lldp_packet_read_port_id(tlv_packet *tlv,
	461	uint8_t *type,
	462	uint8_t **data,
	463	uint16_t *length) {
	464	uint8_t subtype;
	465	char *s;
564cabd4	466	int r, r2;
b57003ec BG	467
	468	assert_return(tlv, -EINVAL);
	469
	470	r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_ID);
	471	if (r < 0)
	472	goto out2;
	473
	474	r = tlv_packet_read_u8(tlv, &subtype);
	475	if (r < 0)
	476	goto out1;
	477
	478	switch (subtype) {
	479	case LLDP_PORT_SUBTYPE_PORT_COMPONENT:
	480	case LLDP_PORT_SUBTYPE_INTERFACE_ALIAS:
	481	case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
	482	case LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED:
	483
	484	r = tlv_packet_read_string(tlv, &s, length);
	485	if (r < 0)
	486	goto out1;
	487
	488	data = (uint8_t ) s;
	489
	490	break;
	491	case LLDP_PORT_SUBTYPE_MAC_ADDRESS:
	492
	493	r = tlv_packet_read_bytes(tlv, data, length);
	494	if (r < 0)
	495	goto out1;
	496
	497	break;
	498	default:
	499	r = -EOPNOTSUPP;
	500	break;
	501	}
	502
	503	*type = subtype;
	504
	505	out1:
564cabd4	506	r2 = lldp_tlv_packet_exit_container(tlv);
b57003ec BG	507
b57003ec BG	508	out2:
564cabd4	509	return r < 0 ? r : r2;
b57003ec BG	510	}
	511
	512	int sd_lldp_packet_read_ttl(tlv_packet tlv, uint16_t ttl) {
11e83571	513	return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_TTL, ttl);
b57003ec BG	514	}
	515
	516	int sd_lldp_packet_read_system_name(tlv_packet *tlv,
	517	char **data,
	518	uint16_t *length) {
11e83571	519	return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_NAME, data, length);
b57003ec BG	520	}
	521
	522	int sd_lldp_packet_read_system_description(tlv_packet *tlv,
	523	char **data,
	524	uint16_t *length) {
11e83571	525	return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_DESCRIPTION, data, length);
b57003ec BG	526	}
	527
	528	int sd_lldp_packet_read_port_description(tlv_packet *tlv,
	529	char **data,
	530	uint16_t *length) {
11e83571	531	return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_PORT_DESCRIPTION, data, length);
b57003ec BG	532	}
	533
	534	int sd_lldp_packet_read_system_capability(tlv_packet tlv, uint16_t data) {
11e83571	535	return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_SYSTEM_CAPABILITIES, data);
b57003ec	536	}
4fc6de5d	537
d8c89d61	538	int sd_lldp_packet_read_port_vlan_id(tlv_packet tlv, uint16_t id) {
564cabd4	539	int r, r2;
d8c89d61 BG	540
	541	assert_return(tlv, -EINVAL);
	542
	543	r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID);
	544	if (r < 0)
	545	goto out;
	546
	547	r = tlv_packet_read_u16(tlv, id);
564cabd4	548	r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d61 BG	549
d8c89d61 BG	550	out:
564cabd4	551	return r < 0 ? r : r2;
d8c89d61 BG	552	}
	553
	554	int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet tlv, uint8_t flags, uint16_t *id) {
564cabd4	555	int r, r2;
d8c89d61 BG	556
	557	assert_return(tlv, -EINVAL);
	558
	559	r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID);
	560	if (r < 0)
	561	goto out;
	562
	563	r = tlv_packet_read_u8(tlv, flags);
	564	if (r >= 0)
	565	r = tlv_packet_read_u16(tlv, id);
	566
564cabd4	567	r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d61 BG	568
d8c89d61 BG	569	out:
564cabd4	570	return r < 0 ? r : r2;
d8c89d61 BG	571	}
	572
	573	int sd_lldp_packet_read_vlan_name(tlv_packet tlv, uint16_t vlan_id, char *name, uint16_t length) {
564cabd4	574	int r, r2;
d8c89d61 BG	575	uint8_t len = 0;
	576
	577	assert_return(tlv, -EINVAL);
	578
	579	r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME);
	580	if (r < 0)
	581	goto out;
	582
	583	r = tlv_packet_read_u16(tlv, vlan_id);
	584	if (r >= 0)
	585	r = tlv_packet_read_u8(tlv, &len);
	586	if (r >= 0)
	587	r = tlv_packet_read_string(tlv, name, length);
	588
	589	if (r >= 0 && len < *length)
	590	*length = len;
	591
564cabd4	592	r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d61 BG	593
d8c89d61 BG	594	out:
564cabd4	595	return r < 0 ? r : r2;
d8c89d61 BG	596	}
	597
	598	int sd_lldp_packet_read_management_vid(tlv_packet tlv, uint16_t id) {
564cabd4	599	int r, r2;
d8c89d61 BG	600
	601	assert_return(tlv, -EINVAL);
	602
	603	r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID);
	604	if (r < 0)
	605	goto out;
	606
	607	r = tlv_packet_read_u16(tlv, id);
564cabd4	608	r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d61 BG	609
d8c89d61 BG	610	out:
564cabd4	611	return r < 0 ? r : r2;
d8c89d61 BG	612	}
	613
	614	int sd_lldp_packet_read_link_aggregation(sd_lldp_packet tlv, uint8_t status, uint32_t *id) {
564cabd4	615	int r, r2;
d8c89d61 BG	616
	617	assert_return(tlv, -EINVAL);
	618
	619	r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION);
	620	if (r < 0)
	621	goto out;
	622
	623	r = tlv_packet_read_u8(tlv, status);
	624	if (r >= 0)
	625	r = tlv_packet_read_u32(tlv, id);
	626
564cabd4	627	r2 = lldp_tlv_packet_exit_container(tlv);
d8c89d61 BG	628
d8c89d61 BG	629	out:
564cabd4	630	return r < 0 ? r : r2;
d8c89d61 BG	631	}
d8c89d61 BG	632
4fc6de5d BG	633	int sd_lldp_packet_get_destination_type(tlv_packet tlv, int dest) {
	634	assert_return(tlv, -EINVAL);
	635	assert_return(dest, -EINVAL);
	636
	637	/* 802.1AB-2009, Table 7-1 */
	638	if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_BRIDGE, ETH_ALEN))
	639	*dest = SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE;
	640	else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_NON_TPMR_BRIDGE, ETH_ALEN))
	641	*dest = SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE;
	642	else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_CUSTOMER_BRIDGE, ETH_ALEN))
	643	*dest = SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE;
	644	else
	645	return -EINVAL;
	646
	647	return 0;
	648	}