diff options
Diffstat (limited to 'src/libsystemd-network/lldp-tlv.c')
| -rw-r--r-- | src/libsystemd-network/lldp-tlv.c | 638 |
1 files changed, 638 insertions, 0 deletions
diff --git a/src/libsystemd-network/lldp-tlv.c b/src/libsystemd-network/lldp-tlv.c new file mode 100644 index 0000000000..9170b50691 --- /dev/null +++ b/src/libsystemd-network/lldp-tlv.c @@ -0,0 +1,638 @@ +/*** + 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 <arpa/inet.h> +#include <net/ethernet.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(§ion); + 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) + return r; + + r = tlv_packet_read_u16(tlv, value); + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + r = tlv_packet_read_u8(tlv, &subtype); + if (r < 0) + goto out; + + switch (subtype) { + case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS: + + r = tlv_packet_read_bytes(tlv, data, length); + if (r < 0) + goto out; + + break; + default: + r = -EOPNOTSUPP; + break; + } + + *type = subtype; + + out: + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + r = tlv_packet_read_u8(tlv, &subtype); + if (r < 0) + goto out; + + 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 out; + + *data = (uint8_t *) s; + + break; + case LLDP_PORT_SUBTYPE_MAC_ADDRESS: + + r = tlv_packet_read_bytes(tlv, data, length); + if (r < 0) + goto out; + + break; + default: + r = -EOPNOTSUPP; + break; + } + + *type = subtype; + + out: + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + r = tlv_packet_read_u16(tlv, id); + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + r = tlv_packet_read_u8(tlv, flags); + if (r >= 0) + r = tlv_packet_read_u16(tlv, id); + + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + 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); + + 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) + return r; + + r = tlv_packet_read_u16(tlv, id); + r2 = lldp_tlv_packet_exit_container(tlv); + + 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) + return r; + + r = tlv_packet_read_u8(tlv, status); + if (r >= 0) + r = tlv_packet_read_u32(tlv, id); + + r2 = lldp_tlv_packet_exit_container(tlv); + + 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; +} |