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