src/libsystemd-network/sd-lldp.c

   1 /*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
   2
   3 /***
   4     This file is part of systemd.
   5
   6     Copyright (C) 2014 Tom Gundersen
   7     Copyright (C) 2014 Susant Sahani
   8
   9     systemd is free software; you can redistribute it and/or modify it
  10     under the terms of the GNU Lesser General Public License as published by
  11     the Free Software Foundation; either version 2.1 of the License, or
  12     (at your option) any later version.
  13
  14     systemd is distributed in the hope that it will be useful, but
  15     WITHOUT ANY WARRANTY; without even the implied warranty of
  16     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  17     Lesser General Public License for more details.
  18
  19     You should have received a copy of the GNU Lesser General Public License
  20     along with systemd; If not, see <http://www.gnu.org/licenses/>.
  21 ***/
  22
  23 #include <arpa/inet.h>
  24
  25 #include "siphash24.h"
  26 #include "hashmap.h"
  27 #include "event-util.h"
  28
  29 #include "lldp-tlv.h"
  30 #include "lldp-port.h"
  31 #include "sd-lldp.h"
  32 #include "prioq.h"
  33 #include "strv.h"
  34 #include "lldp-internal.h"
  35 #include "lldp-util.h"
  36 #include "ether-addr-util.h"
  37
  38 typedef enum LLDPAgentRXState {
  39         LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL = 4,
  40         LLDP_AGENT_RX_DELETE_AGED_INFO,
  41         LLDP_AGENT_RX_LLDP_INITIALIZE,
  42         LLDP_AGENT_RX_WAIT_FOR_FRAME,
  43         LLDP_AGENT_RX_RX_FRAME,
  44         LLDP_AGENT_RX_DELETE_INFO,
  45         LLDP_AGENT_RX_UPDATE_INFO,
  46         _LLDP_AGENT_RX_STATE_MAX,
  47         _LLDP_AGENT_RX_INVALID = -1,
  48 } LLDPAgentRXState;
  49
  50 /* Section 10.5.2.2 Reception counters */
  51 struct lldp_agent_statitics {
  52         uint64_t stats_ageouts_total;
  53         uint64_t stats_frames_discarded_total;
  54         uint64_t stats_frames_in_errors_total;
  55         uint64_t stats_frames_in_total;
  56         uint64_t stats_tlvs_discarded_total;
  57         uint64_t stats_tlvs_unrecognized_total;
  58 };
  59
  60 struct sd_lldp {
  61         lldp_port *port;
  62
  63         Prioq *by_expiry;
  64         Hashmap *neighbour_mib;
  65
  66         sd_lldp_cb_t cb;
  67
  68         void *userdata;
  69
  70         LLDPAgentRXState rx_state;
  71         lldp_agent_statitics statitics;
  72 };
  73
  74 static unsigned long chassis_id_hash_func(const void *p,
  75                                           const uint8_t hash_key[HASH_KEY_SIZE]) {
  76         uint64_t u;
  77         const lldp_chassis_id *id = p;
  78
  79         assert(id);
  80
  81         siphash24((uint8_t *) &u, id->data, id->length, hash_key);
  82
  83         return (unsigned long) u;
  84 }
  85
  86 static int chassis_id_compare_func(const void *_a, const void *_b) {
  87         const lldp_chassis_id *a, *b;
  88
  89         a = _a;
  90         b = _b;
  91
  92         assert(!a->length || a->data);
  93         assert(!b->length || b->data);
  94
  95         if (a->type != b->type)
  96                 return -1;
  97
  98         if (a->length != b->length)
  99                 return a->length < b->length ? -1 : 1;
 100
 101         return memcmp(a->data, b->data, a->length);
 102 }
 103
 104 static const struct hash_ops chassis_id_hash_ops = {
 105         .hash = chassis_id_hash_func,
 106         .compare = chassis_id_compare_func
 107 };
 108
 109 static void lldp_mib_delete_objects(sd_lldp *lldp);
 110 static void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state);
 111 static void lldp_run_state_machine(sd_lldp *ll);
 112
 113 static int lldp_receive_frame(sd_lldp *lldp, tlv_packet *tlv) {
 114         int r;
 115
 116         assert(lldp);
 117         assert(tlv);
 118
 119         /* Remove expired packets */
 120         if (prioq_size(lldp->by_expiry) > 0) {
 121
 122                 lldp_set_state(lldp, LLDP_AGENT_RX_DELETE_INFO);
 123
 124                 lldp_mib_delete_objects(lldp);
 125         }
 126
 127         r = lldp_mib_add_objects(lldp->by_expiry, lldp->neighbour_mib, tlv);
 128         if (r < 0)
 129                 goto out;
 130
 131         lldp_set_state(lldp, LLDP_AGENT_RX_UPDATE_INFO);
 132
 133         log_lldp("Packet added. MIB size: %d , PQ size: %d",
 134                  hashmap_size(lldp->neighbour_mib),
 135                  prioq_size(lldp->by_expiry));
 136
 137         lldp->statitics.stats_frames_in_total ++;
 138
 139         return 0;
 140
 141  out:
 142         if (r < 0)
 143                 log_lldp("Receive frame failed: %s", strerror(-r));
 144
 145         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 146
 147         return 0;
 148 }
 149
 150 /* 10.3.2 LLDPDU validation: rxProcessFrame() */
 151 int lldp_handle_packet(tlv_packet *tlv, uint16_t length) {
 152         uint16_t type, len, i, l, t;
 153         bool chassis_id = false;
 154         bool malformed = false;
 155         bool port_id = false;
 156         bool ttl = false;
 157         bool end = false;
 158         lldp_port *port;
 159         uint8_t *p, *q;
 160         sd_lldp *lldp;
 161         int r;
 162
 163         assert(tlv);
 164         assert(length > 0);
 165
 166         port = (lldp_port *) tlv->userdata;
 167         lldp = (sd_lldp *) port->userdata;
 168
 169         if (lldp->port->status == LLDP_PORT_STATUS_DISABLED) {
 170                 log_lldp("Port is disabled : %s . Dropping ...",
 171                          lldp->port->ifname);
 172                 goto out;
 173         }
 174
 175         lldp_set_state(lldp, LLDP_AGENT_RX_RX_FRAME);
 176
 177         p = tlv->pdu;
 178         p += sizeof(struct ether_header);
 179
 180         for (i = 1, l = 0; l <= length; i++) {
 181
 182                 memcpy(&t, p, sizeof(uint16_t));
 183
 184                 type = ntohs(t) >> 9;
 185                 len = ntohs(t) & 0x01ff;
 186
 187                 if (type == LLDP_TYPE_END) {
 188                         if (len != 0) {
 189                                 log_lldp("TLV type end is not length 0. Length:%d received . Dropping ...",
 190                                          len);
 191
 192                                 malformed = true;
 193                                 goto out;
 194                         }
 195
 196                         end = true;
 197
 198                         break;
 199                 } else if (type >=_LLDP_TYPE_MAX) {
 200                         log_lldp("TLV type not recognized %d . Dropping ...",
 201                                  type);
 202
 203                         malformed = true;
 204                         goto out;
 205                 }
 206
 207                 /* skip type and lengh encoding */
 208                 p += 2;
 209                 q = p;
 210
 211                 p += len;
 212                 l += (len + 2);
 213
 214                 if (i <= 3) {
 215                         if (i != type) {
 216                                 log_lldp("TLV missing or out of order. Dropping ...");
 217
 218                                 malformed = true;
 219                                 goto out;
 220                         }
 221                 }
 222
 223                 switch(type) {
 224                 case LLDP_TYPE_CHASSIS_ID:
 225
 226                         if (len < 2) {
 227                                 log_lldp("Received malformed Chassis ID TLV len = %d. Dropping",
 228                                          len);
 229
 230                                 malformed = true;
 231                                 goto out;
 232                         }
 233
 234                         if (chassis_id) {
 235                                 log_lldp("Duplicate Chassis ID TLV found. Dropping ...");
 236
 237                                 malformed = true;
 238                                 goto out;
 239                         }
 240
 241                         /* Look what subtype it has */
 242                         if (*q == LLDP_CHASSIS_SUBTYPE_RESERVED ||
 243                             *q > LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED) {
 244                                 log_lldp("Unknown subtype: %d found in Chassis ID TLV . Dropping ...",
 245                                          *q);
 246
 247                                 malformed = true;
 248                                 goto out;
 249
 250                         }
 251
 252                         chassis_id = true;
 253
 254                         break;
 255                 case LLDP_TYPE_PORT_ID:
 256
 257                         if (len < 2) {
 258                                 log_lldp("Received malformed Port ID TLV len = %d. Dropping",
 259                                          len);
 260
 261                                 malformed = true;
 262                                 goto out;
 263                         }
 264
 265                         if (port_id) {
 266                                 log_lldp("Duplicate Port ID TLV found. Dropping ...");
 267
 268                                 malformed = true;
 269                                 goto out;
 270                         }
 271
 272                         /* Look what subtype it has */
 273                         if (*q == LLDP_PORT_SUBTYPE_RESERVED ||
 274                             *q > LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED) {
 275                                 log_lldp("Unknown subtype: %d found in Port ID TLV . Dropping ...",
 276                                          *q);
 277
 278                                 malformed = true;
 279                                 goto out;
 280
 281                         }
 282
 283                         port_id = true;
 284
 285                         break;
 286                 case LLDP_TYPE_TTL:
 287
 288                         if(len != 2) {
 289                                 log_lldp(
 290                                          "Received invalid lenth: %d TTL TLV. Dropping ...",
 291                                          len);
 292
 293                                 malformed = true;
 294                                 goto out;
 295                         }
 296
 297                         if (ttl) {
 298                                 log_lldp("Duplicate TTL TLV found. Dropping ...");
 299
 300                                 malformed = true;
 301                                 goto out;
 302                         }
 303
 304                         ttl = true;
 305
 306                         break;
 307                 default:
 308
 309                         if (len == 0) {
 310                                 log_lldp("TLV type = %d's, length 0 received . Dropping ...",
 311                                          type);
 312
 313                                 malformed = true;
 314                                 goto out;
 315                         }
 316                         break;
 317                 }
 318         }
 319
 320         if(!chassis_id || !port_id || !ttl || !end) {
 321                 log_lldp( "One or more mandotory TLV missing . Dropping ...");
 322
 323                 malformed = true;
 324                 goto out;
 325
 326         }
 327
 328         r = tlv_packet_parse_pdu(tlv, length);
 329         if (r < 0) {
 330                 log_lldp( "Failed to parse the TLV. Dropping ...");
 331
 332                 malformed = true;
 333                 goto out;
 334         }
 335
 336         return lldp_receive_frame(lldp, tlv);
 337
 338  out:
 339         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 340
 341         if (malformed) {
 342                 lldp->statitics.stats_frames_discarded_total ++;
 343                 lldp->statitics.stats_frames_in_errors_total ++;
 344         }
 345
 346         tlv_packet_free(tlv);
 347
 348         return 0;
 349 }
 350
 351 static int ttl_expiry_item_prioq_compare_func(const void *a, const void *b) {
 352         const lldp_neighbour_port *p = a, *q = b;
 353
 354         if (p->until < q->until)
 355                 return -1;
 356
 357         if (p->until > q->until)
 358                 return 1;
 359
 360         return 0;
 361 }
 362
 363 static void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state) {
 364
 365         assert(lldp);
 366         assert(state < _LLDP_AGENT_RX_STATE_MAX);
 367
 368         lldp->rx_state = state;
 369
 370         lldp_run_state_machine(lldp);
 371 }
 372
 373 static void lldp_run_state_machine(sd_lldp *lldp) {
 374
 375         if (lldp->rx_state == LLDP_AGENT_RX_UPDATE_INFO)
 376                 if (lldp->cb)
 377                         lldp->cb(lldp, LLDP_AGENT_RX_UPDATE_INFO, lldp->userdata);
 378 }
 379
 380 /* 10.5.5.2.1 mibDeleteObjects ()
 381  * The mibDeleteObjects () procedure deletes all information in the LLDP remote
 382  * systems MIB associated with the MSAP identifier if an LLDPDU is received with
 383  * an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */
 384
 385 static void lldp_mib_delete_objects(sd_lldp *lldp) {
 386         lldp_neighbour_port *p;
 387         usec_t t = 0;
 388
 389         /* Remove all entries that are past their TTL */
 390         for (;;) {
 391
 392                 if (prioq_size(lldp->by_expiry) <= 0)
 393                         break;
 394
 395                 p = prioq_peek(lldp->by_expiry);
 396                 if (!p)
 397                         break;
 398
 399                 if (t <= 0)
 400                         t = now(CLOCK_BOOTTIME);
 401
 402                 if (p->until > t)
 403                         break;
 404
 405                 lldp_neighbour_port_remove_and_free(p);
 406
 407                 lldp->statitics.stats_ageouts_total ++;
 408         }
 409 }
 410
 411 static void lldp_mib_objects_flush(sd_lldp *lldp) {
 412         lldp_neighbour_port *p, *q;
 413         lldp_chassis *c;
 414
 415         assert(lldp);
 416         assert(lldp->neighbour_mib);
 417         assert(lldp->by_expiry);
 418
 419         /* Drop all packets */
 420         while ((c = hashmap_steal_first(lldp->neighbour_mib))) {
 421
 422                 LIST_FOREACH_SAFE(port, p, q, c->ports) {
 423                         lldp_neighbour_port_remove_and_free(p);
 424                 }
 425         }
 426
 427         assert(hashmap_size(lldp->neighbour_mib) == 0);
 428         assert(prioq_size(lldp->by_expiry) == 0);
 429 }
 430
 431 int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) {
 432         _cleanup_free_ char *s = NULL, *t = NULL, *k = NULL;
 433         _cleanup_free_ char *temp_path = NULL;
 434         _cleanup_fclose_ FILE *f = NULL;
 435         uint8_t *mac, *port_id, type;
 436         lldp_neighbour_port *p;
 437         uint16_t data = 0, length = 0;
 438         char buf[LINE_MAX];
 439         lldp_chassis *c;
 440         usec_t time;
 441         Iterator i;
 442         int r;
 443
 444         assert(lldp);
 445         assert(lldp_file);
 446
 447         r = fopen_temporary(lldp_file, &f, &temp_path);
 448         if (r < 0)
 449                 goto finish;
 450
 451         fchmod(fileno(f), 0644);
 452
 453         HASHMAP_FOREACH(c, lldp->neighbour_mib, i) {
 454                 LIST_FOREACH(port, p, c->ports) {
 455
 456                         r = lldp_read_chassis_id(p->packet, &type, &length, &mac);
 457                         if (r < 0)
 458                                 continue;
 459
 460                         memzero(buf, LINE_MAX);
 461
 462                         sprintf(buf, "'_Chassis=%02x:%02x:%02x:%02x:%02x:%02x' '_CType=%d' ",
 463                                 mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
 464
 465                         s = strdup(buf);
 466                         if (!s)
 467                                 return -ENOMEM;
 468
 469                         r = lldp_read_port_id(p->packet, &type, &length, &port_id);
 470                         if (r < 0) {
 471                                 free(s);
 472                                 continue;
 473                         }
 474
 475                         memzero(buf, LINE_MAX);
 476                         if (type != LLDP_PORT_SUBTYPE_MAC_ADDRESS) {
 477
 478                                 k = strndup((char *) port_id, length -1);
 479                                 if (!k)
 480                                         return -ENOMEM;
 481
 482                                 sprintf(buf, "'_Port=%s' '_PType=%d' ", k , type);
 483
 484                                 t = strappend(s, buf);
 485
 486                                 free(k);
 487                         } else {
 488
 489                                 mac = port_id;
 490
 491                                 sprintf(buf, "'_Port=%02x:%02x:%02x:%02x:%02x:%02x' '_PType=%d' ",
 492                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
 493
 494                                 t = strappend(s, buf);
 495                         }
 496
 497                         if (!t)
 498                                 return -ENOMEM;
 499
 500                         free(s);
 501                         s = t;
 502
 503                         time = now(CLOCK_BOOTTIME);
 504
 505                         /* Don't write expired packets */
 506                         if(time - p->until <= 0) {
 507                                 free(s);
 508                                 continue;
 509                         }
 510
 511                         memzero(buf, LINE_MAX);
 512                         sprintf(buf, "'_TTL=%lu' ", p->until);
 513
 514                         t = strappend(s, buf);
 515                         if (!t)
 516                                 return -ENOMEM;
 517
 518                         free(s);
 519                         s = t;
 520
 521                         r = lldp_read_system_name(p->packet, &length, &k);
 522                         if (r < 0)
 523                                 k = strappend(s, "'_NAME=N/A' ");
 524                         else {
 525                                 t = strndup(k, length);
 526                                 if (!t)
 527                                         return -ENOMEM;
 528
 529                                 k = strjoin(s, "'_NAME=", t, "' ", NULL);
 530                         }
 531
 532                         if (!k)
 533                                 return -ENOMEM;
 534
 535                         free(s);
 536                         s = k;
 537
 538                         memzero(buf, LINE_MAX);
 539
 540                         (void)lldp_read_system_capability(p->packet, &data);
 541
 542                         sprintf(buf, "'_CAP=%x'", data);
 543
 544                         t = strappend(s, buf);
 545                         if (!t)
 546                                 return -ENOMEM;
 547
 548                         fprintf(f, "%s\n", t);
 549
 550                         free(s);
 551                         free(t);
 552                 }
 553         }
 554         r = 0;
 555
 556         s = NULL;
 557         t = NULL;
 558         k = NULL;
 559
 560         fflush(f);
 561
 562         if (ferror(f) || rename(temp_path, lldp_file) < 0) {
 563                 r = -errno;
 564                 unlink(lldp_file);
 565                 unlink(temp_path);
 566         }
 567
 568  finish:
 569         if (r < 0)
 570                 log_error("Failed to save lldp data %s: %s", lldp_file, strerror(-r));
 571
 572         return r;
 573 }
 574
 575 int sd_lldp_start(sd_lldp *lldp) {
 576         int r;
 577
 578         assert_return(lldp, -EINVAL);
 579         assert_return(lldp->port, -EINVAL);
 580
 581         lldp->port->status = LLDP_PORT_STATUS_ENABLED;
 582
 583         lldp_set_state(lldp, LLDP_AGENT_RX_LLDP_INITIALIZE);
 584
 585         r = lldp_port_start(lldp->port);
 586         if (r < 0) {
 587                 log_lldp("Failed to start Port : %s , %s",
 588                          lldp->port->ifname,
 589                          strerror(-r));
 590
 591                 lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL);
 592
 593                 return r;
 594         }
 595
 596         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 597
 598         return 0;
 599 }
 600
 601 int sd_lldp_stop(sd_lldp *lldp) {
 602         int r;
 603
 604         assert_return(lldp, -EINVAL);
 605         assert_return(lldp->port, -EINVAL);
 606
 607         lldp->port->status = LLDP_PORT_STATUS_DISABLED;
 608
 609         r = lldp_port_stop(lldp->port);
 610         if (r < 0)
 611                 return r;
 612
 613         lldp_mib_objects_flush(lldp);
 614
 615         return 0;
 616 }
 617
 618 int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority) {
 619         int r;
 620
 621         assert_return(lldp, -EINVAL);
 622         assert_return(!lldp->port->event, -EBUSY);
 623
 624         if (event)
 625                 lldp->port->event = sd_event_ref(event);
 626         else {
 627                 r = sd_event_default(&lldp->port->event);
 628                 if (r < 0)
 629                         return r;
 630         }
 631
 632         lldp->port->event_priority = priority;
 633
 634         return 0;
 635 }
 636
 637 int sd_lldp_detach_event(sd_lldp *lldp) {
 638
 639         assert_return(lldp, -EINVAL);
 640
 641         lldp->port->event = sd_event_unref(lldp->port->event);
 642
 643         return 0;
 644 }
 645
 646 int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_cb_t cb, void *userdata) {
 647         assert_return(lldp, -EINVAL);
 648
 649         lldp->cb = cb;
 650         lldp->userdata = userdata;
 651
 652         return 0;
 653 }
 654
 655 void sd_lldp_free(sd_lldp *lldp) {
 656
 657         if (!lldp)
 658                 return;
 659
 660         /* Drop all packets */
 661         lldp_mib_objects_flush(lldp);
 662
 663         lldp_port_free(lldp->port);
 664
 665         hashmap_free(lldp->neighbour_mib);
 666         prioq_free(lldp->by_expiry);
 667
 668         free(lldp);
 669 }
 670
 671 int sd_lldp_new(int ifindex,
 672                 const char *ifname,
 673                 const struct ether_addr *mac,
 674                 sd_lldp **ret) {
 675         _cleanup_lldp_free_ sd_lldp *lldp = NULL;
 676         int r;
 677
 678         assert_return(ret, -EINVAL);
 679         assert_return(ifindex > 0, -EINVAL);
 680         assert_return(ifname, -EINVAL);
 681         assert_return(mac, -EINVAL);
 682
 683         lldp = new0(sd_lldp, 1);
 684         if (!lldp)
 685                 return -ENOMEM;
 686
 687         r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port);
 688         if (r < 0)
 689                 return r;
 690
 691         lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops);
 692         if (!lldp->neighbour_mib)
 693                 return -ENOMEM;
 694
 695         r = prioq_ensure_allocated(&lldp->by_expiry,
 696                                    ttl_expiry_item_prioq_compare_func);
 697         if (r < 0)
 698                 return r;
 699
 700         lldp->rx_state = LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL;
 701
 702         *ret = lldp;
 703         lldp = NULL;
 704
 705         return 0;
 706 }