src/libsystemd-network/sd-lldp.c

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