From ad1ad5c8e36ea795034fcdac660b15d7c141d55b Mon Sep 17 00:00:00 2001 From: Susant Sahani Date: Sun, 23 Nov 2014 09:46:36 +0530 Subject: [PATCH] networkd: Introduce Link Layer Discovery Protocol (LLDP) This patch introduces LLDP support to networkd. it implements the receiver side of the protocol. The Link Layer Discovery Protocol (LLDP) is an industry-standard, vendor-neutral method to allow networked devices to advertise capabilities, identity, and other information onto a LAN. The Layer 2 protocol, detailed in IEEE 802.1AB-2005.LLDP allows network devices that operate at the lower layers of a protocol stack (such as Layer 2 bridges and switches) to learn some of the capabilities and characteristics of LAN devices available to higher layer protocols. --- Makefile.am | 27 +- src/libsystemd-network/lldp-internal.c | 437 ++++++++++++++++++++++ src/libsystemd-network/lldp-internal.h | 93 +++++ src/libsystemd-network/lldp-network.c | 111 ++++++ src/libsystemd-network/lldp-network.h | 28 ++ src/libsystemd-network/lldp-port.c | 116 ++++++ src/libsystemd-network/lldp-port.h | 61 +++ src/libsystemd-network/lldp-tlv.c | 321 ++++++++++++++++ src/libsystemd-network/lldp-tlv.h | 87 +++++ src/libsystemd-network/lldp.h | 115 ++++++ src/libsystemd-network/sd-lldp.c | 493 +++++++++++++++++++++++++ src/libsystemd-network/sd-lldp.h | 51 +++ src/libsystemd-network/test-lldp.c | 233 ++++++++++++ 13 files changed, 2171 insertions(+), 2 deletions(-) create mode 100644 src/libsystemd-network/lldp-internal.c create mode 100644 src/libsystemd-network/lldp-internal.h create mode 100644 src/libsystemd-network/lldp-network.c create mode 100644 src/libsystemd-network/lldp-network.h create mode 100644 src/libsystemd-network/lldp-port.c create mode 100644 src/libsystemd-network/lldp-port.h create mode 100644 src/libsystemd-network/lldp-tlv.c create mode 100644 src/libsystemd-network/lldp-tlv.h create mode 100644 src/libsystemd-network/lldp.h create mode 100644 src/libsystemd-network/sd-lldp.c create mode 100644 src/libsystemd-network/sd-lldp.h create mode 100644 src/libsystemd-network/test-lldp.c diff --git a/Makefile.am b/Makefile.am index 2cc19a9c5..a7a2b6d59 100644 --- a/Makefile.am +++ b/Makefile.am @@ -3079,7 +3079,18 @@ libsystemd_network_la_SOURCES = \ src/libsystemd-network/dhcp6-network.c \ src/libsystemd-network/dhcp6-option.c \ src/libsystemd-network/dhcp6-lease-internal.h \ - src/libsystemd-network/sd-dhcp6-lease.c + src/libsystemd-network/sd-dhcp6-lease.c \ + src/libsystemd-network/lldp.h \ + src/libsystemd-network/lldp-tlv.h \ + src/libsystemd-network/lldp-tlv.c \ + src/libsystemd-network/lldp-network.h \ + src/libsystemd-network/lldp-network.c \ + src/libsystemd-network/lldp-port.h \ + src/libsystemd-network/lldp-port.c \ + src/libsystemd-network/lldp-internal.h \ + src/libsystemd-network/lldp-internal.c \ + src/libsystemd-network/sd-lldp.h \ + src/libsystemd-network/sd-lldp.c libsystemd_network_la_LIBADD = \ libudev-internal.la \ @@ -3158,13 +3169,25 @@ test_dhcp6_client_LDADD = \ libsystemd-internal.la \ libsystemd-shared.la +test_lldp_SOURCES = \ + src/libsystemd-network/lldp.h \ + src/libsystemd-network/lldp-tlv.h \ + src/libsystemd-network/lldp-tlv.c \ + src/libsystemd-network/test-lldp.c + +test_lldp_LDADD = \ + libsystemd-network.la \ + libsystemd-internal.la \ + libsystemd-shared.la + tests += \ test-dhcp-option \ test-dhcp-client \ test-dhcp-server \ test-ipv4ll \ test-icmp6-rs \ - test-dhcp6-client + test-dhcp6-client \ + test-lldp manual_tests += \ test-pppoe diff --git a/src/libsystemd-network/lldp-internal.c b/src/libsystemd-network/lldp-internal.c new file mode 100644 index 000000000..7085a0249 --- /dev/null +++ b/src/libsystemd-network/lldp-internal.c @@ -0,0 +1,437 @@ +/*-*- 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 . +***/ + +#include "lldp-internal.h" + +/* We store maximum 1K chassis entries */ +#define LLDP_MIB_MAX_CHASSIS 1024 + +/* 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 = -ENOTSUP; + 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_INTERFACE_NAME: + + 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 = -ENOTSUP; + 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; +} + +/* 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 + * indicated by the LLDP remote systems update process defined in 10.3.5 */ + +int lldp_mib_update_objects(lldp_chassis *c, tlv_packet *tlv) { + lldp_neighbour_port *p; + uint16_t length, ttl; + uint8_t *data; + uint8_t type; + int r; + + assert_return(c, -EINVAL); + assert_return(tlv, -EINVAL); + + r = lldp_read_port_id(tlv, &type, &length, &data); + if (r < 0) + return r; + + /* Update the packet if we already have */ + LIST_FOREACH(port, p, c->ports) { + + if ((p->type == type && p->length == length && !memcmp(p->data, data, p->length))) { + + r = lldp_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); + p->packet = tlv; + + prioq_reshuffle(p->c->by_expiry, p, &p->prioq_idx); + + return 0; + } + } + + return -1; +} + +int lldp_mib_remove_objects(lldp_chassis *c, tlv_packet *tlv) { + lldp_neighbour_port *p, *q; + uint8_t *data; + uint16_t length; + uint8_t type; + int r; + + assert_return(c, -EINVAL); + assert_return(tlv, -EINVAL); + + r = lldp_read_port_id(tlv, &type, &length, &data); + if (r < 0) + return r; + + LIST_FOREACH_SAFE(port, p, q, c->ports) { + + /* Find the port */ + if (p->type == type && p->length == length && !memcmp(p->data, data, p->length)) { + lldp_neighbour_port_remove_and_free(p); + break; + } + } + + return 0; +} + +int lldp_mib_add_objects(Prioq *by_expiry, + Hashmap *neighbour_mib, + tlv_packet *tlv) { + _cleanup_lldp_neighbour_port_free_ lldp_neighbour_port *p = NULL; + _cleanup_lldp_chassis_free_ lldp_chassis *c = NULL; + lldp_chassis_id chassis_id; + bool new_chassis = false; + uint8_t subtype, *data; + uint16_t ttl, length; + int r; + + assert_return(by_expiry, -EINVAL); + assert_return(neighbour_mib, -EINVAL); + assert_return(tlv, -EINVAL); + + r = lldp_read_chassis_id(tlv, &subtype, &length, &data); + if (r < 0) + goto drop; + + r = lldp_read_ttl(tlv, &ttl); + if (r < 0) + goto drop; + + /* Make hash key */ + chassis_id.type = subtype; + chassis_id.length = length; + chassis_id.data = data; + + /* Try to find the Chassis */ + c = hashmap_get(neighbour_mib, &chassis_id); + if (!c) { + + /* Don't create chassis if ttl 0 is received . Silently drop it */ + if (ttl == 0) { + log_lldp("TTL value 0 received. Skiping Chassis creation."); + goto drop; + } + + /* Admission Control: Can we store this packet ? */ + if (hashmap_size(neighbour_mib) >= LLDP_MIB_MAX_CHASSIS) { + + log_lldp("Exceeding number of chassie: %d. Dropping ...", + hashmap_size(neighbour_mib)); + goto drop; + } + + r = lldp_chassis_new(tlv, by_expiry, neighbour_mib, &c); + if (r < 0) + goto drop; + + new_chassis = true; + + r = hashmap_put(neighbour_mib, &c->chassis_id, c); + if (r < 0) + goto drop; + + } else { + + /* When the TTL field is set to zero, the receiving LLDP agent is notified all + * system information associated with the LLDP agent/port is to be deleted */ + if (ttl == 0) { + log_lldp("TTL value 0 received . Deleting associated Port ..."); + + lldp_mib_remove_objects(c, tlv); + + c = NULL; + goto drop; + } + + /* if we already have this port just update it */ + r = lldp_mib_update_objects(c, tlv); + if (r >= 0) { + c = NULL; + return r; + } + + /* Admission Control: Can this port attached to the existing chassis ? */ + if (REFCNT_GET(c->n_ref) >= LLDP_MIB_MAX_PORT_PER_CHASSIS) { + log_lldp("Port limit reached. Chassis has: %d ports. Dropping ...", + REFCNT_GET(c->n_ref)); + + c = NULL; + goto drop; + } + } + + /* This is a new port */ + r = lldp_neighbour_port_new(c, tlv, &p); + if (r < 0) + goto drop; + + r = prioq_put(c->by_expiry, p, &p->prioq_idx); + if (r < 0) + goto drop; + + /* Attach new port to chassis */ + LIST_PREPEND(port, c->ports, p); + REFCNT_INC(c->n_ref); + + p = NULL; + c = NULL; + + return 0; + + drop: + tlv_packet_free(tlv); + + if (new_chassis) + hashmap_remove(neighbour_mib, &c->chassis_id); + + return r; +} + +void lldp_neighbour_port_remove_and_free(lldp_neighbour_port *p) { + lldp_chassis *c; + + assert(p); + assert(p->c); + + c = p->c; + + prioq_remove(c->by_expiry, p, &p->prioq_idx); + + LIST_REMOVE(port, c->ports, p); + lldp_neighbour_port_free(p); + + /* Drop the Chassis if no port is attached */ + if (REFCNT_DEC(c->n_ref) <= 1) { + hashmap_remove(c->neighbour_mib, &c->chassis_id); + lldp_chassis_free(c); + } +} + +void lldp_neighbour_port_free(lldp_neighbour_port *p) { + + if(!p) + return; + + tlv_packet_free(p->packet); + + free(p->data); + free(p); +} + +int lldp_neighbour_port_new(lldp_chassis *c, + tlv_packet *tlv, + lldp_neighbour_port **ret) { + _cleanup_lldp_neighbour_port_free_ lldp_neighbour_port *p; + uint16_t length, ttl; + uint8_t *data; + uint8_t type; + int r; + + assert(tlv); + + r = lldp_read_port_id(tlv, &type, &length, &data); + if (r < 0) + return r; + + r = lldp_read_ttl(tlv, &ttl); + if (r < 0) + return r; + + p = new0(lldp_neighbour_port, 1); + if (!p) + return -ENOMEM; + + p->c = c; + p->type = type; + p->length = length; + p->packet = tlv; + p->prioq_idx = PRIOQ_IDX_NULL; + p->until = ttl * USEC_PER_SEC + now(clock_boottime_or_monotonic()); + + p->data = memdup(data, length); + if (!p->data) + return -ENOMEM; + + *ret = p; + p = NULL; + + return 0; +} + +void lldp_chassis_free(lldp_chassis *c) { + + if (!c) + return; + + if (REFCNT_GET(c->n_ref) > 1) + return; + + free(c->chassis_id.data); + free(c); +} + +int lldp_chassis_new(tlv_packet *tlv, + Prioq *by_expiry, + Hashmap *neighbour_mib, + lldp_chassis **ret) { + _cleanup_lldp_chassis_free_ lldp_chassis *c; + uint16_t length; + uint8_t *data; + uint8_t type; + int r; + + assert(tlv); + + r = lldp_read_chassis_id(tlv, &type, &length, &data); + if (r < 0) + return r; + + c = new0(lldp_chassis, 1); + if (!c) + return -ENOMEM; + + c->n_ref = REFCNT_INIT; + c->chassis_id.type = type; + c->chassis_id.length = length; + + c->chassis_id.data = memdup(data, length); + if (!c->chassis_id.data) + return -ENOMEM; + + LIST_HEAD_INIT(c->ports); + + c->by_expiry = by_expiry; + c->neighbour_mib = neighbour_mib; + + *ret = c; + c = NULL; + + return 0; +} diff --git a/src/libsystemd-network/lldp-internal.h b/src/libsystemd-network/lldp-internal.h new file mode 100644 index 000000000..dbaf38979 --- /dev/null +++ b/src/libsystemd-network/lldp-internal.h @@ -0,0 +1,93 @@ +/*-*- 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 . +***/ + +#pragma once + +#include "log.h" +#include "list.h" +#include "refcnt.h" +#include "lldp-tlv.h" +#include "prioq.h" + +typedef struct lldp_neighbour_port lldp_neighbour_port; +typedef struct lldp_chassis lldp_chassis; +typedef struct lldp_chassis_id lldp_chassis_id; + +struct lldp_neighbour_port { + uint8_t type; + uint8_t *data; + + uint16_t length; + usec_t until; + + unsigned prioq_idx; + + lldp_chassis *c; + tlv_packet *packet; + + LIST_FIELDS(lldp_neighbour_port, port); +}; + +int lldp_neighbour_port_new(lldp_chassis *c, tlv_packet *tlv, lldp_neighbour_port **ret); +void lldp_neighbour_port_free(lldp_neighbour_port *p); +void lldp_neighbour_port_remove_and_free(lldp_neighbour_port *p); + +DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_neighbour_port *, lldp_neighbour_port_free); +#define _cleanup_lldp_neighbour_port_free_ _cleanup_(lldp_neighbour_port_freep) + +struct lldp_chassis_id { + uint8_t type; + uint16_t length; + + uint8_t *data; +}; + +struct lldp_chassis { + RefCount n_ref; + + lldp_chassis_id chassis_id; + + Prioq *by_expiry; + Hashmap *neighbour_mib; + + LIST_HEAD(lldp_neighbour_port, ports); +}; + +int lldp_chassis_new(tlv_packet *tlv, + Prioq *by_expiry, + Hashmap *neighbour_mib, + lldp_chassis **ret); + +void lldp_chassis_free(lldp_chassis *c); + +DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_chassis *, lldp_chassis_free); +#define _cleanup_lldp_chassis_free_ _cleanup_(lldp_chassis_freep) + +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); + +#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 new file mode 100644 index 000000000..cfab91146 --- /dev/null +++ b/src/libsystemd-network/lldp-network.c @@ -0,0 +1,111 @@ +/*-*- 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 . +***/ + +#include +#include + +#include "socket-util.h" +#include "lldp-tlv.h" +#include "lldp-network.h" +#include "sd-lldp.h" + +int lldp_network_bind_raw_socket(int ifindex) { + typedef struct LLDPFrame { + struct ethhdr hdr; + uint8_t tlvs[0]; + } LLDPFrame; + + struct sock_filter filter[] = { + BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest)), /* A <- 4 bytes of destination MAC */ + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0180c200, 1, 0), /* A != 01:80:c2:00 */ + BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_dest) + 4), /* A <- remaining 2 bytes of destination MAC */ + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0000, 3, 0), /* A != 00:00 */ + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x0003, 2, 0), /* A != 00:03 */ + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x000e, 1, 0), /* A != 00:0e */ + BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(LLDPFrame, hdr.h_proto)), /* A <- protocol */ + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, ETHERTYPE_LLDP, 1, 0), /* A != ETHERTYPE_LLDP */ + BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ + BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1), /* accept packet */ + }; + + struct sock_fprog fprog = { + .len = ELEMENTSOF(filter), + .filter = filter + }; + + _cleanup_close_ int s = -1; + + union sockaddr_union saddrll = { + .ll.sll_family = AF_PACKET, + .ll.sll_ifindex = ifindex, + }; + + int r; + + assert(ifindex > 0); + + s = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL)); + if (s < 0) + return -errno; + + r = setsockopt(s, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)); + if (r < 0) + return -errno; + + r = bind(s, &saddrll.sa, sizeof(saddrll.ll)); + if (r < 0) + return -errno; + + r = s; + s = -1; + + 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 new file mode 100644 index 000000000..b7f8d3bf8 --- /dev/null +++ b/src/libsystemd-network/lldp-network.h @@ -0,0 +1,28 @@ +/*-*- 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 . +***/ + +#pragma once + +#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 new file mode 100644 index 000000000..12b72eedc --- /dev/null +++ b/src/libsystemd-network/lldp-port.c @@ -0,0 +1,116 @@ +/*-*- 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 . +***/ + +#include "async.h" +#include "lldp-port.h" +#include "lldp-network.h" + +int lldp_port_start(lldp_port *p) { + int r; + + assert_return(p, -EINVAL); + + r = lldp_network_bind_raw_socket(p->ifindex); + if (r < 0) + return r; + + p->rawfd = r; + + r = sd_event_add_io(p->event, &p->lldp_port_rx, + p->rawfd, EPOLLIN, lldp_receive_packet, p); + if (r < 0) { + log_debug("Failed to allocate event source: %s", strerror(-r)); + return r; + } + + r = sd_event_source_set_priority(p->lldp_port_rx, p->event_priority); + if (r < 0) { + log_debug("Failed to set event priority: %s", strerror(-r)); + goto fail; + } + + r = sd_event_source_set_description(p->lldp_port_rx, "lldp-port-rx"); + if (r < 0) { + log_debug("Failed to set event name: %s", strerror(-r)); + goto fail; + } + + return 0; + +fail: + lldp_port_stop(p); + + return r; +} + +int lldp_port_stop(lldp_port *p) { + + assert_return(p, -EINVAL); + + p->rawfd = asynchronous_close(p->rawfd); + p->lldp_port_rx = sd_event_source_unref(p->lldp_port_rx); + + return 0; +} + +void lldp_port_free(lldp_port *p) { + if (!p) + return; + + lldp_port_stop(p); + + free(p->ifname); + free(p); +} + +int lldp_port_new(int ifindex, + char *ifname, + const struct ether_addr *addr, + void *userdata, + lldp_port **ret) { + _cleanup_free_ lldp_port *p = NULL; + + assert_return(ifindex, -EINVAL); + assert_return(ifname, -EINVAL); + assert_return(addr, -EINVAL); + + p = new0(lldp_port, 1); + if (!p) + return -ENOMEM; + + p->rawfd = -1; + p->ifindex = ifindex; + + p->ifname = strdup(ifname); + if (!p->ifname) + return -ENOMEM; + + memcpy(&p->mac, addr, ETH_ALEN); + + p->userdata = userdata; + + *ret = p; + + p = NULL; + + return 0; +} diff --git a/src/libsystemd-network/lldp-port.h b/src/libsystemd-network/lldp-port.h new file mode 100644 index 000000000..1b1ae040d --- /dev/null +++ b/src/libsystemd-network/lldp-port.h @@ -0,0 +1,61 @@ +/*-*- 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 . +***/ + +#pragma once + +#include + +#include "sd-event.h" +#include "sd-lldp.h" + +typedef struct lldp_port lldp_port; + +struct lldp_port { + LLDPPortStatus status; + + int ifindex; + char *ifname; + + struct ether_addr mac; + + int rawfd; + + sd_event *event; + sd_event_source *lldp_port_rx; + + int event_priority; + + void *userdata; +}; + +int lldp_port_new(int ifindex, + char *ifname, + const struct ether_addr *addr, + void *userdata, + lldp_port **ret); +void lldp_port_free(lldp_port *p); + +DEFINE_TRIVIAL_CLEANUP_FUNC(lldp_port*, lldp_port_free); +#define _cleanup_lldp_port_free_ _cleanup_(lldp_port_freep) + +int lldp_port_start(lldp_port *p); +int lldp_port_stop(lldp_port *p); diff --git a/src/libsystemd-network/lldp-tlv.c b/src/libsystemd-network/lldp-tlv.c new file mode 100644 index 000000000..e43d70d3c --- /dev/null +++ b/src/libsystemd-network/lldp-tlv.c @@ -0,0 +1,321 @@ +/*-*- 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 . +***/ + +#include +#include + +#include "macro.h" +#include "lldp-tlv.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); + + *ret = m; + + return 0; +} + +void tlv_packet_free(tlv_packet *m) { + tlv_section *s, *n; + + if (!m) + return; + + LIST_FOREACH_SAFE(section, s, n, m->sections) + tlv_section_free(s); + + free(m); +} + +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; + 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; + 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; + 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->length; + + m->container->read_pos += m->container->length; + + return 0; +} + +int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) { + void *val; + 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->length; + + m->container->read_pos += m->container->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 herader */ + 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; + 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); + + 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 = 0; + return -1; + } + + return 0; +} + +int lldp_tlv_packet_exit_container(tlv_packet *m) { + assert_return(m, -EINVAL); + + m->container = 0; + + return 0; +} diff --git a/src/libsystemd-network/lldp-tlv.h b/src/libsystemd-network/lldp-tlv.h new file mode 100644 index 000000000..ce3334e11 --- /dev/null +++ b/src/libsystemd-network/lldp-tlv.h @@ -0,0 +1,87 @@ +/*-*- 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 . +***/ + +#pragma once + +#include + +#include "util.h" +#include "lldp.h" +#include "list.h" + +typedef struct tlv_packet tlv_packet; +typedef struct tlv_section tlv_section; + +struct tlv_section { + uint16_t type; + uint16_t length; + + uint8_t *read_pos; + uint8_t *data; + + LIST_FIELDS(tlv_section, section); +}; + +int tlv_section_new(tlv_section **ret); +void tlv_section_free(tlv_section *ret); + +struct tlv_packet { + uint16_t type; + uint16_t length; + usec_t ts; + + uint8_t *container_pos; + uint8_t pdu[ETHER_MAX_LEN]; + + void *userdata; + + struct ether_addr mac; + tlv_section *container; + + LIST_HEAD(tlv_section, sections); +}; + +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) + +int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type); +int lldp_tlv_packet_close_container(tlv_packet *m); + +int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length); +int tlv_packet_append_u8(tlv_packet *m, uint8_t data); +int tlv_packet_append_u16(tlv_packet *m, uint16_t data); +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_exit_container(tlv_packet *m); + +int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length); +int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length); +int tlv_packet_read_u8(tlv_packet *m, uint8_t *data); +int tlv_packet_read_u16(tlv_packet *m, uint16_t *data); +int tlv_packet_read_u32(tlv_packet *m, uint32_t *data); + +int tlv_packet_parse_pdu(tlv_packet *t, uint16_t size); diff --git a/src/libsystemd-network/lldp.h b/src/libsystemd-network/lldp.h new file mode 100644 index 000000000..5e4b283e2 --- /dev/null +++ b/src/libsystemd-network/lldp.h @@ -0,0 +1,115 @@ +/*-*- 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 . +***/ + +#pragma once + +#define LLDP_MULTICAST_ADDR { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e } + +#define ETHERTYPE_LLDP 0x88cc + +/* IEEE 802.3AB Clause 9: TLV Types */ +typedef enum LLDPTypes { + LLDP_TYPE_END = 0, + LLDP_TYPE_CHASSIS_ID = 1, + LLDP_TYPE_PORT_ID = 2, + LLDP_TYPE_TTL = 3, + LLDP_TYPE_PORT_DESCRIPTION = 4, + LLDP_TYPE_SYSTEM_NAME = 5, + LLDP_TYPE_SYSTEM_DESCRIPTION = 6, + LLDP_TYPE_SYSTEM_CAPABILITIES = 7, + LLDP_TYPE_MGMT_ADDRESS = 8, + LLDP_TYPE_PRIVATE = 127, + _LLDP_TYPE_MAX, + _LLDP_TYPE_INVALID = -1, +} LLDPTypes; + +/* IEEE 802.3AB Clause 9.5.2: Chassis subtypes */ +typedef enum LLDPChassisSubtypes { + LLDP_CHASSIS_SUBTYPE_RESERVED = 0, + LLDP_CHASSIS_SUBTYPE_CHASSIS_COMPONENT = 1, + LLDP_CHASSIS_SUBTYPE_INTERFACE_ALIAS = 2, + LLDP_CHASSIS_SUBTYPE_PORT_COMPONENT = 3, + LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS = 4, + LLDP_CHASSIS_SUBTYPE_NETWORK_ADDRESS = 5, + LLDP_CHASSIS_SUBTYPE_INTERFACE_NAME = 6, + LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED = 7, + _LLDP_CHASSIS_SUBTYPE_MAX, + _LLDP_CHASSIS_SUBTYPE_INVALID = -1, +} LLDPChassisSubtypes; + +/* IEEE 802.3AB Clause 9.5.3: Port subtype */ +typedef enum LLDPPortSubtypes { + LLDP_PORT_SUBTYPE_RESERVED = 0, + LLDP_PORT_SUBTYPE_INTERFACE_ALIAS = 1, + LLDP_PORT_SUBTYPE_PORT_COMPONENT = 2, + LLDP_PORT_SUBTYPE_MAC_ADDRESS = 3, + LLDP_PORT_SUBTYPE_NETWORK = 4, + LLDP_PORT_SUBTYPE_INTERFACE_NAME = 5, + LLDP_PORT_SUBTYPE_AGENT_CIRCUIT_ID = 6, + LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED = 7, + _LLDP_PORT_SUBTYPE_MAX, + _LLDP_PORT_SUBTYPE_INVALID = -1 +} LLDPPortSubtypes; + +typedef enum LLDPSystemCapabilities { + LLDP_SYSTEM_CAPABILITIES_OTHER = 1 << 0, + LLDP_SYSTEM_CAPABILITIES_REPEATER = 1 << 1, + LLDP_SYSTEM_CAPABILITIES_BRIDGE = 1 << 2, + LLDP_SYSTEM_CAPABILITIES_WLAN_AP = 1 << 3, + LLDP_SYSTEM_CAPABILITIES_ROUTER = 1 << 4, + LLDP_SYSTEM_CAPABILITIES_PHONE = 1 << 5, + LLDP_SYSTEM_CAPABILITIES_DOCSIS = 1 << 6, + LLDP_SYSTEM_CAPABILITIES_STATION = 1 << 7, + LLDP_SYSTEM_CAPABILITIES_CVLAN = 1 << 8, + LLDP_SYSTEM_CAPABILITIES_SVLAN = 1 << 9, + LLDP_SYSTEM_CAPABILITIES_TPMR = 1 << 10, + _LLDP_SYSTEM_CAPABILITIES_MAX, + _LLDP_SYSTEM_CAPABILITIES_INVALID = -1, +} LLDPSystemCapabilities; + +typedef enum LLDPMedSubtype { + LLDP_MED_SUBTYPE_RESERVED = 0, + LLDP_MED_SUBTYPE_CAPABILITIES = 1, + LLDP_MED_SUBTYPE_NETWORK_POLICY = 2, + LLDP_MED_SUBTYPE_LOCATION_ID = 3, + LLDP_MED_SUBTYPE_EXTENDED_PVMDI = 4, + LLDP_MED_SUBTYPE_INV_HWREV = 5, + LLDP_MED_SUBTYPE_INV_FWREV = 6, + LLDP_MED_SUBTYPE_INV_SWREV = 7, + LLDP_MED_SUBTYPE_INV_SERIAL = 8, + LLDP_MED_SUBTYPE_INV_MANUFACTURER = 9, + LLDP_MED_SUBTYPE_INV_MODELNAME = 10, + LLDP_MED_SUBTYPE_INV_ASSETID = 11, + _LLDP_MED_SUBTYPE_MAX, + _LLDP_MED_SUBTYPE_INVALID = -1, +} LLDPMedSubtype; + +typedef enum LLDPMedCapability { + LLDP_MED_CAPABILITY_CAPAPILITIES = 1 << 0, + LLDP_MED_CAPABILITY_NETWORK_POLICY = 1 << 1, + LLDP_MED_CAPABILITY_LOCATION_ID = 1 << 2, + LLDP_MED_CAPABILITY_EXTENDED_PSE = 1 << 3, + LLDP_MED_CAPABILITY_EXTENDED_PD = 1 << 4, + LLDP_MED_CAPABILITY_INVENTORY = 1 << 5, + LLDP_MED_CAPABILITY_MAX, + LLDP_MED_CAPABILITY_INVALID = -1, +} LLDPMedCapability; diff --git a/src/libsystemd-network/sd-lldp.c b/src/libsystemd-network/sd-lldp.c new file mode 100644 index 000000000..97d157844 --- /dev/null +++ b/src/libsystemd-network/sd-lldp.c @@ -0,0 +1,493 @@ +/*-*- 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 . +***/ + +#include + +#include "siphash24.h" +#include "hashmap.h" +#include "event-util.h" + +#include "lldp-tlv.h" +#include "lldp-port.h" +#include "sd-lldp.h" +#include "prioq.h" +#include "lldp-internal.h" + +/* Section 10.5.2.2 Reception counters */ +struct lldp_agent_statitics { + uint64_t stats_ageouts_total; + uint64_t stats_frames_discarded_total; + uint64_t stats_frames_in_errors_total; + uint64_t stats_frames_in_total; + uint64_t stats_tlvs_discarded_total; + uint64_t stats_tlvs_unrecognized_total; +}; + +struct sd_lldp { + lldp_port *port; + + Prioq *by_expiry; + Hashmap *neighbour_mib; + + lldp_agent_statitics statitics; +}; + +static unsigned long chassis_id_hash_func(const void *p, + const uint8_t hash_key[HASH_KEY_SIZE]) { + uint64_t u; + const lldp_chassis_id *id = p; + + assert(id); + + siphash24((uint8_t *) &u, id->data, id->length, hash_key); + + return (unsigned long) u; +} + +static int chassis_id_compare_func(const void *_a, const void *_b) { + const lldp_chassis_id *a, *b; + + a = _a; + b = _b; + + assert(!a->length || a->data); + assert(!b->length || b->data); + + if (a->type != b->type) + return -1; + + if (a->length != b->length) + return a->length < b->length ? -1 : 1; + + return memcmp(a->data, b->data, a->length); +} + +static const struct hash_ops chassis_id_hash_ops = { + .hash = chassis_id_hash_func, + .compare = chassis_id_compare_func +}; + +static void lldp_mib_delete_objects(sd_lldp *lldp); + +static int lldp_receive_frame(sd_lldp *lldp, tlv_packet *tlv) { + int r; + + assert(lldp); + assert(tlv); + + /* Remove expired packets */ + if (prioq_size(lldp->by_expiry) > 0) + lldp_mib_delete_objects(lldp); + + r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv); + if (r < 0) + goto out; + + log_lldp("Packet added. MIB size: %d , PQ size: %d", + hashmap_size(lldp->neighbour_mib), + prioq_size(lldp->by_expiry)); + + lldp->statitics.stats_frames_in_total ++; + + return 0; + + out: + if (r < 0) + log_lldp("Receive frame failed: %s", strerror(-r)); + + return 0; +} + +/* 10.3.2 LLDPDU validation: rxProcessFrame() */ +int lldp_handle_packet(tlv_packet *tlv, uint16_t length) { + uint16_t type, len, i, l, t; + bool chassis_id = false; + bool malformed = false; + bool port_id = false; + bool ttl = false; + bool end = false; + lldp_port *port; + uint8_t *p, *q; + sd_lldp *lldp; + int r; + + assert(tlv); + assert(length > 0); + + port = (lldp_port *) tlv->userdata; + lldp = (sd_lldp *) port->userdata; + + if (lldp->port->status == LLDP_PORT_STATUS_DISABLED) { + log_lldp("Port is disabled : %s . Dropping ...", + lldp->port->ifname); + goto out; + } + + p = tlv->pdu; + p += sizeof(struct ether_header); + + for (i = 1, l = 0; l <= length; i++) { + + memcpy(&t, p, sizeof(uint16_t)); + + type = ntohs(t) >> 9; + len = ntohs(t) & 0x01ff; + + if (type == LLDP_TYPE_END) { + if (len != 0) { + log_lldp("TLV type end is not length 0. Length:%d received . Dropping ...", + len); + + malformed = true; + goto out; + } + + end = true; + + break; + } else if (type >=_LLDP_TYPE_MAX) { + log_lldp("TLV type not recognized %d . Dropping ...", + type); + + malformed = true; + goto out; + } + + /* skip type and lengh encoding */ + p += 2; + q = p; + + p += len; + l += (len + 2); + + if (i <= 3) { + if (i != type) { + log_lldp("TLV missing or out of order. Dropping ..."); + + malformed = true; + goto out; + } + } + + switch(type) { + case LLDP_TYPE_CHASSIS_ID: + + if (len < 2) { + log_lldp("Received malformed Chassis ID TLV len = %d. Dropping", + len); + + malformed = true; + goto out; + } + + if (chassis_id) { + log_lldp("Duplicate Chassis ID TLV found. Dropping ..."); + + malformed = true; + goto out; + } + + /* Look what subtype it has */ + if (*q == LLDP_CHASSIS_SUBTYPE_RESERVED || + *q > LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED) { + log_lldp("Unknown subtype: %d found in Chassis ID TLV . Dropping ...", + *q); + + malformed = true; + goto out; + + } + + chassis_id = true; + + break; + case LLDP_TYPE_PORT_ID: + + if (len < 2) { + log_lldp("Received malformed Port ID TLV len = %d. Dropping", + len); + + malformed = true; + goto out; + } + + if (port_id) { + log_lldp("Duplicate Port ID TLV found. Dropping ..."); + + malformed = true; + goto out; + } + + /* Look what subtype it has */ + if (*q == LLDP_PORT_SUBTYPE_RESERVED || + *q > LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED) { + log_lldp("Unknown subtype: %d found in Port ID TLV . Dropping ...", + *q); + + malformed = true; + goto out; + + } + + port_id = true; + + break; + case LLDP_TYPE_TTL: + + if(len != 2) { + log_lldp( + "Received invalid lenth: %d TTL TLV. Dropping ...", + len); + + malformed = true; + goto out; + } + + if (ttl) { + log_lldp("Duplicate TTL TLV found. Dropping ..."); + + malformed = true; + goto out; + } + + ttl = true; + + break; + default: + + if (len == 0) { + log_lldp("TLV type = %d's, length 0 received . Dropping ...", + type); + + malformed = true; + goto out; + } + break; + } + } + + if(!chassis_id || !port_id || !ttl || !end) { + log_lldp( "One or more mandotory TLV missing . Dropping ..."); + + malformed = true; + goto out; + + } + + r = tlv_packet_parse_pdu(tlv, length); + if (r < 0) { + log_lldp( "Failed to parse the TLV. Dropping ..."); + + malformed = true; + goto out; + } + + return lldp_receive_frame(lldp, tlv); + + out: + if (malformed) { + lldp->statitics.stats_frames_discarded_total ++; + lldp->statitics.stats_frames_in_errors_total ++; + } + + tlv_packet_free(tlv); + + return 0; +} + +static int ttl_expiry_item_prioq_compare_func(const void *a, const void *b) { + const lldp_neighbour_port *p = a, *q = b; + + if (p->until < q->until) + return -1; + + if (p->until > q->until) + return 1; + + return 0; +} + +/* 10.5.5.2.1 mibDeleteObjects () + * The mibDeleteObjects () procedure deletes all information in the LLDP remote + * systems MIB associated with the MSAP identifier if an LLDPDU is received with + * an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */ + +static void lldp_mib_delete_objects(sd_lldp *lldp) { + lldp_neighbour_port *p; + usec_t t = 0; + + /* Remove all entries that are past their TTL */ + for (;;) { + + if (prioq_size(lldp->by_expiry) <= 0) + break; + + p = prioq_peek(lldp->by_expiry); + if (!p) + break; + + if (t <= 0) + t = now(CLOCK_BOOTTIME); + + if (p->until > t) + break; + + lldp_neighbour_port_remove_and_free(p); + + lldp->statitics.stats_ageouts_total ++; + } +} + +static void lldp_mib_objects_flush(sd_lldp *lldp) { + lldp_neighbour_port *p, *q; + lldp_chassis *c; + + assert(lldp); + assert(lldp->neighbour_mib); + assert(lldp->by_expiry); + + /* Drop all packets */ + while ((c = hashmap_steal_first(lldp->neighbour_mib))) { + + LIST_FOREACH_SAFE(port, p, q, c->ports) { + lldp_neighbour_port_remove_and_free(p); + } + } + + assert(hashmap_size(lldp->neighbour_mib) == 0); + assert(prioq_size(lldp->by_expiry) == 0); +} + +int sd_lldp_start(sd_lldp *lldp) { + int r; + + assert_return(lldp, -EINVAL); + assert_return(lldp->port, -EINVAL); + + lldp->port->status = LLDP_PORT_STATUS_ENABLED; + + r = lldp_port_start(lldp->port); + if (r < 0) { + log_lldp("Failed to start Port : %s , %s", + lldp->port->ifname, + strerror(-r)); + return r; + } + + return 0; +} + +int sd_lldp_stop(sd_lldp *lldp) { + int r; + + assert_return(lldp, -EINVAL); + assert_return(lldp->port, -EINVAL); + + lldp->port->status = LLDP_PORT_STATUS_DISABLED; + + r = lldp_port_stop(lldp->port); + if (r < 0) + return r; + + lldp_mib_objects_flush(lldp); + + return 0; +} + +int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority) { + int r; + + assert_return(lldp, -EINVAL); + assert_return(!lldp->port->event, -EBUSY); + + if (event) + lldp->port->event = sd_event_ref(event); + else { + r = sd_event_default(&lldp->port->event); + if (r < 0) + return r; + } + + lldp->port->event_priority = priority; + + return 0; +} + +int sd_lldp_detach_event(sd_lldp *lldp) { + + assert_return(lldp, -EINVAL); + + lldp->port->event = sd_event_unref(lldp->port->event); + + return 0; +} + +void sd_lldp_free(sd_lldp *lldp) { + + if (!lldp) + return; + + /* Drop all packets */ + lldp_mib_objects_flush(lldp); + + lldp_port_free(lldp->port); + + hashmap_free(lldp->neighbour_mib); + prioq_free(lldp->by_expiry); + + free(lldp); +} + +int sd_lldp_new(int ifindex, + char *ifname, + struct ether_addr *mac, + sd_lldp **ret) { + _cleanup_sd_lldp_free_ sd_lldp *lldp = NULL; + int r; + + assert_return(ret, -EINVAL); + assert_return(ifindex > 0, -EINVAL); + assert_return(ifname, -EINVAL); + assert_return(mac, -EINVAL); + + lldp = new0(sd_lldp, 1); + if (!lldp) + return -ENOMEM; + + r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port); + if (r < 0) + return r; + + lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops); + if (!lldp->neighbour_mib) + return -ENOMEM; + + r = prioq_ensure_allocated(&lldp->by_expiry, + ttl_expiry_item_prioq_compare_func); + if (r < 0) + return r; + + *ret = lldp; + lldp = NULL; + + return 0; +} diff --git a/src/libsystemd-network/sd-lldp.h b/src/libsystemd-network/sd-lldp.h new file mode 100644 index 000000000..95d802df8 --- /dev/null +++ b/src/libsystemd-network/sd-lldp.h @@ -0,0 +1,51 @@ +/*-*- 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 . +***/ + +#pragma once + +#include "lldp-tlv.h" +#include "sd-event.h" + +typedef struct sd_lldp sd_lldp; +typedef struct lldp_agent_statitics lldp_agent_statitics; + +typedef enum LLDPPortStatus { + LLDP_PORT_STATUS_NONE, + LLDP_PORT_STATUS_ENABLED, + LLDP_PORT_STATUS_DISABLED, + _LLDP_PORT_STATUS_MAX, + _LLDP_PORT_STATUS_INVALID = -1, +} LLDPPortStatus; + +int sd_lldp_new(int ifindex, char *ifname, struct ether_addr *mac, sd_lldp **ret); +void sd_lldp_free(sd_lldp *lldp); + +DEFINE_TRIVIAL_CLEANUP_FUNC(sd_lldp *, sd_lldp_free); +#define _cleanup_sd_lldp_free_ _cleanup_(sd_lldp_freep) + +int sd_lldp_start(sd_lldp *lldp); +int sd_lldp_stop(sd_lldp *lldp); + +int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority); +int sd_lldp_detach_event(sd_lldp *lldp); + +int lldp_handle_packet(tlv_packet *m, uint16_t length); diff --git a/src/libsystemd-network/test-lldp.c b/src/libsystemd-network/test-lldp.c new file mode 100644 index 000000000..e9d5d7bb6 --- /dev/null +++ b/src/libsystemd-network/test-lldp.c @@ -0,0 +1,233 @@ +/*-*- 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 . +***/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "macro.h" +#include "lldp.h" +#include "lldp-tlv.h" + +#define TEST_LLDP_PORT "em1" +#define TEST_LLDP_TYPE_SYSTEM_NAME "systemd-lldp" +#define TEST_LLDP_TYPE_SYSTEM_DESC "systemd-lldp-desc" + +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; + const uint8_t lldp_dst[] = LLDP_MULTICAST_ADDR; + struct ether_header ether; + + /* Append ethernet header */ + memset(ðer, 0, sizeof(ether)); + memcpy(ðer.ether_dhost, lldp_dst, ETHER_ADDR_LEN); + memcpy(ðer.ether_shost, &mac_addr, ETHER_ADDR_LEN); + ether.ether_type = htons(ETHERTYPE_LLDP); + + assert_se(tlv_packet_new(&m) >= 0); + + assert_se(tlv_packet_append_bytes(m, ðer, sizeof(struct ether_header)) >= 0); + + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_CHASSIS_ID) >= 0); + + assert_se(tlv_packet_append_u8(m, LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS) >= 0); + assert_se(tlv_packet_append_bytes(m, &mac_addr, ETHER_ADDR_LEN) >= 0); + + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + /* port name */ + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_PORT_ID) >= 0); + + assert_se(tlv_packet_append_u8(m, LLDP_PORT_SUBTYPE_INTERFACE_NAME) >= 0); + assert_se(tlv_packet_append_bytes(m, TEST_LLDP_PORT, strlen(TEST_LLDP_PORT) + 1) >= 0); + + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + /* ttl */ + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_TTL) >= 0); + + assert_se(tlv_packet_append_u16(m, 170) >= 0); + + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + /* system name */ + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0); + + assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_NAME, + strlen(TEST_LLDP_TYPE_SYSTEM_NAME)) >= 0); + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + /* system descrition */ + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0); + + assert_se(tlv_packet_append_bytes(m, TEST_LLDP_TYPE_SYSTEM_DESC, + strlen(TEST_LLDP_TYPE_SYSTEM_DESC)) >= 0); + + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + /* Mark end of packet */ + assert_se(lldp_tlv_packet_open_container(m, LLDP_TYPE_END) >= 0); + assert_se(lldp_tlv_packet_close_container(m) >= 0); + + *ret = m; + + m = NULL; + + return 0; +} + +static int lldp_parse_chassis_tlv(tlv_packet *m, uint8_t *type) { + uint8_t *p, subtype; + uint16_t length; + + assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_CHASSIS_ID) >= 0); + assert_se(tlv_packet_read_u8(m, &subtype) >= 0); + + switch (subtype) { + case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS: + + *type = LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS; + assert_se(tlv_packet_read_bytes(m, &p, &length) >= 0); + + assert_se(memcmp(p, &mac_addr.ether_addr_octet, ETHER_ADDR_LEN) == 0); + + break; + default: + assert_not_reached("Unhandled option"); + } + + assert_se(lldp_tlv_packet_exit_container(m) >= 0); + + return 0; +} + +static int lldp_parse_port_id_tlv(tlv_packet *m) { + char *str = NULL, *p; + uint16_t length; + uint8_t subtype; + + assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_PORT_ID) >= 0); + + assert_se(tlv_packet_read_u8(m, &subtype) >= 0); + + switch (subtype) { + case LLDP_PORT_SUBTYPE_INTERFACE_NAME: + assert_se(tlv_packet_read_string(m, &str, &length) >= 0); + + p = malloc0(length + 1); + strncpy(p, str, length-1); + + assert_se(streq(p, TEST_LLDP_PORT) == 1); + break; + default: + assert_not_reached("Unhandled option"); + } + + assert_se(lldp_tlv_packet_exit_container(m) >= 0); + + return 0; +} + +static int lldp_parse_system_name_tlv(tlv_packet *m) { + char *str = NULL, *p; + uint16_t length; + + assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_NAME) >= 0); + assert_se(tlv_packet_read_string(m, &str, &length) >= 0); + + p = malloc0(length + 1); + strncpy(p, str, length); + + assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_NAME) == 1); + + assert_se(lldp_tlv_packet_exit_container(m) >= 0); + + return 1; +} + +static int lldp_parse_system_desc_tlv(tlv_packet *m) { + char *str = NULL, *p; + uint16_t length; + + assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_SYSTEM_DESCRIPTION) >= 0); + assert_se(tlv_packet_read_string(m, &str, &length) >= 0); + + p = malloc0(length + 1); + strncpy(p, str, length); + + assert_se(streq(p, TEST_LLDP_TYPE_SYSTEM_DESC) == 1); + + assert_se(lldp_tlv_packet_exit_container(m) >= 0); + + return 0; +} + +static int lldp_parse_ttl_tlv(tlv_packet *m) { + uint16_t ttl; + + assert_se(lldp_tlv_packet_enter_container(m, LLDP_TYPE_TTL) >= 0); + assert_se(tlv_packet_read_u16(m, &ttl) >= 0); + + assert_se(ttl == 170); + + assert_se(lldp_tlv_packet_exit_container(m) >= 0); + + return 0; +} + +static int lldp_parse_tlv_packet(tlv_packet *m, int len) { + uint8_t subtype; + + assert_se(tlv_packet_parse_pdu(m, len) >= 0); + assert_se(lldp_parse_chassis_tlv(m, &subtype) >= 0); + assert_se(lldp_parse_port_id_tlv(m) >= 0); + assert_se(lldp_parse_system_name_tlv(m) >= 0); + assert_se(lldp_parse_ttl_tlv(m) >= 0); + assert_se(lldp_parse_system_desc_tlv(m) >= 0); + + return 0; +} + +int main(int argc, char *argv[]) { + _cleanup_tlv_packet_free_ 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); + + return 0; +} -- 2.30.2