path: root/src/libsystemd-network/sd-lldp.c

/***
  This file is part of systemd.

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

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

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

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

#include <arpa/inet.h>

#include "sd-lldp.h"

#include "alloc-util.h"
#include "fd-util.h"
#include "lldp-internal.h"
#include "lldp-neighbor.h"
#include "lldp-network.h"
#include "socket-util.h"
#include "ether-addr-util.h"

#define LLDP_DEFAULT_NEIGHBORS_MAX 128U

static void lldp_flush_neighbors(sd_lldp *lldp) {
        sd_lldp_neighbor *n;

        assert(lldp);

        while ((n = hashmap_first(lldp->neighbor_by_id)))
                lldp_neighbor_unlink(n);
}

static int lldp_make_space(sd_lldp *lldp, size_t extra) {
        sd_lldp_neighbor *n;
        usec_t t = USEC_INFINITY;
        bool changed = false;

        assert(lldp);

        /* Remove all entries that are past their TTL, and more until at least the specified number of extra entries
         * are free. */

        for (;;) {
                n = prioq_peek(lldp->neighbor_by_expiry);
                if (!n)
                        break;

                if (hashmap_size(lldp->neighbor_by_id) > LESS_BY(lldp->neighbors_max, extra))
                        goto remove_one;

                if (t == USEC_INFINITY)
                        t = now(clock_boottime_or_monotonic());

                if (n->until > t)
                        break;

        remove_one:
                lldp_neighbor_unlink(n);
                changed = true;
        }

        return changed;
}

static int lldp_add_neighbor(sd_lldp *lldp, sd_lldp_neighbor *n) {
        sd_lldp_neighbor *old;
        bool changed = false;
        int r;

        assert(lldp);
        assert(n);
        assert(!n->lldp);

        /* First retrieve the old entry for this MSAP */
        old = hashmap_get(lldp->neighbor_by_id, &n->id);
        if (old) {
                if (lldp_neighbor_equal(n, old)) {
                        /* Is this equal, then restart the TTL counter, but don't do anyting else. */
                        lldp_neighbor_start_ttl(old);
                        return 0;
                }

                /* Data changed, remove the old entry, and add a new one */
                lldp_neighbor_unlink(old);
                changed = true;
        }

        /* Then, add the new entry in its place, but only if it has a non-zero TTL. */
        if (n->ttl <= 0)
                return changed;

        /* Filter out the filter address */
        if (!ether_addr_is_null(&lldp->filter_address) &&
            ether_addr_equal(&lldp->filter_address, &n->source_address))
                return changed;

        /* 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->capability_mask) == 0)
                return changed;

        /* Then, make room for at least one new neighbor */
        lldp_make_space(lldp, 1);

        r = hashmap_put(lldp->neighbor_by_id, &n->id, n);
        if (r < 0)
                return r;

        r = prioq_put(lldp->neighbor_by_expiry, n, &n->prioq_idx);
        if (r < 0) {
                assert_se(hashmap_remove(lldp->neighbor_by_id, &n->id) == n);
                return r;
        }

        n->lldp = lldp;

        return true;
}

static int lldp_handle_datagram(sd_lldp *lldp, sd_lldp_neighbor *n) {
        int r;

        assert(lldp);
        assert(n);

        r = lldp_neighbor_parse(n);
        if (r == -EBADMSG) /* Ignore bad messages */
                return 0;
        if (r < 0)
                return r;

        lldp_neighbor_start_ttl(n);

        r = lldp_add_neighbor(lldp, n);
        if (r < 0) {
                log_lldp_errno(r, "Failed to add datagram. Ignoring.");
                return 0;
        }

        log_lldp("Successfully processed LLDP datagram.");

        if (r > 0 && lldp->callback) /* Only invoke the callback if something actually changed. */
                lldp->callback(lldp, lldp->userdata);

        return 0;
}

static int lldp_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 *lldp = userdata;

        assert(fd >= 0);
        assert(lldp);

        space = next_datagram_size_fd(fd);
        if (space < 0)
                return log_lldp_errno(space, "Failed to determine datagram size to read: %m");

        n = lldp_neighbor_new(space);
        if (!n)
                return -ENOMEM;

        length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT);
        if (length < 0)
                return log_lldp_errno(errno, "Failed to read LLDP datagram: %m");

        if ((size_t) length != n->raw_size) {
                log_lldp("Packet size mismatch.");
                return -EINVAL;
        }

        return lldp_handle_datagram(lldp, n);
}

_public_ int sd_lldp_start(sd_lldp *lldp) {
        int r;

        assert_return(lldp, -EINVAL);

        if (lldp->fd >= 0)
                return 0;

        assert(!lldp->io_event_source);

        lldp->fd = lldp_network_bind_raw_socket(lldp->ifindex);
        if (lldp->fd < 0)
                return lldp->fd;

        if (lldp->event) {
                r = sd_event_add_io(lldp->event, &lldp->io_event_source, lldp->fd, EPOLLIN, lldp_receive_datagram, lldp);
                if (r < 0)
                        goto fail;

                r = sd_event_source_set_priority(lldp->io_event_source, lldp->event_priority);
                if (r < 0)
                        goto fail;

                (void) sd_event_source_set_description(lldp->io_event_source, "lldp-io");
        }

        return 1;

fail:
        lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
        lldp->fd = safe_close(lldp->fd);

        return r;
}

_public_ int sd_lldp_stop(sd_lldp *lldp) {
        assert_return(lldp, -EINVAL);

        if (lldp->fd < 0)
                return 0;

        lldp->timer_event_source = sd_event_source_unref(lldp->timer_event_source);
        lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
        lldp->fd = safe_close(lldp->fd);

        lldp_flush_neighbors(lldp);

        return 1;
}

_public_ int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int64_t priority) {
        int r;

        assert_return(lldp, -EINVAL);
        assert_return(lldp->fd < 0, -EBUSY);
        assert_return(!lldp->event, -EBUSY);

        if (event)
                lldp->event = sd_event_ref(event);
        else {
                r = sd_event_default(&lldp->event);
                if (r < 0)
                        return r;
        }

        lldp->event_priority = priority;

        return 0;
}

_public_ int sd_lldp_detach_event(sd_lldp *lldp) {

        assert_return(lldp, -EINVAL);
        assert_return(lldp->fd < 0, -EBUSY);

        lldp->event = sd_event_unref(lldp->event);
        return 0;
}

_public_ int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_callback_t cb, void *userdata) {
        assert_return(lldp, -EINVAL);

        lldp->callback = cb;
        lldp->userdata = userdata;

        return 0;
}

_public_ sd_lldp* sd_lldp_unref(sd_lldp *lldp) {

        if (!lldp)
                return NULL;

        lldp_flush_neighbors(lldp);

        hashmap_free(lldp->neighbor_by_id);
        prioq_free(lldp->neighbor_by_expiry);

        sd_event_source_unref(lldp->io_event_source);
        sd_event_source_unref(lldp->timer_event_source);
        sd_event_unref(lldp->event);
        safe_close(lldp->fd);

        free(lldp);

        return NULL;
}

_public_ int sd_lldp_new(sd_lldp **ret, int ifindex) {
        _cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
        int r;

        assert_return(ret, -EINVAL);
        assert_return(ifindex > 0, -EINVAL);

        lldp = new0(sd_lldp, 1);
        if (!lldp)
                return -ENOMEM;

        lldp->fd = -1;
        lldp->ifindex = ifindex;
        lldp->neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX;
        lldp->capability_mask = (uint16_t) -1;

        lldp->neighbor_by_id = hashmap_new(&lldp_neighbor_id_hash_ops);
        if (!lldp->neighbor_by_id)
                return -ENOMEM;

        r = prioq_ensure_allocated(&lldp->neighbor_by_expiry, lldp_neighbor_prioq_compare_func);
        if (r < 0)
                return r;

        *ret = lldp;
        lldp = NULL;

        return 0;
}

static int neighbor_compare_func(const void *a, const void *b) {
        const sd_lldp_neighbor * const*x = a, * const *y = b;

        return lldp_neighbor_id_hash_ops.compare(&(*x)->id, &(*y)->id);
}

static int lldp_start_timer(sd_lldp *lldp);

static int on_timer_event(sd_event_source *s, uint64_t usec, void *userdata) {
        sd_lldp *lldp = userdata;
        int r, q;

        r = lldp_make_space(lldp, 0);
        if (r < 0)
                return log_lldp_errno(r, "Failed to make space: %m");

        q = lldp_start_timer(lldp);
        if (q < 0)
                return log_lldp_errno(q, "Failed to restart timer: %m");

        log_lldp("LLDP timer event hit.");
        if (r > 0 && lldp->callback) /* Invoke callback if we dropped an entry */
                lldp->callback(lldp, lldp->userdata);

        return 0;
}

static int lldp_start_timer(sd_lldp *lldp) {
        sd_lldp_neighbor *n;
        int r;

        assert(lldp);

        n = prioq_peek(lldp->neighbor_by_expiry);
        if (!n) {

                if (lldp->timer_event_source)
                        return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_OFF);

                return 0;
        }

        if (lldp->timer_event_source) {
                r = sd_event_source_set_time(lldp->timer_event_source, n->until);
                if (r < 0)
                        return r;

                return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_ONESHOT);
        }

        if (!lldp->event)
                return 0;

        r = sd_event_add_time(lldp->event, &lldp->timer_event_source, clock_boottime_or_monotonic(), n->until, 0, on_timer_event, lldp);
        if (r < 0)
                return r;

        r = sd_event_source_set_priority(lldp->timer_event_source, lldp->event_priority);
        if (r < 0)
                return r;

        (void) sd_event_source_set_description(lldp->timer_event_source, "lldp-timer");
        return 0;
}

_public_ int sd_lldp_get_neighbors(sd_lldp *lldp, sd_lldp_neighbor ***ret) {
        sd_lldp_neighbor **l = NULL, *n;
        Iterator i;
        int k = 0, r;

        assert_return(lldp, -EINVAL);
        assert_return(ret, -EINVAL);

        /* Flush out old entries, before we return data */
        (void) lldp_make_space(lldp, 0);

        if (hashmap_isempty(lldp->neighbor_by_id)) { /* Special shortcut */
                *ret = NULL;
                return 0;
        }

        l = new0(sd_lldp_neighbor*, hashmap_size(lldp->neighbor_by_id));
        if (!l)
                return -ENOMEM;

        r = lldp_start_timer(lldp);
        if (r < 0) {
                free(l);
                return r;
        }

        HASHMAP_FOREACH(n, lldp->neighbor_by_id, i)
                l[k++] = sd_lldp_neighbor_ref(n);

        assert((size_t) k == hashmap_size(lldp->neighbor_by_id));

        /* Return things in a stable order */
        qsort(l, k, sizeof(sd_lldp_neighbor*), neighbor_compare_func);
        *ret = l;

        return k;
}

_public_ int sd_lldp_set_neighbors_max(sd_lldp *lldp, uint64_t m) {
        assert_return(lldp, -EINVAL);
        assert_return(m <= 0, -EINVAL);

        lldp->neighbors_max = m;
        lldp_make_space(lldp, 0);

        return 0;
}

_public_ int sd_lldp_match_capabilities(sd_lldp *lldp, uint16_t mask) {
        assert_return(lldp, -EINVAL);
        assert_return(mask != 0, -EINVAL);

        lldp->capability_mask = mask;

        return 0;
}

_public_ int sd_lldp_set_filter_address(sd_lldp *lldp, const struct ether_addr *addr) {
        assert_return(lldp, -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) {
                zero(lldp->filter_address);
                return 0;
        }

        lldp->filter_address = *addr;
        return 0;
}