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_add_neighbor(sd_lldp *lldp, sd_lldp_neighbor *n) {
 116         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *old = NULL;
 117         bool keep;
 118         int r;
 119
 120         assert(lldp);
 121         assert(n);
 122         assert(!n->lldp);
 123
 124         keep = lldp_keep_neighbor(lldp, n);
 125
 126         /* First retrieve the old entry for this MSAP */
 127         old = hashmap_get(lldp->neighbor_by_id, &n->id);
 128         if (old) {
 129                 sd_lldp_neighbor_ref(old);
 130
 131                 if (!keep) {
 132                         lldp_neighbor_unlink(old);
 133                         lldp_callback(lldp, SD_LLDP_EVENT_REMOVED, old);
 134                         return 0;
 135                 }
 136
 137                 if (lldp_neighbor_equal(n, old)) {
 138                         /* Is this equal, then restart the TTL counter, but don't do anyting else. */
 139                         lldp_neighbor_start_ttl(old);
 140                         lldp_callback(lldp, SD_LLDP_EVENT_REFRESHED, old);
 141                         return 0;
 142                 }
 143
 144                 /* Data changed, remove the old entry, and add a new one */
 145                 lldp_neighbor_unlink(old);
 146
 147         } else if (!keep)
 148                 return 0;
 149
 150         /* Then, make room for at least one new neighbor */
 151         lldp_make_space(lldp, 1);
 152
 153         r = hashmap_put(lldp->neighbor_by_id, &n->id, n);
 154         if (r < 0)
 155                 goto finish;
 156
 157         r = prioq_put(lldp->neighbor_by_expiry, n, &n->prioq_idx);
 158         if (r < 0) {
 159                 assert_se(hashmap_remove(lldp->neighbor_by_id, &n->id) == n);
 160                 goto finish;
 161         }
 162
 163         n->lldp = lldp;
 164
 165         lldp_neighbor_start_ttl(n);
 166         lldp_callback(lldp, old ? SD_LLDP_EVENT_UPDATED : SD_LLDP_EVENT_ADDED, n);
 167
 168         return 1;
 169
 170 finish:
 171         if (old)
 172                 lldp_callback(lldp, SD_LLDP_EVENT_REMOVED, n);
 173
 174         return r;
 175 }
 176
 177 static int lldp_handle_datagram(sd_lldp *lldp, sd_lldp_neighbor *n) {
 178         int r;
 179
 180         assert(lldp);
 181         assert(n);
 182
 183         r = lldp_neighbor_parse(n);
 184         if (r == -EBADMSG) /* Ignore bad messages */
 185                 return 0;
 186         if (r < 0)
 187                 return r;
 188
 189         r = lldp_add_neighbor(lldp, n);
 190         if (r < 0) {
 191                 log_lldp_errno(r, "Failed to add datagram. Ignoring.");
 192                 return 0;
 193         }
 194
 195         log_lldp("Successfully processed LLDP datagram.");
 196         return 0;
 197 }
 198
 199 static int lldp_receive_datagram(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
 200         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
 201         ssize_t space, length;
 202         sd_lldp *lldp = userdata;
 203
 204         assert(fd >= 0);
 205         assert(lldp);
 206
 207         space = next_datagram_size_fd(fd);
 208         if (space < 0)
 209                 return log_lldp_errno(space, "Failed to determine datagram size to read: %m");
 210
 211         n = lldp_neighbor_new(space);
 212         if (!n)
 213                 return -ENOMEM;
 214
 215         length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT);
 216         if (length < 0)
 217                 return log_lldp_errno(errno, "Failed to read LLDP datagram: %m");
 218
 219         if ((size_t) length != n->raw_size) {
 220                 log_lldp("Packet size mismatch.");
 221                 return -EINVAL;
 222         }
 223
 224         return lldp_handle_datagram(lldp, n);
 225 }
 226
 227 _public_ int sd_lldp_start(sd_lldp *lldp) {
 228         int r;
 229
 230         assert_return(lldp, -EINVAL);
 231
 232         if (lldp->fd >= 0)
 233                 return 0;
 234
 235         assert(!lldp->io_event_source);
 236
 237         lldp->fd = lldp_network_bind_raw_socket(lldp->ifindex);
 238         if (lldp->fd < 0)
 239                 return lldp->fd;
 240
 241         if (lldp->event) {
 242                 r = sd_event_add_io(lldp->event, &lldp->io_event_source, lldp->fd, EPOLLIN, lldp_receive_datagram, lldp);
 243                 if (r < 0)
 244                         goto fail;
 245
 246                 r = sd_event_source_set_priority(lldp->io_event_source, lldp->event_priority);
 247                 if (r < 0)
 248                         goto fail;
 249
 250                 (void) sd_event_source_set_description(lldp->io_event_source, "lldp-io");
 251         }
 252
 253         return 1;
 254
 255 fail:
 256         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 257         lldp->fd = safe_close(lldp->fd);
 258
 259         return r;
 260 }
 261
 262 _public_ int sd_lldp_stop(sd_lldp *lldp) {
 263         assert_return(lldp, -EINVAL);
 264
 265         if (lldp->fd < 0)
 266                 return 0;
 267
 268         lldp->timer_event_source = sd_event_source_unref(lldp->timer_event_source);
 269         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 270         lldp->fd = safe_close(lldp->fd);
 271
 272         lldp_flush_neighbors(lldp);
 273
 274         return 1;
 275 }
 276
 277 _public_ int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int64_t priority) {
 278         int r;
 279
 280         assert_return(lldp, -EINVAL);
 281         assert_return(lldp->fd < 0, -EBUSY);
 282         assert_return(!lldp->event, -EBUSY);
 283
 284         if (event)
 285                 lldp->event = sd_event_ref(event);
 286         else {
 287                 r = sd_event_default(&lldp->event);
 288                 if (r < 0)
 289                         return r;
 290         }
 291
 292         lldp->event_priority = priority;
 293
 294         return 0;
 295 }
 296
 297 _public_ int sd_lldp_detach_event(sd_lldp *lldp) {
 298
 299         assert_return(lldp, -EINVAL);
 300         assert_return(lldp->fd < 0, -EBUSY);
 301
 302         lldp->event = sd_event_unref(lldp->event);
 303         return 0;
 304 }
 305
 306 _public_ int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_callback_t cb, void *userdata) {
 307         assert_return(lldp, -EINVAL);
 308
 309         lldp->callback = cb;
 310         lldp->userdata = userdata;
 311
 312         return 0;
 313 }
 314
 315 _public_ sd_lldp* sd_lldp_unref(sd_lldp *lldp) {
 316
 317         if (!lldp)
 318                 return NULL;
 319
 320         lldp_flush_neighbors(lldp);
 321
 322         hashmap_free(lldp->neighbor_by_id);
 323         prioq_free(lldp->neighbor_by_expiry);
 324
 325         sd_event_source_unref(lldp->io_event_source);
 326         sd_event_source_unref(lldp->timer_event_source);
 327         sd_event_unref(lldp->event);
 328         safe_close(lldp->fd);
 329
 330         free(lldp);
 331
 332         return NULL;
 333 }
 334
 335 _public_ int sd_lldp_new(sd_lldp **ret, int ifindex) {
 336         _cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
 337         int r;
 338
 339         assert_return(ret, -EINVAL);
 340         assert_return(ifindex > 0, -EINVAL);
 341
 342         lldp = new0(sd_lldp, 1);
 343         if (!lldp)
 344                 return -ENOMEM;
 345
 346         lldp->fd = -1;
 347         lldp->ifindex = ifindex;
 348         lldp->neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX;
 349         lldp->capability_mask = (uint16_t) -1;
 350
 351         lldp->neighbor_by_id = hashmap_new(&lldp_neighbor_id_hash_ops);
 352         if (!lldp->neighbor_by_id)
 353                 return -ENOMEM;
 354
 355         r = prioq_ensure_allocated(&lldp->neighbor_by_expiry, lldp_neighbor_prioq_compare_func);
 356         if (r < 0)
 357                 return r;
 358
 359         *ret = lldp;
 360         lldp = NULL;
 361
 362         return 0;
 363 }
 364
 365 static int neighbor_compare_func(const void *a, const void *b) {
 366         const sd_lldp_neighbor * const*x = a, * const *y = b;
 367
 368         return lldp_neighbor_id_hash_ops.compare(&(*x)->id, &(*y)->id);
 369 }
 370
 371 static int lldp_start_timer(sd_lldp *lldp);
 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);
 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) {
 389         sd_lldp_neighbor *n;
 390         int r;
 391
 392         assert(lldp);
 393
 394         n = prioq_peek(lldp->neighbor_by_expiry);
 395         if (!n) {
 396
 397                 if (lldp->timer_event_source)
 398                         return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_OFF);
 399
 400                 return 0;
 401         }
 402
 403         if (lldp->timer_event_source) {
 404                 r = sd_event_source_set_time(lldp->timer_event_source, n->until);
 405                 if (r < 0)
 406                         return r;
 407
 408                 return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_ONESHOT);
 409         }
 410
 411         if (!lldp->event)
 412                 return 0;
 413
 414         r = sd_event_add_time(lldp->event, &lldp->timer_event_source, clock_boottime_or_monotonic(), n->until, 0, on_timer_event, lldp);
 415         if (r < 0)
 416                 return r;
 417
 418         r = sd_event_source_set_priority(lldp->timer_event_source, lldp->event_priority);
 419         if (r < 0)
 420                 return r;
 421
 422         (void) sd_event_source_set_description(lldp->timer_event_source, "lldp-timer");
 423         return 0;
 424 }
 425
 426 _public_ int sd_lldp_get_neighbors(sd_lldp *lldp, sd_lldp_neighbor ***ret) {
 427         sd_lldp_neighbor **l = NULL, *n;
 428         Iterator i;
 429         int k = 0, r;
 430
 431         assert_return(lldp, -EINVAL);
 432         assert_return(ret, -EINVAL);
 433
 434         if (hashmap_isempty(lldp->neighbor_by_id)) { /* Special shortcut */
 435                 *ret = NULL;
 436                 return 0;
 437         }
 438
 439         l = new0(sd_lldp_neighbor*, hashmap_size(lldp->neighbor_by_id));
 440         if (!l)
 441                 return -ENOMEM;
 442
 443         r = lldp_start_timer(lldp);
 444         if (r < 0) {
 445                 free(l);
 446                 return r;
 447         }
 448
 449         HASHMAP_FOREACH(n, lldp->neighbor_by_id, i)
 450                 l[k++] = sd_lldp_neighbor_ref(n);
 451
 452         assert((size_t) k == hashmap_size(lldp->neighbor_by_id));
 453
 454         /* Return things in a stable order */
 455         qsort(l, k, sizeof(sd_lldp_neighbor*), neighbor_compare_func);
 456         *ret = l;
 457
 458         return k;
 459 }
 460
 461 _public_ int sd_lldp_set_neighbors_max(sd_lldp *lldp, uint64_t m) {
 462         assert_return(lldp, -EINVAL);
 463         assert_return(m <= 0, -EINVAL);
 464
 465         lldp->neighbors_max = m;
 466         lldp_make_space(lldp, 0);
 467
 468         return 0;
 469 }
 470
 471 _public_ int sd_lldp_match_capabilities(sd_lldp *lldp, uint16_t mask) {
 472         assert_return(lldp, -EINVAL);
 473         assert_return(mask != 0, -EINVAL);
 474
 475         lldp->capability_mask = mask;
 476
 477         return 0;
 478 }
 479
 480 _public_ int sd_lldp_set_filter_address(sd_lldp *lldp, const struct ether_addr *addr) {
 481         assert_return(lldp, -EINVAL);
 482
 483         /* In order to deal nicely with bridges that send back our own packets, allow one address to be filtered, so
 484          * that our own can be filtered out here. */
 485
 486         if (!addr) {
 487                 zero(lldp->filter_address);
 488                 return 0;
 489         }
 490
 491         lldp->filter_address = *addr;
 492         return 0;
 493 }