src/libsystemd-network/sd-lldp.c

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