src/libsystemd-network/sd-lldp.c

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