From b750101eb236130cf056c675997decbac904cc49 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Sun, 7 Apr 2024 17:35:18 +0200 Subject: Adding upstream version 252.22. Signed-off-by: Daniel Baumann --- src/libsystemd-network/sd-lldp-rx.c | 525 ++++++++++++++++++++++++++++++++++++ 1 file changed, 525 insertions(+) create mode 100644 src/libsystemd-network/sd-lldp-rx.c (limited to 'src/libsystemd-network/sd-lldp-rx.c') diff --git a/src/libsystemd-network/sd-lldp-rx.c b/src/libsystemd-network/sd-lldp-rx.c new file mode 100644 index 0000000..0479cff --- /dev/null +++ b/src/libsystemd-network/sd-lldp-rx.c @@ -0,0 +1,525 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ + +#include +#include +#include + +#include "sd-lldp-rx.h" + +#include "alloc-util.h" +#include "ether-addr-util.h" +#include "event-util.h" +#include "fd-util.h" +#include "lldp-neighbor.h" +#include "lldp-network.h" +#include "lldp-rx-internal.h" +#include "memory-util.h" +#include "network-common.h" +#include "socket-util.h" +#include "sort-util.h" +#include "string-table.h" + +#define LLDP_DEFAULT_NEIGHBORS_MAX 128U + +static const char * const lldp_rx_event_table[_SD_LLDP_RX_EVENT_MAX] = { + [SD_LLDP_RX_EVENT_ADDED] = "added", + [SD_LLDP_RX_EVENT_REMOVED] = "removed", + [SD_LLDP_RX_EVENT_UPDATED] = "updated", + [SD_LLDP_RX_EVENT_REFRESHED] = "refreshed", +}; + +DEFINE_STRING_TABLE_LOOKUP(lldp_rx_event, sd_lldp_rx_event_t); + +static void lldp_rx_flush_neighbors(sd_lldp_rx *lldp_rx) { + assert(lldp_rx); + + hashmap_clear(lldp_rx->neighbor_by_id); +} + +static void lldp_rx_callback(sd_lldp_rx *lldp_rx, sd_lldp_rx_event_t event, sd_lldp_neighbor *n) { + assert(lldp_rx); + assert(event >= 0 && event < _SD_LLDP_RX_EVENT_MAX); + + if (!lldp_rx->callback) + return (void) log_lldp_rx(lldp_rx, "Received '%s' event.", lldp_rx_event_to_string(event)); + + log_lldp_rx(lldp_rx, "Invoking callback for '%s' event.", lldp_rx_event_to_string(event)); + lldp_rx->callback(lldp_rx, event, n, lldp_rx->userdata); +} + +static int lldp_rx_make_space(sd_lldp_rx *lldp_rx, size_t extra) { + usec_t t = USEC_INFINITY; + bool changed = false; + + assert(lldp_rx); + + /* Remove all entries that are past their TTL, and more until at least the specified number of extra entries + * are free. */ + + for (;;) { + _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL; + + n = prioq_peek(lldp_rx->neighbor_by_expiry); + if (!n) + break; + + sd_lldp_neighbor_ref(n); + + if (hashmap_size(lldp_rx->neighbor_by_id) > LESS_BY(lldp_rx->neighbors_max, extra)) + goto remove_one; + + if (t == USEC_INFINITY) + t = now(CLOCK_BOOTTIME); + + if (n->until > t) + break; + + remove_one: + lldp_neighbor_unlink(n); + lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, n); + changed = true; + } + + return changed; +} + +static bool lldp_rx_keep_neighbor(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) { + assert(lldp_rx); + assert(n); + + /* Don't keep data with a zero TTL */ + if (n->ttl <= 0) + return false; + + /* Filter out data from the filter address */ + if (!ether_addr_is_null(&lldp_rx->filter_address) && + ether_addr_equal(&lldp_rx->filter_address, &n->source_address)) + return false; + + /* Only add if the neighbor has a capability we are interested in. Note that we also store all neighbors with + * no caps field set. */ + if (n->has_capabilities && + (n->enabled_capabilities & lldp_rx->capability_mask) == 0) + return false; + + /* Keep everything else */ + return true; +} + +static int lldp_rx_start_timer(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *neighbor); + +static int lldp_rx_add_neighbor(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) { + _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *old = NULL; + bool keep; + int r; + + assert(lldp_rx); + assert(n); + assert(!n->lldp_rx); + + keep = lldp_rx_keep_neighbor(lldp_rx, n); + + /* First retrieve the old entry for this MSAP */ + old = hashmap_get(lldp_rx->neighbor_by_id, &n->id); + if (old) { + sd_lldp_neighbor_ref(old); + + if (!keep) { + lldp_neighbor_unlink(old); + lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, old); + return 0; + } + + if (lldp_neighbor_equal(n, old)) { + /* Is this equal, then restart the TTL counter, but don't do anything else. */ + old->timestamp = n->timestamp; + lldp_rx_start_timer(lldp_rx, old); + lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REFRESHED, old); + return 0; + } + + /* Data changed, remove the old entry, and add a new one */ + lldp_neighbor_unlink(old); + + } else if (!keep) + return 0; + + /* Then, make room for at least one new neighbor */ + lldp_rx_make_space(lldp_rx, 1); + + r = hashmap_ensure_put(&lldp_rx->neighbor_by_id, &lldp_neighbor_hash_ops, &n->id, n); + if (r < 0) + goto finish; + + r = prioq_ensure_put(&lldp_rx->neighbor_by_expiry, lldp_neighbor_prioq_compare_func, n, &n->prioq_idx); + if (r < 0) { + assert_se(hashmap_remove(lldp_rx->neighbor_by_id, &n->id) == n); + goto finish; + } + + n->lldp_rx = lldp_rx; + + lldp_rx_start_timer(lldp_rx, n); + lldp_rx_callback(lldp_rx, old ? SD_LLDP_RX_EVENT_UPDATED : SD_LLDP_RX_EVENT_ADDED, n); + + return 1; + +finish: + if (old) + lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, old); + + return r; +} + +static int lldp_rx_handle_datagram(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) { + int r; + + assert(lldp_rx); + assert(n); + + r = lldp_neighbor_parse(n); + if (r < 0) + return r; + + r = lldp_rx_add_neighbor(lldp_rx, n); + if (r < 0) + return log_lldp_rx_errno(lldp_rx, r, "Failed to add datagram. Ignoring."); + + log_lldp_rx(lldp_rx, "Successfully processed LLDP datagram."); + return 0; +} + +static int lldp_rx_receive_datagram(sd_event_source *s, int fd, uint32_t revents, void *userdata) { + _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL; + ssize_t space, length; + sd_lldp_rx *lldp_rx = ASSERT_PTR(userdata); + struct timespec ts; + + assert(fd >= 0); + + space = next_datagram_size_fd(fd); + if (space < 0) { + if (ERRNO_IS_TRANSIENT(space) || ERRNO_IS_DISCONNECT(space)) + return 0; + + log_lldp_rx_errno(lldp_rx, space, "Failed to determine datagram size to read, ignoring: %m"); + return 0; + } + + n = lldp_neighbor_new(space); + if (!n) { + log_oom_debug(); + return 0; + } + + length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT); + if (length < 0) { + if (ERRNO_IS_TRANSIENT(errno) || ERRNO_IS_DISCONNECT(errno)) + return 0; + + log_lldp_rx_errno(lldp_rx, errno, "Failed to read LLDP datagram, ignoring: %m"); + return 0; + } + + if ((size_t) length != n->raw_size) { + log_lldp_rx(lldp_rx, "Packet size mismatch, ignoring"); + return 0; + } + + /* Try to get the timestamp of this packet if it is known */ + if (ioctl(fd, SIOCGSTAMPNS, &ts) >= 0) + triple_timestamp_from_realtime(&n->timestamp, timespec_load(&ts)); + else + triple_timestamp_get(&n->timestamp); + + (void) lldp_rx_handle_datagram(lldp_rx, n); + return 0; +} + +static void lldp_rx_reset(sd_lldp_rx *lldp_rx) { + assert(lldp_rx); + + (void) event_source_disable(lldp_rx->timer_event_source); + lldp_rx->io_event_source = sd_event_source_disable_unref(lldp_rx->io_event_source); + lldp_rx->fd = safe_close(lldp_rx->fd); +} + +int sd_lldp_rx_is_running(sd_lldp_rx *lldp_rx) { + if (!lldp_rx) + return false; + + return lldp_rx->fd >= 0; +} + +int sd_lldp_rx_start(sd_lldp_rx *lldp_rx) { + int r; + + assert_return(lldp_rx, -EINVAL); + assert_return(lldp_rx->event, -EINVAL); + assert_return(lldp_rx->ifindex > 0, -EINVAL); + + if (sd_lldp_rx_is_running(lldp_rx)) + return 0; + + assert(!lldp_rx->io_event_source); + + lldp_rx->fd = lldp_network_bind_raw_socket(lldp_rx->ifindex); + if (lldp_rx->fd < 0) + return lldp_rx->fd; + + r = sd_event_add_io(lldp_rx->event, &lldp_rx->io_event_source, lldp_rx->fd, EPOLLIN, lldp_rx_receive_datagram, lldp_rx); + if (r < 0) + goto fail; + + r = sd_event_source_set_priority(lldp_rx->io_event_source, lldp_rx->event_priority); + if (r < 0) + goto fail; + + (void) sd_event_source_set_description(lldp_rx->io_event_source, "lldp-rx-io"); + + log_lldp_rx(lldp_rx, "Started LLDP client"); + return 1; + +fail: + lldp_rx_reset(lldp_rx); + return r; +} + +int sd_lldp_rx_stop(sd_lldp_rx *lldp_rx) { + if (!sd_lldp_rx_is_running(lldp_rx)) + return 0; + + log_lldp_rx(lldp_rx, "Stopping LLDP client"); + + lldp_rx_reset(lldp_rx); + lldp_rx_flush_neighbors(lldp_rx); + + return 1; +} + +int sd_lldp_rx_attach_event(sd_lldp_rx *lldp_rx, sd_event *event, int64_t priority) { + int r; + + assert_return(lldp_rx, -EINVAL); + assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY); + assert_return(!lldp_rx->event, -EBUSY); + + if (event) + lldp_rx->event = sd_event_ref(event); + else { + r = sd_event_default(&lldp_rx->event); + if (r < 0) + return r; + } + + lldp_rx->event_priority = priority; + + return 0; +} + +int sd_lldp_rx_detach_event(sd_lldp_rx *lldp_rx) { + assert_return(lldp_rx, -EINVAL); + assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY); + + lldp_rx->io_event_source = sd_event_source_disable_unref(lldp_rx->io_event_source); + lldp_rx->timer_event_source = sd_event_source_disable_unref(lldp_rx->timer_event_source); + lldp_rx->event = sd_event_unref(lldp_rx->event); + return 0; +} + +sd_event* sd_lldp_rx_get_event(sd_lldp_rx *lldp_rx) { + assert_return(lldp_rx, NULL); + + return lldp_rx->event; +} + +int sd_lldp_rx_set_callback(sd_lldp_rx *lldp_rx, sd_lldp_rx_callback_t cb, void *userdata) { + assert_return(lldp_rx, -EINVAL); + + lldp_rx->callback = cb; + lldp_rx->userdata = userdata; + + return 0; +} + +int sd_lldp_rx_set_ifindex(sd_lldp_rx *lldp_rx, int ifindex) { + assert_return(lldp_rx, -EINVAL); + assert_return(ifindex > 0, -EINVAL); + assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY); + + lldp_rx->ifindex = ifindex; + return 0; +} + +int sd_lldp_rx_set_ifname(sd_lldp_rx *lldp_rx, const char *ifname) { + assert_return(lldp_rx, -EINVAL); + assert_return(ifname, -EINVAL); + + if (!ifname_valid_full(ifname, IFNAME_VALID_ALTERNATIVE)) + return -EINVAL; + + return free_and_strdup(&lldp_rx->ifname, ifname); +} + +int sd_lldp_rx_get_ifname(sd_lldp_rx *lldp_rx, const char **ret) { + int r; + + assert_return(lldp_rx, -EINVAL); + + r = get_ifname(lldp_rx->ifindex, &lldp_rx->ifname); + if (r < 0) + return r; + + if (ret) + *ret = lldp_rx->ifname; + + return 0; +} + +static sd_lldp_rx *lldp_rx_free(sd_lldp_rx *lldp_rx) { + if (!lldp_rx) + return NULL; + + lldp_rx_reset(lldp_rx); + + sd_lldp_rx_detach_event(lldp_rx); + + lldp_rx_flush_neighbors(lldp_rx); + + hashmap_free(lldp_rx->neighbor_by_id); + prioq_free(lldp_rx->neighbor_by_expiry); + free(lldp_rx->ifname); + return mfree(lldp_rx); +} + +DEFINE_PUBLIC_TRIVIAL_REF_UNREF_FUNC(sd_lldp_rx, sd_lldp_rx, lldp_rx_free); + +int sd_lldp_rx_new(sd_lldp_rx **ret) { + _cleanup_(sd_lldp_rx_unrefp) sd_lldp_rx *lldp_rx = NULL; + + assert_return(ret, -EINVAL); + + lldp_rx = new(sd_lldp_rx, 1); + if (!lldp_rx) + return -ENOMEM; + + *lldp_rx = (sd_lldp_rx) { + .n_ref = 1, + .fd = -1, + .neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX, + .capability_mask = UINT16_MAX, + }; + + *ret = TAKE_PTR(lldp_rx); + return 0; +} + +static int on_timer_event(sd_event_source *s, uint64_t usec, void *userdata) { + sd_lldp_rx *lldp_rx = userdata; + int r; + + r = lldp_rx_make_space(lldp_rx, 0); + if (r < 0) { + log_lldp_rx_errno(lldp_rx, r, "Failed to make space, ignoring: %m"); + return 0; + } + + r = lldp_rx_start_timer(lldp_rx, NULL); + if (r < 0) { + log_lldp_rx_errno(lldp_rx, r, "Failed to restart timer, ignoring: %m"); + return 0; + } + + return 0; +} + +static int lldp_rx_start_timer(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *neighbor) { + sd_lldp_neighbor *n; + + assert(lldp_rx); + assert(lldp_rx->event); + + if (neighbor) + lldp_neighbor_start_ttl(neighbor); + + n = prioq_peek(lldp_rx->neighbor_by_expiry); + if (!n) + return event_source_disable(lldp_rx->timer_event_source); + + return event_reset_time(lldp_rx->event, &lldp_rx->timer_event_source, + CLOCK_BOOTTIME, + n->until, 0, + on_timer_event, lldp_rx, + lldp_rx->event_priority, "lldp-rx-timer", true); +} + +static inline int neighbor_compare_func(sd_lldp_neighbor * const *a, sd_lldp_neighbor * const *b) { + assert(a); + assert(b); + assert(*a); + assert(*b); + + return lldp_neighbor_id_compare_func(&(*a)->id, &(*b)->id); +} + +int sd_lldp_rx_get_neighbors(sd_lldp_rx *lldp_rx, sd_lldp_neighbor ***ret) { + _cleanup_free_ sd_lldp_neighbor **l = NULL; + sd_lldp_neighbor *n; + int k = 0; + + assert_return(lldp_rx, -EINVAL); + assert_return(ret, -EINVAL); + + if (hashmap_isempty(lldp_rx->neighbor_by_id)) { /* Special shortcut */ + *ret = NULL; + return 0; + } + + l = new0(sd_lldp_neighbor*, hashmap_size(lldp_rx->neighbor_by_id)); + if (!l) + return -ENOMEM; + + HASHMAP_FOREACH(n, lldp_rx->neighbor_by_id) + l[k++] = sd_lldp_neighbor_ref(n); + + assert((size_t) k == hashmap_size(lldp_rx->neighbor_by_id)); + + /* Return things in a stable order */ + typesafe_qsort(l, k, neighbor_compare_func); + *ret = TAKE_PTR(l); + + return k; +} + +int sd_lldp_rx_set_neighbors_max(sd_lldp_rx *lldp_rx, uint64_t m) { + assert_return(lldp_rx, -EINVAL); + assert_return(m > 0, -EINVAL); + + lldp_rx->neighbors_max = m; + lldp_rx_make_space(lldp_rx, 0); + + return 0; +} + +int sd_lldp_rx_match_capabilities(sd_lldp_rx *lldp_rx, uint16_t mask) { + assert_return(lldp_rx, -EINVAL); + assert_return(mask != 0, -EINVAL); + + lldp_rx->capability_mask = mask; + + return 0; +} + +int sd_lldp_rx_set_filter_address(sd_lldp_rx *lldp_rx, const struct ether_addr *addr) { + assert_return(lldp_rx, -EINVAL); + + /* In order to deal nicely with bridges that send back our own packets, allow one address to be filtered, so + * that our own can be filtered out here. */ + + if (addr) + lldp_rx->filter_address = *addr; + else + zero(lldp_rx->filter_address); + + return 0; +} -- cgit v1.2.3