src/libsystemd-network/sd-lldp.c

   1 /***
   2   This file is part of systemd.
   3
   4   Copyright (C) 2014 Tom Gundersen
   5   Copyright (C) 2014 Susant Sahani
   6
   7   systemd is free software; you can redistribute it and/or modify it
   8   under the terms of the GNU Lesser General Public License as published by
   9   the Free Software Foundation; either version 2.1 of the License, or
  10   (at your option) any later version.
  11
  12   systemd is distributed in the hope that it will be useful, but
  13   WITHOUT ANY WARRANTY; without even the implied warranty of
  14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  15   Lesser General Public License for more details.
  16
  17   You should have received a copy of the GNU Lesser General Public License
  18   along with systemd; If not, see <http://www.gnu.org/licenses/>.
  19 ***/
  20
  21 #include <arpa/inet.h>
  22
  23 #include "sd-lldp.h"
  24
  25 #include "alloc-util.h"
  26 #include "fd-util.h"
  27 #include "lldp-internal.h"
  28 #include "lldp-neighbor.h"
  29 #include "lldp-network.h"
  30 #include "socket-util.h"
  31 #include "ether-addr-util.h"
  32
  33 #define LLDP_DEFAULT_NEIGHBORS_MAX 128U
  34
  35 static void lldp_flush_neighbors(sd_lldp *lldp) {
  36         sd_lldp_neighbor *n;
  37
  38         assert(lldp);
  39
  40         while ((n = hashmap_first(lldp->neighbor_by_id)))
  41                 lldp_neighbor_unlink(n);
  42 }
  43
  44 static void lldp_callback(sd_lldp *lldp, sd_lldp_event event, sd_lldp_neighbor *n) {
  45         assert(lldp);
  46         assert(n);
  47
  48         log_lldp("Invoking callback for '%c'.", event);
  49
  50         if (!lldp->callback)
  51                 return;
  52
  53         lldp->callback(lldp, event, n, lldp->userdata);
  54 }
  55
  56 static int lldp_make_space(sd_lldp *lldp, size_t extra) {
  57         usec_t t = USEC_INFINITY;
  58         bool changed = false;
  59
  60         assert(lldp);
  61
  62         /* Remove all entries that are past their TTL, and more until at least the specified number of extra entries
  63          * are free. */
  64
  65         for (;;) {
  66                 _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
  67
  68                 n = prioq_peek(lldp->neighbor_by_expiry);
  69                 if (!n)
  70                         break;
  71
  72                 sd_lldp_neighbor_ref(n);
  73
  74                 if (hashmap_size(lldp->neighbor_by_id) > LESS_BY(lldp->neighbors_max, extra))
  75                         goto remove_one;
  76
  77                 if (t == USEC_INFINITY)
  78                         t = now(clock_boottime_or_monotonic());
  79
  80                 if (n->until > t)
  81                         break;
  82
  83         remove_one:
  84                 lldp_neighbor_unlink(n);
  85                 lldp_callback(lldp, SD_LLDP_EVENT_REMOVED, n);
  86                 changed = true;
  87         }
  88
  89         return changed;
  90 }
  91
  92 static bool lldp_keep_neighbor(sd_lldp *lldp, sd_lldp_neighbor *n) {
  93         assert(lldp);
  94         assert(n);
  95
  96         /* Don't keep data with a zero TTL */
  97         if (n->ttl <= 0)
  98                 return false;
  99
 100         /* Filter out data from the filter address */
 101         if (!ether_addr_is_null(&lldp->filter_address) &&
 102             ether_addr_equal(&lldp->filter_address, &n->source_address))
 103                 return false;
 104
 105         /* Only add if the neighbor has a capability we are interested in. Note that we also store all neighbors with
 106          * no caps field set. */
 107         if (n->has_capabilities &&
 108             (n->enabled_capabilities & lldp->capability_mask) == 0)
 109                 return false;
 110
 111         /* Keep everything else */
 112         return true;
 113 }
 114
 115 static int lldp_start_timer(sd_lldp *lldp, sd_lldp_neighbor *neighbor);
 116
 117 static int lldp_add_neighbor(sd_lldp *lldp, sd_lldp_neighbor *n) {
 118         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *old = NULL;
 119         bool keep;
 120         int r;
 121
 122         assert(lldp);
 123         assert(n);
 124         assert(!n->lldp);
 125
 126         keep = lldp_keep_neighbor(lldp, n);
 127
 128         /* First retrieve the old entry for this MSAP */
 129         old = hashmap_get(lldp->neighbor_by_id, &n->id);
 130         if (old) {
 131                 sd_lldp_neighbor_ref(old);
 132
 133                 if (!keep) {
 134                         lldp_neighbor_unlink(old);
 135                         lldp_callback(lldp, SD_LLDP_EVENT_REMOVED, old);
 136                         return 0;
 137                 }
 138
 139                 if (lldp_neighbor_equal(n, old)) {
 140                         /* Is this equal, then restart the TTL counter, but don't do anyting else. */
 141                         lldp_start_timer(lldp, old);
 142                         lldp_callback(lldp, SD_LLDP_EVENT_REFRESHED, old);
 143                         return 0;
 144                 }
 145
 146                 /* Data changed, remove the old entry, and add a new one */
 147                 lldp_neighbor_unlink(old);
 148
 149         } else if (!keep)
 150                 return 0;
 151
 152         /* Then, make room for at least one new neighbor */
 153         lldp_make_space(lldp, 1);
 154
 155         r = hashmap_put(lldp->neighbor_by_id, &n->id, n);
 156         if (r < 0)
 157                 goto finish;
 158
 159         r = prioq_put(lldp->neighbor_by_expiry, n, &n->prioq_idx);
 160         if (r < 0) {
 161                 assert_se(hashmap_remove(lldp->neighbor_by_id, &n->id) == n);
 162                 goto finish;
 163         }
 164
 165         n->lldp = lldp;
 166
 167         lldp_start_timer(lldp, n);
 168         lldp_callback(lldp, old ? SD_LLDP_EVENT_UPDATED : SD_LLDP_EVENT_ADDED, n);
 169
 170         return 1;
 171
 172 finish:
 173         if (old)
 174                 lldp_callback(lldp, SD_LLDP_EVENT_REMOVED, n);
 175
 176         return r;
 177 }
 178
 179 static int lldp_handle_datagram(sd_lldp *lldp, sd_lldp_neighbor *n) {
 180         int r;
 181
 182         assert(lldp);
 183         assert(n);
 184
 185         r = lldp_neighbor_parse(n);
 186         if (r == -EBADMSG) /* Ignore bad messages */
 187                 return 0;
 188         if (r < 0)
 189                 return r;
 190
 191         r = lldp_add_neighbor(lldp, n);
 192         if (r < 0) {
 193                 log_lldp_errno(r, "Failed to add datagram. Ignoring.");
 194                 return 0;
 195         }
 196
 197         log_lldp("Successfully processed LLDP datagram.");
 198         return 0;
 199 }
 200
 201 static int lldp_receive_datagram(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
 202         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
 203         ssize_t space, length;
 204         sd_lldp *lldp = userdata;
 205
 206         assert(fd >= 0);
 207         assert(lldp);
 208
 209         space = next_datagram_size_fd(fd);
 210         if (space < 0)
 211                 return log_lldp_errno(space, "Failed to determine datagram size to read: %m");
 212
 213         n = lldp_neighbor_new(space);
 214         if (!n)
 215                 return -ENOMEM;
 216
 217         length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT);
 218         if (length < 0)
 219                 return log_lldp_errno(errno, "Failed to read LLDP datagram: %m");
 220
 221         if ((size_t) length != n->raw_size) {
 222                 log_lldp("Packet size mismatch.");
 223                 return -EINVAL;
 224         }
 225
 226         return lldp_handle_datagram(lldp, n);
 227 }
 228
 229 _public_ int sd_lldp_start(sd_lldp *lldp) {
 230         int r;
 231
 232         assert_return(lldp, -EINVAL);
 233
 234         if (lldp->fd >= 0)
 235                 return 0;
 236
 237         assert(!lldp->io_event_source);
 238
 239         lldp->fd = lldp_network_bind_raw_socket(lldp->ifindex);
 240         if (lldp->fd < 0)
 241                 return lldp->fd;
 242
 243         if (lldp->event) {
 244                 r = sd_event_add_io(lldp->event, &lldp->io_event_source, lldp->fd, EPOLLIN, lldp_receive_datagram, lldp);
 245                 if (r < 0)
 246                         goto fail;
 247
 248                 r = sd_event_source_set_priority(lldp->io_event_source, lldp->event_priority);
 249                 if (r < 0)
 250                         goto fail;
 251
 252                 (void) sd_event_source_set_description(lldp->io_event_source, "lldp-io");
 253         }
 254
 255         return 1;
 256
 257 fail:
 258         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 259         lldp->fd = safe_close(lldp->fd);
 260
 261         return r;
 262 }
 263
 264 _public_ int sd_lldp_stop(sd_lldp *lldp) {
 265         assert_return(lldp, -EINVAL);
 266
 267         if (lldp->fd < 0)
 268                 return 0;
 269
 270         lldp->timer_event_source = sd_event_source_unref(lldp->timer_event_source);
 271         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 272         lldp->fd = safe_close(lldp->fd);
 273
 274         lldp_flush_neighbors(lldp);
 275
 276         return 1;
 277 }
 278
 279 _public_ int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int64_t priority) {
 280         int r;
 281
 282         assert_return(lldp, -EINVAL);
 283         assert_return(lldp->fd < 0, -EBUSY);
 284         assert_return(!lldp->event, -EBUSY);
 285
 286         if (event)
 287                 lldp->event = sd_event_ref(event);
 288         else {
 289                 r = sd_event_default(&lldp->event);
 290                 if (r < 0)
 291                         return r;
 292         }
 293
 294         lldp->event_priority = priority;
 295
 296         return 0;
 297 }
 298
 299 _public_ int sd_lldp_detach_event(sd_lldp *lldp) {
 300
 301         assert_return(lldp, -EINVAL);
 302         assert_return(lldp->fd < 0, -EBUSY);
 303
 304         lldp->event = sd_event_unref(lldp->event);
 305         return 0;
 306 }
 307
 308 _public_ int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_callback_t cb, void *userdata) {
 309         assert_return(lldp, -EINVAL);
 310
 311         lldp->callback = cb;
 312         lldp->userdata = userdata;
 313
 314         return 0;
 315 }
 316
 317 _public_ sd_lldp* sd_lldp_unref(sd_lldp *lldp) {
 318
 319         if (!lldp)
 320                 return NULL;
 321
 322         lldp_flush_neighbors(lldp);
 323
 324         hashmap_free(lldp->neighbor_by_id);
 325         prioq_free(lldp->neighbor_by_expiry);
 326
 327         sd_event_source_unref(lldp->io_event_source);
 328         sd_event_source_unref(lldp->timer_event_source);
 329         sd_event_unref(lldp->event);
 330         safe_close(lldp->fd);
 331
 332         free(lldp);
 333
 334         return NULL;
 335 }
 336
 337 _public_ int sd_lldp_new(sd_lldp **ret, int ifindex) {
 338         _cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
 339         int r;
 340
 341         assert_return(ret, -EINVAL);
 342         assert_return(ifindex > 0, -EINVAL);
 343
 344         lldp = new0(sd_lldp, 1);
 345         if (!lldp)
 346                 return -ENOMEM;
 347
 348         lldp->fd = -1;
 349         lldp->ifindex = ifindex;
 350         lldp->neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX;
 351         lldp->capability_mask = (uint16_t) -1;
 352
 353         lldp->neighbor_by_id = hashmap_new(&lldp_neighbor_id_hash_ops);
 354         if (!lldp->neighbor_by_id)
 355                 return -ENOMEM;
 356
 357         r = prioq_ensure_allocated(&lldp->neighbor_by_expiry, lldp_neighbor_prioq_compare_func);
 358         if (r < 0)
 359                 return r;
 360
 361         *ret = lldp;
 362         lldp = NULL;
 363
 364         return 0;
 365 }
 366
 367 static int neighbor_compare_func(const void *a, const void *b) {
 368         const sd_lldp_neighbor * const*x = a, * const *y = b;
 369
 370         return lldp_neighbor_id_hash_ops.compare(&(*x)->id, &(*y)->id);
 371 }
 372
 373 static int on_timer_event(sd_event_source *s, uint64_t usec, void *userdata) {
 374         sd_lldp *lldp = userdata;
 375         int r, q;
 376
 377         r = lldp_make_space(lldp, 0);
 378         if (r < 0)
 379                 return log_lldp_errno(r, "Failed to make space: %m");
 380
 381         q = lldp_start_timer(lldp, NULL);
 382         if (q < 0)
 383                 return log_lldp_errno(q, "Failed to restart timer: %m");
 384
 385         return 0;
 386 }
 387
 388 static int lldp_start_timer(sd_lldp *lldp, sd_lldp_neighbor *neighbor) {
 389         sd_lldp_neighbor *n;
 390         int r;
 391
 392         assert(lldp);
 393
 394         if (neighbor)
 395                 lldp_neighbor_start_ttl(neighbor);
 396
 397         n = prioq_peek(lldp->neighbor_by_expiry);
 398         if (!n) {
 399
 400                 if (lldp->timer_event_source)
 401                         return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_OFF);
 402
 403                 return 0;
 404         }
 405
 406         if (lldp->timer_event_source) {
 407                 r = sd_event_source_set_time(lldp->timer_event_source, n->until);
 408                 if (r < 0)
 409                         return r;
 410
 411                 return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_ONESHOT);
 412         }
 413
 414         if (!lldp->event)
 415                 return 0;
 416
 417         r = sd_event_add_time(lldp->event, &lldp->timer_event_source, clock_boottime_or_monotonic(), n->until, 0, on_timer_event, lldp);
 418         if (r < 0)
 419                 return r;
 420
 421         r = sd_event_source_set_priority(lldp->timer_event_source, lldp->event_priority);
 422         if (r < 0)
 423                 return r;
 424
 425         (void) sd_event_source_set_description(lldp->timer_event_source, "lldp-timer");
 426         return 0;
 427 }
 428
 429 _public_ int sd_lldp_get_neighbors(sd_lldp *lldp, sd_lldp_neighbor ***ret) {
 430         sd_lldp_neighbor **l = NULL, *n;
 431         Iterator i;
 432         int k = 0, r;
 433
 434         assert_return(lldp, -EINVAL);
 435         assert_return(ret, -EINVAL);
 436
 437         if (hashmap_isempty(lldp->neighbor_by_id)) { /* Special shortcut */
 438                 *ret = NULL;
 439                 return 0;
 440         }
 441
 442         l = new0(sd_lldp_neighbor*, hashmap_size(lldp->neighbor_by_id));
 443         if (!l)
 444                 return -ENOMEM;
 445
 446         r = lldp_start_timer(lldp, NULL);
 447         if (r < 0) {
 448                 free(l);
 449                 return r;
 450         }
 451
 452         HASHMAP_FOREACH(n, lldp->neighbor_by_id, i)
 453                 l[k++] = sd_lldp_neighbor_ref(n);
 454
 455         assert((size_t) k == hashmap_size(lldp->neighbor_by_id));
 456
 457         /* Return things in a stable order */
 458         qsort(l, k, sizeof(sd_lldp_neighbor*), neighbor_compare_func);
 459         *ret = l;
 460
 461         return k;
 462 }
 463
 464 _public_ int sd_lldp_set_neighbors_max(sd_lldp *lldp, uint64_t m) {
 465         assert_return(lldp, -EINVAL);
 466         assert_return(m <= 0, -EINVAL);
 467
 468         lldp->neighbors_max = m;
 469         lldp_make_space(lldp, 0);
 470
 471         return 0;
 472 }
 473
 474 _public_ int sd_lldp_match_capabilities(sd_lldp *lldp, uint16_t mask) {
 475         assert_return(lldp, -EINVAL);
 476         assert_return(mask != 0, -EINVAL);
 477
 478         lldp->capability_mask = mask;
 479
 480         return 0;
 481 }
 482
 483 _public_ int sd_lldp_set_filter_address(sd_lldp *lldp, const struct ether_addr *addr) {
 484         assert_return(lldp, -EINVAL);
 485
 486         /* In order to deal nicely with bridges that send back our own packets, allow one address to be filtered, so
 487          * that our own can be filtered out here. */
 488
 489         if (!addr) {
 490                 zero(lldp->filter_address);
 491                 return 0;
 492         }
 493
 494         lldp->filter_address = *addr;
 495         return 0;
 496 }