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
  32 #define LLDP_DEFAULT_NEIGHBORS_MAX 128U
  33
  34 static void lldp_flush_neighbors(sd_lldp *lldp) {
  35         sd_lldp_neighbor *n;
  36
  37         assert(lldp);
  38
  39         while ((n = hashmap_first(lldp->neighbor_by_id)))
  40                 lldp_neighbor_unlink(n);
  41 }
  42
  43 static int lldp_make_space(sd_lldp *lldp, size_t extra) {
  44         sd_lldp_neighbor *n;
  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                 n = prioq_peek(lldp->neighbor_by_expiry);
  55                 if (!n)
  56                         break;
  57
  58                 if (hashmap_size(lldp->neighbor_by_id) > LESS_BY(lldp->neighbors_max, extra))
  59                         goto remove_one;
  60
  61                 if (t == USEC_INFINITY)
  62                         t = now(clock_boottime_or_monotonic());
  63
  64                 if (n->until > t)
  65                         break;
  66
  67         remove_one:
  68                 lldp_neighbor_unlink(n);
  69                 changed = true;
  70         }
  71
  72         return changed;
  73 }
  74
  75 static int lldp_add_neighbor(sd_lldp *lldp, sd_lldp_neighbor *n) {
  76         sd_lldp_neighbor *old;
  77         bool changed = false;
  78         int r;
  79
  80         assert(lldp);
  81         assert(n);
  82         assert(!n->lldp);
  83
  84         /* First retrieve the old entry for this MSAP */
  85         old = hashmap_get(lldp->neighbor_by_id, &n->id);
  86         if (old) {
  87                 if (lldp_neighbor_equal(n, old)) {
  88                         /* Is this equal, then restart the TTL counter, but don't do anyting else. */
  89                         lldp_neighbor_start_ttl(old);
  90                         return 0;
  91                 }
  92
  93                 /* Data changed, remove the old entry, and add a new one */
  94                 lldp_neighbor_unlink(old);
  95                 changed = true;
  96         }
  97
  98         /* Then, add the new entry in its place, but only if it has a non-zero TTL. */
  99         if (n->ttl <= 0)
 100                 return changed;
 101
 102         /* Only add if the neighbor has a capability we are interested in. Note that we also store all neighbors with
 103          * no caps field set. */
 104         if (n->has_capabilities &&
 105             (n->enabled_capabilities & lldp->capability_mask) == 0)
 106                 return changed;
 107
 108         /* Then, make room for at least one new neighbor */
 109         lldp_make_space(lldp, 1);
 110
 111         r = hashmap_put(lldp->neighbor_by_id, &n->id, n);
 112         if (r < 0)
 113                 return r;
 114
 115         r = prioq_put(lldp->neighbor_by_expiry, n, &n->prioq_idx);
 116         if (r < 0) {
 117                 assert_se(hashmap_remove(lldp->neighbor_by_id, &n->id) == n);
 118                 return r;
 119         }
 120
 121         n->lldp = lldp;
 122
 123         return true;
 124 }
 125
 126 static int lldp_handle_datagram(sd_lldp *lldp, sd_lldp_neighbor *n) {
 127         int r;
 128
 129         assert(lldp);
 130         assert(n);
 131
 132         r = lldp_neighbor_parse(n);
 133         if (r == -EBADMSG) /* Ignore bad messages */
 134                 return 0;
 135         if (r < 0)
 136                 return r;
 137
 138         lldp_neighbor_start_ttl(n);
 139
 140         r = lldp_add_neighbor(lldp, n);
 141         if (r < 0) {
 142                 log_lldp_errno(r, "Failed to add datagram. Ignoring.");
 143                 return 0;
 144         }
 145
 146         log_lldp("Successfully processed LLDP datagram.");
 147
 148         if (r > 0 && lldp->callback) /* Only invoke the callback if something actually changed. */
 149                 lldp->callback(lldp, lldp->userdata);
 150
 151         return 0;
 152 }
 153
 154 static int lldp_receive_datagram(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
 155         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
 156         ssize_t space, length;
 157         sd_lldp *lldp = userdata;
 158
 159         assert(fd >= 0);
 160         assert(lldp);
 161
 162         space = next_datagram_size_fd(fd);
 163         if (space < 0)
 164                 return log_lldp_errno(space, "Failed to determine datagram size to read: %m");
 165
 166         n = lldp_neighbor_new(space);
 167         if (!n)
 168                 return -ENOMEM;
 169
 170         length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT);
 171         if (length < 0)
 172                 return log_lldp_errno(errno, "Failed to read LLDP datagram: %m");
 173
 174         if ((size_t) length != n->raw_size) {
 175                 log_lldp("Packet size mismatch.");
 176                 return -EINVAL;
 177         }
 178
 179         return lldp_handle_datagram(lldp, n);
 180 }
 181
 182 _public_ int sd_lldp_start(sd_lldp *lldp) {
 183         int r;
 184
 185         assert_return(lldp, -EINVAL);
 186
 187         if (lldp->fd >= 0)
 188                 return 0;
 189
 190         assert(!lldp->io_event_source);
 191
 192         lldp->fd = lldp_network_bind_raw_socket(lldp->ifindex);
 193         if (lldp->fd < 0)
 194                 return lldp->fd;
 195
 196         if (lldp->event) {
 197                 r = sd_event_add_io(lldp->event, &lldp->io_event_source, lldp->fd, EPOLLIN, lldp_receive_datagram, lldp);
 198                 if (r < 0)
 199                         goto fail;
 200
 201                 r = sd_event_source_set_priority(lldp->io_event_source, lldp->event_priority);
 202                 if (r < 0)
 203                         goto fail;
 204
 205                 (void) sd_event_source_set_description(lldp->io_event_source, "lldp-io");
 206         }
 207
 208         return 1;
 209
 210 fail:
 211         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 212         lldp->fd = safe_close(lldp->fd);
 213
 214         return r;
 215 }
 216
 217 _public_ int sd_lldp_stop(sd_lldp *lldp) {
 218         assert_return(lldp, -EINVAL);
 219
 220         if (lldp->fd < 0)
 221                 return 0;
 222
 223         lldp->timer_event_source = sd_event_source_unref(lldp->timer_event_source);
 224         lldp->io_event_source = sd_event_source_unref(lldp->io_event_source);
 225         lldp->fd = safe_close(lldp->fd);
 226
 227         lldp_flush_neighbors(lldp);
 228
 229         return 1;
 230 }
 231
 232 _public_ int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int64_t priority) {
 233         int r;
 234
 235         assert_return(lldp, -EINVAL);
 236         assert_return(lldp->fd < 0, -EBUSY);
 237         assert_return(!lldp->event, -EBUSY);
 238
 239         if (event)
 240                 lldp->event = sd_event_ref(event);
 241         else {
 242                 r = sd_event_default(&lldp->event);
 243                 if (r < 0)
 244                         return r;
 245         }
 246
 247         lldp->event_priority = priority;
 248
 249         return 0;
 250 }
 251
 252 _public_ int sd_lldp_detach_event(sd_lldp *lldp) {
 253
 254         assert_return(lldp, -EINVAL);
 255         assert_return(lldp->fd < 0, -EBUSY);
 256
 257         lldp->event = sd_event_unref(lldp->event);
 258         return 0;
 259 }
 260
 261 _public_ int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_callback_t cb, void *userdata) {
 262         assert_return(lldp, -EINVAL);
 263
 264         lldp->callback = cb;
 265         lldp->userdata = userdata;
 266
 267         return 0;
 268 }
 269
 270 _public_ sd_lldp* sd_lldp_unref(sd_lldp *lldp) {
 271
 272         if (!lldp)
 273                 return NULL;
 274
 275         lldp_flush_neighbors(lldp);
 276
 277         hashmap_free(lldp->neighbor_by_id);
 278         prioq_free(lldp->neighbor_by_expiry);
 279
 280         sd_event_source_unref(lldp->io_event_source);
 281         sd_event_source_unref(lldp->timer_event_source);
 282         sd_event_unref(lldp->event);
 283         safe_close(lldp->fd);
 284
 285         free(lldp);
 286
 287         return NULL;
 288 }
 289
 290 _public_ int sd_lldp_new(sd_lldp **ret, int ifindex) {
 291         _cleanup_(sd_lldp_unrefp) sd_lldp *lldp = NULL;
 292         int r;
 293
 294         assert_return(ret, -EINVAL);
 295         assert_return(ifindex > 0, -EINVAL);
 296
 297         lldp = new0(sd_lldp, 1);
 298         if (!lldp)
 299                 return -ENOMEM;
 300
 301         lldp->fd = -1;
 302         lldp->ifindex = ifindex;
 303         lldp->neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX;
 304         lldp->capability_mask = (uint16_t) -1;
 305
 306         lldp->neighbor_by_id = hashmap_new(&lldp_neighbor_id_hash_ops);
 307         if (!lldp->neighbor_by_id)
 308                 return -ENOMEM;
 309
 310         r = prioq_ensure_allocated(&lldp->neighbor_by_expiry, lldp_neighbor_prioq_compare_func);
 311         if (r < 0)
 312                 return r;
 313
 314         *ret = lldp;
 315         lldp = NULL;
 316
 317         return 0;
 318 }
 319
 320 static int neighbor_compare_func(const void *a, const void *b) {
 321         const sd_lldp_neighbor * const*x = a, * const *y = b;
 322
 323         return lldp_neighbor_id_hash_ops.compare(&(*x)->id, &(*y)->id);
 324 }
 325
 326 static int lldp_start_timer(sd_lldp *lldp);
 327
 328 static int on_timer_event(sd_event_source *s, uint64_t usec, void *userdata) {
 329         sd_lldp *lldp = userdata;
 330         int r, q;
 331
 332         r = lldp_make_space(lldp, 0);
 333         if (r < 0)
 334                 return log_lldp_errno(r, "Failed to make space: %m");
 335
 336         q = lldp_start_timer(lldp);
 337         if (q < 0)
 338                 return log_lldp_errno(q, "Failed to restart timer: %m");
 339
 340         log_lldp("LLDP timer event hit.");
 341         if (r > 0 && lldp->callback) /* Invoke callback if we dropped an entry */
 342                 lldp->callback(lldp, lldp->userdata);
 343
 344         return 0;
 345 }
 346
 347 static int lldp_start_timer(sd_lldp *lldp) {
 348         sd_lldp_neighbor *n;
 349         int r;
 350
 351         assert(lldp);
 352
 353         n = prioq_peek(lldp->neighbor_by_expiry);
 354         if (!n) {
 355
 356                 if (lldp->timer_event_source)
 357                         return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_OFF);
 358
 359                 return 0;
 360         }
 361
 362         if (lldp->timer_event_source) {
 363                 r = sd_event_source_set_time(lldp->timer_event_source, n->until);
 364                 if (r < 0)
 365                         return r;
 366
 367                 return sd_event_source_set_enabled(lldp->timer_event_source, SD_EVENT_ONESHOT);
 368         }
 369
 370         if (!lldp->event)
 371                 return 0;
 372
 373         r = sd_event_add_time(lldp->event, &lldp->timer_event_source, clock_boottime_or_monotonic(), n->until, 0, on_timer_event, lldp);
 374         if (r < 0)
 375                 return r;
 376
 377         r = sd_event_source_set_priority(lldp->timer_event_source, lldp->event_priority);
 378         if (r < 0)
 379                 return r;
 380
 381         (void) sd_event_source_set_description(lldp->timer_event_source, "lldp-timer");
 382         return 0;
 383 }
 384
 385 _public_ int sd_lldp_get_neighbors(sd_lldp *lldp, sd_lldp_neighbor ***ret) {
 386         sd_lldp_neighbor **l = NULL, *n;
 387         Iterator i;
 388         int k = 0, r;
 389
 390         assert_return(lldp, -EINVAL);
 391         assert_return(ret, -EINVAL);
 392
 393         /* Flush out old entries, before we return data */
 394         (void) lldp_make_space(lldp, 0);
 395
 396         if (hashmap_isempty(lldp->neighbor_by_id)) { /* Special shortcut */
 397                 *ret = NULL;
 398                 return 0;
 399         }
 400
 401         l = new0(sd_lldp_neighbor*, hashmap_size(lldp->neighbor_by_id));
 402         if (!l)
 403                 return -ENOMEM;
 404
 405         r = lldp_start_timer(lldp);
 406         if (r < 0) {
 407                 free(l);
 408                 return r;
 409         }
 410
 411         HASHMAP_FOREACH(n, lldp->neighbor_by_id, i)
 412                 l[k++] = sd_lldp_neighbor_ref(n);
 413
 414         assert((size_t) k == hashmap_size(lldp->neighbor_by_id));
 415
 416         /* Return things in a stable order */
 417         qsort(l, k, sizeof(sd_lldp_neighbor*), neighbor_compare_func);
 418         *ret = l;
 419
 420         return k;
 421 }
 422
 423 _public_ int sd_lldp_set_neighbors_max(sd_lldp *lldp, uint64_t m) {
 424         assert_return(lldp, -EINVAL);
 425         assert_return(m <= 0, -EINVAL);
 426
 427         lldp->neighbors_max = m;
 428         lldp_make_space(lldp, 0);
 429
 430         return 0;
 431 }
 432
 433 _public_ int sd_lldp_match_capabilities(sd_lldp *lldp, uint16_t mask) {
 434         assert_return(lldp, -EINVAL);
 435         assert_return(mask != 0, -EINVAL);
 436
 437         lldp->capability_mask = mask;
 438
 439         return 0;
 440 }