diff options
| -rw-r--r-- | src/libsystemd-network/lldp-internal.c | 251 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-internal.h | 10 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-network.c | 27 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-network.h | 1 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-port.c | 1 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-tlv.c | 342 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp-tlv.h | 18 | ||||
| -rw-r--r-- | src/libsystemd-network/lldp.h | 13 | ||||
| -rw-r--r-- | src/libsystemd-network/sd-lldp.c | 44 | ||||
| -rw-r--r-- | src/libsystemd-network/test-lldp.c | 246 | ||||
| -rw-r--r-- | src/systemd/sd-lldp.h | 29 |
11 files changed, 712 insertions, 270 deletions
diff --git a/src/libsystemd-network/lldp-internal.c b/src/libsystemd-network/lldp-internal.c index 3c04898e92..4012cd483b 100644 --- a/src/libsystemd-network/lldp-internal.c +++ b/src/libsystemd-network/lldp-internal.c @@ -21,6 +21,7 @@ ***/ #include "lldp-internal.h" +#include "sd-lldp.h" /* We store maximum 1K chassis entries */ #define LLDP_MIB_MAX_CHASSIS 1024 @@ -28,207 +29,6 @@ /* Maximum Ports can be attached to any chassis */ #define LLDP_MIB_MAX_PORT_PER_CHASSIS 32 -int lldp_read_chassis_id(tlv_packet *tlv, - uint8_t *type, - uint16_t *length, - uint8_t **data) { - uint8_t subtype; - int r; - - 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: - (void) lldp_tlv_packet_exit_container(tlv); - - out2: - return r; -} - -int lldp_read_port_id(tlv_packet *tlv, - uint8_t *type, - uint16_t *length, - uint8_t **data) { - uint8_t subtype; - char *s; - int r; - - 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: - (void) lldp_tlv_packet_exit_container(tlv); - - out2: - return r; -} - -int lldp_read_ttl(tlv_packet *tlv, uint16_t *ttl) { - int r; - - assert_return(tlv, -EINVAL); - - r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_TTL); - if (r < 0) - goto out; - - r = tlv_packet_read_u16(tlv, ttl); - - (void) lldp_tlv_packet_exit_container(tlv); - - out: - return r; -} - -int lldp_read_system_name(tlv_packet *tlv, - uint16_t *length, - char **data) { - char *s; - int r; - - assert_return(tlv, -EINVAL); - - r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_SYSTEM_NAME); - if (r < 0) - return r; - - r = tlv_packet_read_string(tlv, &s, length); - if (r < 0) - goto out; - - *data = (char *) s; - - out: - (void) lldp_tlv_packet_exit_container(tlv); - - return r; -} - -int lldp_read_system_description(tlv_packet *tlv, - uint16_t *length, - char **data) { - char *s; - int r; - - assert_return(tlv, -EINVAL); - - r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_SYSTEM_DESCRIPTION); - if (r < 0) - return r; - - r = tlv_packet_read_string(tlv, &s, length); - if (r < 0) - goto out; - - *data = (char *) s; - - out: - (void) lldp_tlv_packet_exit_container(tlv); - - return r; -} - -int lldp_read_port_description(tlv_packet *tlv, - uint16_t *length, - char **data) { - char *s; - int r; - - assert_return(tlv, -EINVAL); - - r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_DESCRIPTION); - if (r < 0) - return r; - - r = tlv_packet_read_string(tlv, &s, length); - if (r < 0) - goto out; - - *data = (char *) s; - - out: - (void) lldp_tlv_packet_exit_container(tlv); - - return r; -} - -int lldp_read_system_capability(tlv_packet *tlv, uint16_t *data) { - int r; - - assert_return(tlv, -EINVAL); - - r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_SYSTEM_CAPABILITIES); - if (r < 0) - return r; - - r = tlv_packet_read_u16(tlv, data); - if (r < 0) - goto out; - - return 0; - out: - - (void) lldp_tlv_packet_exit_container(tlv); - - return r; -} - /* 10.5.5.2.2 mibUpdateObjects () * The mibUpdateObjects () procedure updates the MIB objects corresponding to * the TLVs contained in the received LLDPDU for the LLDP remote system @@ -244,7 +44,7 @@ int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv) { assert_return(c, -EINVAL); assert_return(tlv, -EINVAL); - r = lldp_read_port_id(tlv, &type, &length, &data); + r = sd_lldp_packet_read_port_id(tlv, &type, &data, &length); if (r < 0) return r; @@ -253,13 +53,13 @@ int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv) { if ((p->type == type && p->length == length && !memcmp(p->data, data, p->length))) { - r = lldp_read_ttl(tlv, &ttl); + r = sd_lldp_packet_read_ttl(tlv, &ttl); if (r < 0) return r; p->until = ttl * USEC_PER_SEC + now(clock_boottime_or_monotonic()); - tlv_packet_free(p->packet); + sd_lldp_packet_unref(p->packet); p->packet = tlv; prioq_reshuffle(p->c->by_expiry, p, &p->prioq_idx); @@ -281,7 +81,7 @@ int lldp_mib_remove_objects(lldp_chassis *c, tlv_packet *tlv) { assert_return(c, -EINVAL); assert_return(tlv, -EINVAL); - r = lldp_read_port_id(tlv, &type, &length, &data); + r = sd_lldp_packet_read_port_id(tlv, &type, &data, &length); if (r < 0) return r; @@ -312,11 +112,11 @@ int lldp_mib_add_objects(Prioq *by_expiry, assert_return(neighbour_mib, -EINVAL); assert_return(tlv, -EINVAL); - r = lldp_read_chassis_id(tlv, &subtype, &length, &data); + r = sd_lldp_packet_read_chassis_id(tlv, &subtype, &data, &length); if (r < 0) goto drop; - r = lldp_read_ttl(tlv, &ttl); + r = sd_lldp_packet_read_ttl(tlv, &ttl); if (r < 0) goto drop; @@ -401,7 +201,7 @@ int lldp_mib_add_objects(Prioq *by_expiry, return 0; drop: - tlv_packet_free(tlv); + sd_lldp_packet_unref(tlv); if (new_chassis) hashmap_remove(neighbour_mib, &c->chassis_id); @@ -435,7 +235,7 @@ void lldp_neighbour_port_free(lldp_neighbour_port *p) { if(!p) return; - tlv_packet_free(p->packet); + sd_lldp_packet_unref(p->packet); free(p->data); free(p); @@ -452,11 +252,11 @@ int lldp_neighbour_port_new(lldp_chassis *c, assert(tlv); - r = lldp_read_port_id(tlv, &type, &length, &data); + r = sd_lldp_packet_read_port_id(tlv, &type, &data, &length); if (r < 0) return r; - r = lldp_read_ttl(tlv, &ttl); + r = sd_lldp_packet_read_ttl(tlv, &ttl); if (r < 0) return r; @@ -505,7 +305,7 @@ int lldp_chassis_new(tlv_packet *tlv, assert(tlv); - r = lldp_read_chassis_id(tlv, &type, &length, &data); + r = sd_lldp_packet_read_chassis_id(tlv, &type, &data, &length); if (r < 0) return r; @@ -531,3 +331,30 @@ int lldp_chassis_new(tlv_packet *tlv, return 0; } + +int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata) { + _cleanup_lldp_packet_unref_ tlv_packet *packet = NULL; + tlv_packet *p; + uint16_t length; + int r; + + assert(fd); + assert(userdata); + + r = tlv_packet_new(&packet); + if (r < 0) + return r; + + length = read(fd, &packet->pdu, sizeof(packet->pdu)); + + /* Silently drop the packet */ + if ((size_t) length > ETHER_MAX_LEN) + return 0; + + packet->userdata = userdata; + + p = packet; + packet = NULL; + + return lldp_handle_packet(p, (uint16_t) length); +} diff --git a/src/libsystemd-network/lldp-internal.h b/src/libsystemd-network/lldp-internal.h index f4eadbb87e..284cc6720e 100644 --- a/src/libsystemd-network/lldp-internal.h +++ b/src/libsystemd-network/lldp-internal.h @@ -26,6 +26,7 @@ #include "list.h" #include "lldp-tlv.h" #include "prioq.h" +#include "sd-event.h" typedef struct lldp_neighbour_port lldp_neighbour_port; typedef struct lldp_chassis lldp_chassis; @@ -86,13 +87,6 @@ int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv); int lldp_mib_add_objects(Prioq *by_expiry, Hashmap *neighbour_mib, tlv_packet *tlv); int lldp_mib_remove_objects(lldp_chassis *c, tlv_packet *tlv); -int lldp_read_chassis_id(tlv_packet *tlv, uint8_t *type, uint16_t *length, uint8_t **data); -int lldp_read_port_id(tlv_packet *tlv, uint8_t *type, uint16_t *length, uint8_t **data); -int lldp_read_ttl(tlv_packet *tlv, uint16_t *ttl); -int lldp_read_system_name(tlv_packet *tlv, uint16_t *length, char **data); -int lldp_read_system_description(tlv_packet *tlv, uint16_t *length, char **data); -int lldp_read_system_capability(tlv_packet *tlv, uint16_t *data); -int lldp_read_port_description(tlv_packet *tlv, uint16_t *length, char **data); - int lldp_handle_packet(tlv_packet *m, uint16_t length); +int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata); #define log_lldp(fmt, ...) log_internal(LOG_DEBUG, 0, __FILE__, __LINE__, __func__, "LLDP: " fmt, ##__VA_ARGS__) diff --git a/src/libsystemd-network/lldp-network.c b/src/libsystemd-network/lldp-network.c index 664d2f7867..12a6599ff1 100644 --- a/src/libsystemd-network/lldp-network.c +++ b/src/libsystemd-network/lldp-network.c @@ -82,30 +82,3 @@ int lldp_network_bind_raw_socket(int ifindex) { return r; } - -int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata) { - _cleanup_tlv_packet_free_ tlv_packet *packet = NULL; - tlv_packet *p; - uint16_t length; - int r; - - assert(fd); - assert(userdata); - - r = tlv_packet_new(&packet); - if (r < 0) - return r; - - length = read(fd, &packet->pdu, sizeof(packet->pdu)); - - /* Silently drop the packet */ - if ((size_t) length > ETHER_MAX_LEN) - return 0; - - packet->userdata = userdata; - - p = packet; - packet = NULL; - - return lldp_handle_packet(p, (uint16_t) length); -} diff --git a/src/libsystemd-network/lldp-network.h b/src/libsystemd-network/lldp-network.h index b7f8d3bf80..74ee13a414 100644 --- a/src/libsystemd-network/lldp-network.h +++ b/src/libsystemd-network/lldp-network.h @@ -25,4 +25,3 @@ #include "sd-event.h" int lldp_network_bind_raw_socket(int ifindex); -int lldp_receive_packet(sd_event_source *s, int fd, uint32_t revents, void *userdata); diff --git a/src/libsystemd-network/lldp-port.c b/src/libsystemd-network/lldp-port.c index 97fe7c1dd3..7486b4c38f 100644 --- a/src/libsystemd-network/lldp-port.c +++ b/src/libsystemd-network/lldp-port.c @@ -23,6 +23,7 @@ #include "async.h" #include "lldp-port.h" #include "lldp-network.h" +#include "lldp-internal.h" int lldp_port_start(lldp_port *p) { int r; diff --git a/src/libsystemd-network/lldp-tlv.c b/src/libsystemd-network/lldp-tlv.c index 0cea5b10a6..66af22e37d 100644 --- a/src/libsystemd-network/lldp-tlv.c +++ b/src/libsystemd-network/lldp-tlv.c @@ -54,22 +54,41 @@ int tlv_packet_new(tlv_packet **ret) { return -ENOMEM; LIST_HEAD_INIT(m->sections); + m->n_ref = 1; *ret = m; return 0; } -void tlv_packet_free(tlv_packet *m) { +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; + 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) { @@ -221,9 +240,9 @@ int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) { return r; *data = (char *) val; - *data_length = m->container->length; + *data_length = m->container->data + m->container->length - m->container->read_pos; - m->container->read_pos += m->container->length; + m->container->read_pos += *data_length; return 0; } @@ -239,9 +258,9 @@ int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) return r; *data = (uint8_t *) val; - *data_length = m->container->length; + *data_length = m->container->data + m->container->length - m->container->read_pos; - m->container->read_pos += m->container->length; + m->container->read_pos += *data_length; return 0; } @@ -258,7 +277,7 @@ int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) { p = m->pdu; - /* extract ethernet herader */ + /* extract ethernet header */ memcpy(&m->mac, p, ETH_ALEN); p += sizeof(struct ether_header); @@ -278,6 +297,17 @@ int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) { } p += 2; + + if (section->type == LLDP_TYPE_PRIVATE && + section->length >= LLDP_OUI_LEN + 1) { + section->oui = p; + p += LLDP_OUI_LEN; + section->subtype = *p++; + + section->length -= LLDP_OUI_LEN + 1; + l += LLDP_OUI_LEN + 1; + } + section->data = p; LIST_FIND_TAIL(section, m->sections, tail); @@ -294,6 +324,7 @@ int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) { tlv_section *s; assert_return(m, -EINVAL); + assert_return(type != LLDP_TYPE_PRIVATE, -EINVAL); LIST_FOREACH(section, s, m->sections) if (s->type == type) @@ -305,7 +336,35 @@ int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) { m->container->read_pos = s->data; if (!m->container->read_pos) { - m->container = 0; + 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; } @@ -319,3 +378,270 @@ int lldp_tlv_packet_exit_container(tlv_packet *m) { 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; +} diff --git a/src/libsystemd-network/lldp-tlv.h b/src/libsystemd-network/lldp-tlv.h index ce3334e115..2d2c776be6 100644 --- a/src/libsystemd-network/lldp-tlv.h +++ b/src/libsystemd-network/lldp-tlv.h @@ -28,12 +28,18 @@ #include "lldp.h" #include "list.h" +#include "sd-lldp.h" + typedef struct tlv_packet tlv_packet; typedef struct tlv_section tlv_section; +#define LLDP_OUI_LEN 3 + struct tlv_section { uint16_t type; uint16_t length; + uint8_t *oui; + uint8_t subtype; uint8_t *read_pos; uint8_t *data; @@ -41,10 +47,16 @@ struct tlv_section { LIST_FIELDS(tlv_section, section); }; +#define LLDP_MAC_NEAREST_BRIDGE (uint8_t[]) { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e } +#define LLDP_MAC_NEAREST_NON_TPMR_BRIDGE (uint8_t[]) { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03 } +#define LLDP_MAC_NEAREST_CUSTOMER_BRIDGE (uint8_t[]) { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x00 } + int tlv_section_new(tlv_section **ret); void tlv_section_free(tlv_section *ret); struct tlv_packet { + unsigned n_ref; + uint16_t type; uint16_t length; usec_t ts; @@ -61,10 +73,9 @@ struct tlv_packet { }; int tlv_packet_new(tlv_packet **ret); -void tlv_packet_free(tlv_packet *m); -DEFINE_TRIVIAL_CLEANUP_FUNC(tlv_packet*, tlv_packet_free); -#define _cleanup_tlv_packet_free_ _cleanup_(tlv_packet_freep) +DEFINE_TRIVIAL_CLEANUP_FUNC(sd_lldp_packet*, sd_lldp_packet_unref); +#define _cleanup_lldp_packet_unref_ _cleanup_(sd_lldp_packet_unrefp) int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type); int lldp_tlv_packet_close_container(tlv_packet *m); @@ -76,6 +87,7 @@ int tlv_packet_append_u32(tlv_packet *m, uint32_t data); int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size); int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type); +int lldp_tlv_packet_enter_container_oui(tlv_packet *m, const uint8_t *oui, uint8_t subtype); int lldp_tlv_packet_exit_container(tlv_packet *m); int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length); diff --git a/src/libsystemd-network/lldp.h b/src/libsystemd-network/lldp.h index 5e4b283e26..19e5cc5f41 100644 --- a/src/libsystemd-network/lldp.h +++ b/src/libsystemd-network/lldp.h @@ -113,3 +113,16 @@ typedef enum LLDPMedCapability { LLDP_MED_CAPABILITY_MAX, LLDP_MED_CAPABILITY_INVALID = -1, } LLDPMedCapability; + +#define LLDP_OUI_802_1 (uint8_t[]) { 0x00, 0x80, 0xc2 } +#define LLDP_OUI_802_3 (uint8_t[]) { 0x00, 0x12, 0x0f } + +enum { + LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID = 1, + LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID = 2, + LLDP_OUI_SUBTYPE_802_1_VLAN_NAME = 3, + LLDP_OUI_SUBTYPE_802_1_PROTOCOL_IDENTITY = 4, + LLDP_OUI_SUBTYPE_802_1_VID_USAGE_DIGEST = 5, + LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID = 6, + LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION = 7, +}; diff --git a/src/libsystemd-network/sd-lldp.c b/src/libsystemd-network/sd-lldp.c index 17512884f5..7aa405a655 100644 --- a/src/libsystemd-network/sd-lldp.c +++ b/src/libsystemd-network/sd-lldp.c @@ -199,7 +199,7 @@ int lldp_handle_packet(tlv_packet *tlv, uint16_t length) { goto out; } - /* skip type and lengh encoding */ + /* skip type and length encoding */ p += 2; q = p; @@ -338,7 +338,7 @@ int lldp_handle_packet(tlv_packet *tlv, uint16_t length) { lldp->statistics.stats_frames_in_errors_total ++; } - tlv_packet_free(tlv); + sd_lldp_packet_unref(tlv); return 0; } @@ -455,7 +455,7 @@ int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) { _cleanup_free_ char *s = NULL; char *k, *t; - r = lldp_read_chassis_id(p->packet, &type, &length, &mac); + r = sd_lldp_packet_read_chassis_id(p->packet, &type, &mac, &length); if (r < 0) continue; @@ -468,7 +468,7 @@ int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) { goto fail; } - r = lldp_read_port_id(p->packet, &type, &length, &port_id); + r = sd_lldp_packet_read_port_id(p->packet, &type, &port_id, &length); if (r < 0) continue; @@ -513,7 +513,7 @@ int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) { free(s); s = k; - r = lldp_read_system_name(p->packet, &length, &k); + r = sd_lldp_packet_read_system_name(p->packet, &k, &length); if (r < 0) k = strappend(s, "'_NAME=N/A' "); else { @@ -535,7 +535,7 @@ int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) { free(s); s = k; - (void) lldp_read_system_capability(p->packet, &data); + (void) sd_lldp_packet_read_system_capability(p->packet, &data); sprintf(buf, "'_CAP=%x'", data); @@ -702,3 +702,35 @@ int sd_lldp_new(int ifindex, return 0; } + +int sd_lldp_get_packets(sd_lldp *lldp, sd_lldp_packet ***tlvs) { + lldp_neighbour_port *p; + lldp_chassis *c; + Iterator iter; + unsigned count = 0, i; + + assert_return(lldp, -EINVAL); + assert_return(tlvs, -EINVAL); + + HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) { + LIST_FOREACH(port, p, c->ports) + count++; + } + + if (!count) { + *tlvs = NULL; + return 0; + } + + *tlvs = new(sd_lldp_packet *, count); + if (!*tlvs) + return -ENOMEM; + + i = 0; + HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) { + LIST_FOREACH(port, p, c->ports) + (*tlvs)[i++] = sd_lldp_packet_ref(p->packet); + } + + return count; +} diff --git a/src/libsystemd-network/test-lldp.c b/src/libsystemd-network/test-lldp.c index 06545aee59..e57102a576 100644 --- a/src/libsystemd-network/test-lldp.c +++ b/src/libsystemd-network/test-lldp.c @@ -25,20 +25,26 @@ #include <net/ethernet.h> #include <arpa/inet.h> +#include "sd-lldp.h" +#include "sd-event.h" +#include "event-util.h" #include "macro.h" #include "lldp.h" #include "lldp-tlv.h" +#include "lldp-network.h" #define TEST_LLDP_PORT "em1" #define TEST_LLDP_TYPE_SYSTEM_NAME "systemd-lldp" #define TEST_LLDP_TYPE_SYSTEM_DESC "systemd-lldp-desc" +static int test_fd[2]; + static struct ether_addr mac_addr = { .ether_addr_octet = {'A', 'B', 'C', '1', '2', '3'} }; static int lldp_build_tlv_packet(tlv_packet **ret) { - _cleanup_tlv_packet_free_ tlv_packet *m = NULL; + _cleanup_lldp_packet_unref_ tlv_packet *m = NULL; const uint8_t lldp_dst[] = LLDP_MULTICAST_ADDR; struct ether_header ether = { .ether_type = htons(ETHERTYPE_LLDP), @@ -202,6 +208,15 @@ static int lldp_parse_ttl_tlv(tlv_packet *m) { return 0; } +static int lldp_get_destination_type(tlv_packet *m) { + int dest; + + assert_se(sd_lldp_packet_get_destination_type(m, &dest) >= 0); + assert_se(dest == SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE); + + return 0; +} + static int lldp_parse_tlv_packet(tlv_packet *m, int len) { uint8_t subtype; @@ -212,20 +227,241 @@ static int lldp_parse_tlv_packet(tlv_packet *m, int len) { assert_se(lldp_parse_ttl_tlv(m) >= 0); assert_se(lldp_parse_system_desc_tlv(m) >= 0); + assert_se(lldp_get_destination_type(m) >= 0); + return 0; } -int main(int argc, char *argv[]) { - _cleanup_tlv_packet_free_ tlv_packet *tlv = NULL; +static void test_parser(void) { + _cleanup_lldp_packet_unref_ tlv_packet *tlv = NULL; /* form a packet */ lldp_build_tlv_packet(&tlv); - /* parse the packet */ tlv_packet_parse_pdu(tlv, tlv->length); - /* verify */ lldp_parse_tlv_packet(tlv, tlv->length); +} + +int lldp_network_bind_raw_socket(int ifindex) { + if (socketpair(AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK, 0, test_fd) < 0) + return -errno; + + return test_fd[0]; +} + +static int lldp_handler_calls; +static void lldp_handler (sd_lldp *lldp, int event, void *userdata) { + lldp_handler_calls++; +} + +static int start_lldp(sd_lldp **lldp, sd_event *e, sd_lldp_cb_t cb, void *cb_data) { + int r; + + r = sd_lldp_new(42, "dummy", &mac_addr, lldp); + if (r) + return r; + + r = sd_lldp_attach_event(*lldp, e, 0); + if (r) + return r; + + r = sd_lldp_set_callback(*lldp, cb, cb_data); + if (r) + return r; + + r = sd_lldp_start(*lldp); + if (r) + return r; + + return 0; +} + +static int stop_lldp(sd_lldp *lldp) { + int r; + + r = sd_lldp_stop(lldp); + if (r) + return r; + + r = sd_lldp_detach_event(lldp); + if (r) + return r; + + sd_lldp_free(lldp); + safe_close(test_fd[1]); + + return 0; +} + +static void test_receive_basic_packet(sd_event *e) { + sd_lldp *lldp; + sd_lldp_packet **packets; + uint8_t type, *data; + uint16_t length, ttl; + int dest_type; + char *str; + uint8_t frame[] = { + /* Ethernet header */ + 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03, /* Destination MAC*/ + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, /* Source MAC */ + 0x88, 0xcc, /* Ethertype */ + /* LLDP mandatory TLVs */ + 0x02, 0x07, 0x04, 0x00, 0x01, 0x02, /* Chassis: MAC, 00:01:02:03:04:05 */ + 0x03, 0x04, 0x05, + 0x04, 0x04, 0x05, 0x31, 0x2f, 0x33, /* Port: interface name, "1/3" */ + 0x06, 0x02, 0x00, 0x78, /* TTL: 120 seconds*/ + /* LLDP optional TLVs */ + 0x08, 0x04, 0x50, 0x6f, 0x72, 0x74, /* Port Description: "Port" */ + 0x0a, 0x03, 0x53, 0x59, 0x53, /* System Name: "SYS" */ + 0x0c, 0x04, 0x66, 0x6f, 0x6f, 0x00, /* System Description: "foo" (NULL-terminated) */ + 0x00, 0x00 /* End Of LLDPDU */ + }; + + lldp_handler_calls = 0; + assert_se(start_lldp(&lldp, e, lldp_handler, NULL) == 0); + + assert_se(write(test_fd[1], frame, sizeof(frame)) == sizeof(frame)); + sd_event_run(e, 0); + assert_se(lldp_handler_calls == 1); + assert_se(sd_lldp_get_packets(lldp, &packets) == 1); + + assert_se(sd_lldp_packet_read_chassis_id(packets[0], &type, &data, &length) == 0); + assert_se(type == LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS); + assert_se(length == ETH_ALEN); + assert_se(!memcmp(data, "\x00\x01\x02\x03\x04\x05", ETH_ALEN)); + + assert_se(sd_lldp_packet_read_port_id(packets[0], &type, &data, &length) == 0); + assert_se(type == LLDP_PORT_SUBTYPE_INTERFACE_NAME); + assert_se(length == 3); + assert_se(strneq((char *) data, "1/3", 3)); + + assert_se(sd_lldp_packet_read_port_description(packets[0], &str, &length) == 0); + assert_se(length == 4); + assert_se(strneq(str, "Port", 4)); + + assert_se(sd_lldp_packet_read_system_name(packets[0], &str, &length) == 0); + assert_se(length == 3); + assert_se(strneq(str, "SYS", 3)); + + assert_se(sd_lldp_packet_read_system_description(packets[0], &str, &length) == 0); + assert_se(length == 4); /* This is the real length in the TLV packet */ + assert_se(strneq(str, "foo", 3)); + + assert_se(sd_lldp_packet_read_ttl(packets[0], &ttl) == 0); + assert_se(ttl == 120); + + assert_se(sd_lldp_packet_get_destination_type(packets[0], &dest_type) == 0); + assert_se(dest_type == SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE); + + sd_lldp_packet_unref(packets[0]); + free(packets); + + assert_se(stop_lldp(lldp) == 0); +} + +static void test_receive_incomplete_packet(sd_event *e) { + sd_lldp *lldp; + sd_lldp_packet **packets; + uint8_t frame[] = { + /* Ethernet header */ + 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03, /* Destination MAC*/ + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, /* Source MAC */ + 0x88, 0xcc, /* Ethertype */ + /* LLDP mandatory TLVs */ + 0x02, 0x07, 0x04, 0x00, 0x01, 0x02, /* Chassis: MAC, 00:01:02:03:04:05 */ + 0x03, 0x04, 0x05, + 0x04, 0x04, 0x05, 0x31, 0x2f, 0x33, /* Port: interface name, "1/3" */ + /* Missing TTL */ + 0x00, 0x00 /* End Of LLDPDU */ + }; + + lldp_handler_calls = 0; + assert_se(start_lldp(&lldp, e, lldp_handler, NULL) == 0); + + assert_se(write(test_fd[1], frame, sizeof(frame)) == sizeof(frame)); + sd_event_run(e, 0); + assert_se(lldp_handler_calls == 0); + assert_se(sd_lldp_get_packets(lldp, &packets) == 0); + + assert_se(stop_lldp(lldp) == 0); +} + +static void test_receive_oui_packet(sd_event *e) { + sd_lldp *lldp; + sd_lldp_packet **packets; + uint32_t id32; + uint16_t id16, len; + uint8_t flags; + char *str; + uint8_t frame[] = { + /* Ethernet header */ + 0x01, 0x80, 0xc2, 0x00, 0x00, 0x03, /* Destination MAC*/ + 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, /* Source MAC */ + 0x88, 0xcc, /* Ethertype */ + /* LLDP mandatory TLVs */ + 0x02, 0x07, 0x04, 0x00, 0x01, 0x02, /* Chassis: MAC, 00:01:02:03:04:05 */ + 0x03, 0x04, 0x05, + 0x04, 0x04, 0x05, 0x31, 0x2f, 0x33, /* Port TLV: interface name, "1/3" */ + 0x06, 0x02, 0x00, 0x78, /* TTL: 120 seconds*/ + /* LLDP optional TLVs */ + 0xfe, 0x06, 0x00, 0x80, 0xc2, 0x01, /* Port VLAN ID: 0x1234 */ + 0x12, 0x34, + 0xfe, 0x07, 0x00, 0x80, 0xc2, 0x02, /* Port and protocol: flag 1, PPVID 0x7788 */ + 0x01, 0x77, 0x88, + 0xfe, 0x0d, 0x00, 0x80, 0xc2, 0x03, /* VLAN Name: ID 0x1234, name "Vlan51" */ + 0x12, 0x34, 0x06, 0x56, 0x6c, 0x61, + 0x6e, 0x35, 0x31, + 0xfe, 0x06, 0x00, 0x80, 0xc2, 0x06, /* Management VID: 0x0102 */ + 0x01, 0x02, + 0xfe, 0x09, 0x00, 0x80, 0xc2, 0x07, /* Link aggregation: status 1, ID 0x00140012 */ + 0x01, 0x00, 0x14, 0x00, 0x12, + 0x00, 0x00 /* End of LLDPDU */ + }; + + lldp_handler_calls = 0; + assert_se(start_lldp(&lldp, e, lldp_handler, NULL) == 0); + + assert_se(write(test_fd[1], frame, sizeof(frame)) == sizeof(frame)); + sd_event_run(e, 0); + assert_se(lldp_handler_calls == 1); + assert_se(sd_lldp_get_packets(lldp, &packets) == 1); + + assert_se(sd_lldp_packet_read_port_vlan_id(packets[0], &id16) == 0); + assert_se(id16 == 0x1234); + + assert_se(sd_lldp_packet_read_port_protocol_vlan_id(packets[0], &flags, &id16) == 0); + assert_se(flags == 1); + assert_se(id16 == 0x7788); + + assert_se(sd_lldp_packet_read_vlan_name(packets[0], &id16, &str, &len) == 0); + assert_se(id16 == 0x1234); + assert_se(len == 6); + assert_se(strneq(str, "Vlan51", 6)); + + assert_se(sd_lldp_packet_read_management_vid(packets[0], &id16) == 0); + assert_se(id16 == 0x0102); + + assert_se(sd_lldp_packet_read_link_aggregation(packets[0], &flags, &id32) == 0); + assert_se(flags == 1); + assert_se(id32 == 0x00140012); + + sd_lldp_packet_unref(packets[0]); + free(packets); + + assert_se(stop_lldp(lldp) == 0); +} + +int main(int argc, char *argv[]) { + _cleanup_event_unref_ sd_event *e = NULL; + + test_parser(); + + /* LLDP reception tests */ + assert_se(sd_event_new(&e) == 0); + test_receive_basic_packet(e); + test_receive_incomplete_packet(e); + test_receive_oui_packet(e); return 0; } diff --git a/src/systemd/sd-lldp.h b/src/systemd/sd-lldp.h index 0680e526b0..308d42c6e9 100644 --- a/src/systemd/sd-lldp.h +++ b/src/systemd/sd-lldp.h @@ -28,7 +28,14 @@ enum { SD_LLDP_EVENT_UPDATE_INFO = 0, }; +enum { + SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE, + SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE, + SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE, +}; + typedef struct sd_lldp sd_lldp; +typedef struct tlv_packet sd_lldp_packet; typedef void (*sd_lldp_cb_t)(sd_lldp *lldp, int event, void *userdata); @@ -43,3 +50,25 @@ int sd_lldp_detach_event(sd_lldp *lldp); int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_cb_t cb, void *userdata); int sd_lldp_save(sd_lldp *lldp, const char *file); + +int sd_lldp_packet_read_chassis_id(sd_lldp_packet *tlv, uint8_t *type, uint8_t **data, uint16_t *length); +int sd_lldp_packet_read_port_id(sd_lldp_packet *tlv, uint8_t *type, uint8_t **data, uint16_t *length); +int sd_lldp_packet_read_ttl(sd_lldp_packet *tlv, uint16_t *ttl); +int sd_lldp_packet_read_system_name(sd_lldp_packet *tlv, char **data, uint16_t *length); +int sd_lldp_packet_read_system_description(sd_lldp_packet *tlv, char **data, uint16_t *length); +int sd_lldp_packet_read_system_capability(sd_lldp_packet *tlv, uint16_t *data); +int sd_lldp_packet_read_port_description(sd_lldp_packet *tlv, char **data, uint16_t *length); + +/* IEEE 802.1 organizationally specific TLVs */ +int sd_lldp_packet_read_port_vlan_id(sd_lldp_packet *tlv, uint16_t *id); +int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id); +int sd_lldp_packet_read_vlan_name(sd_lldp_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length); +int sd_lldp_packet_read_management_vid(sd_lldp_packet *tlv, uint16_t *id); +int sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id); + +sd_lldp_packet *sd_lldp_packet_ref(sd_lldp_packet *tlv); +sd_lldp_packet *sd_lldp_packet_unref(sd_lldp_packet *tlv); + +int sd_lldp_packet_get_destination_type(sd_lldp_packet *tlv, int *dest); + +int sd_lldp_get_packets(sd_lldp *lldp, sd_lldp_packet ***tlvs); |