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  out:
 137         if (r < 0)
 138                 log_lldp("Receive frame failed: %s", strerror(-r));
 139
 140         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 141
 142         return 0;
 143 }
 144
 145 /* 10.3.2 LLDPDU validation: rxProcessFrame() */
 146 int lldp_handle_packet(tlv_packet *tlv, uint16_t length) {
 147         uint16_t type, len, i, l, t;
 148         bool chassis_id = false;
 149         bool malformed = false;
 150         bool port_id = false;
 151         bool ttl = false;
 152         bool end = false;
 153         lldp_port *port;
 154         uint8_t *p, *q;
 155         sd_lldp *lldp;
 156         int r;
 157
 158         assert(tlv);
 159         assert(length > 0);
 160
 161         port = (lldp_port *) tlv->userdata;
 162         lldp = (sd_lldp *) port->userdata;
 163
 164         if (lldp->port->status == LLDP_PORT_STATUS_DISABLED) {
 165                 log_lldp("Port is disabled : %s . Dropping ...",
 166                          lldp->port->ifname);
 167                 goto out;
 168         }
 169
 170         lldp_set_state(lldp, LLDP_AGENT_RX_RX_FRAME);
 171
 172         p = tlv->pdu;
 173         p += sizeof(struct ether_header);
 174
 175         for (i = 1, l = 0; l <= length; i++) {
 176
 177                 memcpy(&t, p, sizeof(uint16_t));
 178
 179                 type = ntohs(t) >> 9;
 180                 len = ntohs(t) & 0x01ff;
 181
 182                 if (type == LLDP_TYPE_END) {
 183                         if (len != 0) {
 184                                 log_lldp("TLV type end is not length 0. Length:%d received . Dropping ...",
 185                                          len);
 186
 187                                 malformed = true;
 188                                 goto out;
 189                         }
 190
 191                         end = true;
 192
 193                         break;
 194                 } else if (type >=_LLDP_TYPE_MAX) {
 195                         log_lldp("TLV type not recognized %d . Dropping ...",
 196                                  type);
 197
 198                         malformed = true;
 199                         goto out;
 200                 }
 201
 202                 /* skip type and length encoding */
 203                 p += 2;
 204                 q = p;
 205
 206                 p += len;
 207                 l += (len + 2);
 208
 209                 if (i <= 3) {
 210                         if (i != type) {
 211                                 log_lldp("TLV missing or out of order. Dropping ...");
 212
 213                                 malformed = true;
 214                                 goto out;
 215                         }
 216                 }
 217
 218                 switch(type) {
 219                 case LLDP_TYPE_CHASSIS_ID:
 220
 221                         if (len < 2) {
 222                                 log_lldp("Received malformed Chassis ID TLV len = %d. Dropping",
 223                                          len);
 224
 225                                 malformed = true;
 226                                 goto out;
 227                         }
 228
 229                         if (chassis_id) {
 230                                 log_lldp("Duplicate Chassis ID TLV found. Dropping ...");
 231
 232                                 malformed = true;
 233                                 goto out;
 234                         }
 235
 236                         /* Look what subtype it has */
 237                         if (*q == LLDP_CHASSIS_SUBTYPE_RESERVED ||
 238                             *q > LLDP_CHASSIS_SUBTYPE_LOCALLY_ASSIGNED) {
 239                                 log_lldp("Unknown subtype: %d found in Chassis ID TLV . Dropping ...",
 240                                          *q);
 241
 242                                 malformed = true;
 243                                 goto out;
 244
 245                         }
 246
 247                         chassis_id = true;
 248
 249                         break;
 250                 case LLDP_TYPE_PORT_ID:
 251
 252                         if (len < 2) {
 253                                 log_lldp("Received malformed Port ID TLV len = %d. Dropping",
 254                                          len);
 255
 256                                 malformed = true;
 257                                 goto out;
 258                         }
 259
 260                         if (port_id) {
 261                                 log_lldp("Duplicate Port ID TLV found. Dropping ...");
 262
 263                                 malformed = true;
 264                                 goto out;
 265                         }
 266
 267                         /* Look what subtype it has */
 268                         if (*q == LLDP_PORT_SUBTYPE_RESERVED ||
 269                             *q > LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED) {
 270                                 log_lldp("Unknown subtype: %d found in Port ID TLV . Dropping ...",
 271                                          *q);
 272
 273                                 malformed = true;
 274                                 goto out;
 275
 276                         }
 277
 278                         port_id = true;
 279
 280                         break;
 281                 case LLDP_TYPE_TTL:
 282
 283                         if(len != 2) {
 284                                 log_lldp(
 285                                          "Received invalid lenth: %d TTL TLV. Dropping ...",
 286                                          len);
 287
 288                                 malformed = true;
 289                                 goto out;
 290                         }
 291
 292                         if (ttl) {
 293                                 log_lldp("Duplicate TTL TLV found. Dropping ...");
 294
 295                                 malformed = true;
 296                                 goto out;
 297                         }
 298
 299                         ttl = true;
 300
 301                         break;
 302                 default:
 303
 304                         if (len == 0) {
 305                                 log_lldp("TLV type = %d's, length 0 received . Dropping ...",
 306                                          type);
 307
 308                                 malformed = true;
 309                                 goto out;
 310                         }
 311                         break;
 312                 }
 313         }
 314
 315         if(!chassis_id || !port_id || !ttl || !end) {
 316                 log_lldp( "One or more mandotory TLV missing . Dropping ...");
 317
 318                 malformed = true;
 319                 goto out;
 320
 321         }
 322
 323         r = tlv_packet_parse_pdu(tlv, length);
 324         if (r < 0) {
 325                 log_lldp( "Failed to parse the TLV. Dropping ...");
 326
 327                 malformed = true;
 328                 goto out;
 329         }
 330
 331         return lldp_receive_frame(lldp, tlv);
 332
 333  out:
 334         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 335
 336         if (malformed) {
 337                 lldp->statistics.stats_frames_discarded_total ++;
 338                 lldp->statistics.stats_frames_in_errors_total ++;
 339         }
 340
 341         sd_lldp_packet_unref(tlv);
 342
 343         return 0;
 344 }
 345
 346 static int ttl_expiry_item_prioq_compare_func(const void *a, const void *b) {
 347         const lldp_neighbour_port *p = a, *q = b;
 348
 349         if (p->until < q->until)
 350                 return -1;
 351
 352         if (p->until > q->until)
 353                 return 1;
 354
 355         return 0;
 356 }
 357
 358 static void lldp_set_state(sd_lldp *lldp, LLDPAgentRXState state) {
 359
 360         assert(lldp);
 361         assert(state < _LLDP_AGENT_RX_STATE_MAX);
 362
 363         lldp->rx_state = state;
 364
 365         lldp_run_state_machine(lldp);
 366 }
 367
 368 static void lldp_run_state_machine(sd_lldp *lldp) {
 369         if (!lldp->cb)
 370                 return;
 371
 372         switch (lldp->rx_state) {
 373         case LLDP_AGENT_RX_UPDATE_INFO:
 374                 lldp->cb(lldp, SD_LLDP_EVENT_UPDATE_INFO, lldp->userdata);
 375                 break;
 376         default:
 377                 break;
 378         }
 379 }
 380
 381 /* 10.5.5.2.1 mibDeleteObjects ()
 382  * The mibDeleteObjects () procedure deletes all information in the LLDP remote
 383  * systems MIB associated with the MSAP identifier if an LLDPDU is received with
 384  * an rxTTL value of zero (see 10.3.2) or the timing counter rxInfoTTL expires. */
 385
 386 static void lldp_mib_delete_objects(sd_lldp *lldp) {
 387         lldp_neighbour_port *p;
 388         usec_t t = 0;
 389
 390         /* Remove all entries that are past their TTL */
 391         for (;;) {
 392
 393                 if (prioq_size(lldp->by_expiry) <= 0)
 394                         break;
 395
 396                 p = prioq_peek(lldp->by_expiry);
 397                 if (!p)
 398                         break;
 399
 400                 if (t <= 0)
 401                         t = now(clock_boottime_or_monotonic());
 402
 403                 if (p->until > t)
 404                         break;
 405
 406                 lldp_neighbour_port_remove_and_free(p);
 407
 408                 lldp->statistics.stats_ageouts_total ++;
 409         }
 410 }
 411
 412 static void lldp_mib_objects_flush(sd_lldp *lldp) {
 413         lldp_neighbour_port *p, *q;
 414         lldp_chassis *c;
 415
 416         assert(lldp);
 417         assert(lldp->neighbour_mib);
 418         assert(lldp->by_expiry);
 419
 420         /* Drop all packets */
 421         while ((c = hashmap_steal_first(lldp->neighbour_mib))) {
 422
 423                 LIST_FOREACH_SAFE(port, p, q, c->ports) {
 424                         lldp_neighbour_port_remove_and_free(p);
 425                 }
 426         }
 427
 428         assert(hashmap_size(lldp->neighbour_mib) == 0);
 429         assert(prioq_size(lldp->by_expiry) == 0);
 430 }
 431
 432 int sd_lldp_save(sd_lldp *lldp, const char *lldp_file) {
 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 fail;
 450
 451         fchmod(fileno(f), 0644);
 452
 453         HASHMAP_FOREACH(c, lldp->neighbour_mib, i) {
 454                 LIST_FOREACH(port, p, c->ports) {
 455                         _cleanup_free_ char *s = NULL;
 456                         char *k, *t;
 457
 458                         r = sd_lldp_packet_read_chassis_id(p->packet, &type, &mac, &length);
 459                         if (r < 0)
 460                                 continue;
 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                                 r = -ENOMEM;
 468                                 goto fail;
 469                         }
 470
 471                         r = sd_lldp_packet_read_port_id(p->packet, &type, &port_id, &length);
 472                         if (r < 0)
 473                                 continue;
 474
 475                         if (type != LLDP_PORT_SUBTYPE_MAC_ADDRESS) {
 476                                 k = strndup((char *) port_id, length -1);
 477                                 if (!k) {
 478                                         r = -ENOMEM;
 479                                         goto fail;
 480                                 }
 481
 482                                 sprintf(buf, "'_Port=%s' '_PType=%d' ", k , type);
 483                                 free(k);
 484                         } else {
 485                                 mac = port_id;
 486                                 sprintf(buf, "'_Port=%02x:%02x:%02x:%02x:%02x:%02x' '_PType=%d' ",
 487                                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5], type);
 488                         }
 489
 490                         k = strappend(s, buf);
 491                         if (!k) {
 492                                 r = -ENOMEM;
 493                                 goto fail;
 494                         }
 495
 496                         free(s);
 497                         s = k;
 498
 499                         time = now(clock_boottime_or_monotonic());
 500
 501                         /* Don't write expired packets */
 502                         if (time - p->until <= 0)
 503                                 continue;
 504
 505                         sprintf(buf, "'_TTL="USEC_FMT"' ", p->until);
 506
 507                         k = strappend(s, buf);
 508                         if (!k) {
 509                                 r = -ENOMEM;
 510                                 goto fail;
 511                         }
 512
 513                         free(s);
 514                         s = k;
 515
 516                         r = sd_lldp_packet_read_system_name(p->packet, &k, &length);
 517                         if (r < 0)
 518                                 k = strappend(s, "'_NAME=N/A' ");
 519                         else {
 520                                 t = strndup(k, length);
 521                                 if (!t) {
 522                                         r = -ENOMEM;
 523                                         goto fail;
 524                                 }
 525
 526                                 k = strjoin(s, "'_NAME=", t, "' ", NULL);
 527                                 free(t);
 528                         }
 529
 530                         if (!k) {
 531                                 r = -ENOMEM;
 532                                 goto fail;
 533                         }
 534
 535                         free(s);
 536                         s = k;
 537
 538                         (void) sd_lldp_packet_read_system_capability(p->packet, &data);
 539
 540                         sprintf(buf, "'_CAP=%x'", data);
 541
 542                         k = strappend(s, buf);
 543                         if (!k) {
 544                                 r = -ENOMEM;
 545                                 goto fail;
 546                         }
 547
 548                         free(s);
 549                         s = k;
 550
 551                         fprintf(f, "%s\n", s);
 552                 }
 553         }
 554
 555         r = fflush_and_check(f);
 556         if (r < 0)
 557                 goto fail;
 558
 559         if (rename(temp_path, lldp_file) < 0) {
 560                 r = -errno;
 561                 goto fail;
 562         }
 563
 564         return 0;
 565
 566  fail:
 567         if (temp_path)
 568                 (void) unlink(temp_path);
 569
 570         return log_error_errno(r, "Failed to save lldp data %s: %m", lldp_file);
 571 }
 572
 573 int sd_lldp_start(sd_lldp *lldp) {
 574         int r;
 575
 576         assert_return(lldp, -EINVAL);
 577         assert_return(lldp->port, -EINVAL);
 578
 579         lldp->port->status = LLDP_PORT_STATUS_ENABLED;
 580
 581         lldp_set_state(lldp, LLDP_AGENT_RX_LLDP_INITIALIZE);
 582
 583         r = lldp_port_start(lldp->port);
 584         if (r < 0) {
 585                 log_lldp("Failed to start Port : %s , %s",
 586                          lldp->port->ifname,
 587                          strerror(-r));
 588
 589                 lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL);
 590
 591                 return r;
 592         }
 593
 594         lldp_set_state(lldp, LLDP_AGENT_RX_WAIT_FOR_FRAME);
 595
 596         return 0;
 597 }
 598
 599 int sd_lldp_stop(sd_lldp *lldp) {
 600         int r;
 601
 602         assert_return(lldp, -EINVAL);
 603         assert_return(lldp->port, -EINVAL);
 604
 605         lldp->port->status = LLDP_PORT_STATUS_DISABLED;
 606
 607         r = lldp_port_stop(lldp->port);
 608         if (r < 0)
 609                 return r;
 610
 611         lldp_mib_objects_flush(lldp);
 612
 613         return 0;
 614 }
 615
 616 int sd_lldp_attach_event(sd_lldp *lldp, sd_event *event, int priority) {
 617         int r;
 618
 619         assert_return(lldp, -EINVAL);
 620         assert_return(!lldp->port->event, -EBUSY);
 621
 622         if (event)
 623                 lldp->port->event = sd_event_ref(event);
 624         else {
 625                 r = sd_event_default(&lldp->port->event);
 626                 if (r < 0)
 627                         return r;
 628         }
 629
 630         lldp->port->event_priority = priority;
 631
 632         return 0;
 633 }
 634
 635 int sd_lldp_detach_event(sd_lldp *lldp) {
 636
 637         assert_return(lldp, -EINVAL);
 638
 639         lldp->port->event = sd_event_unref(lldp->port->event);
 640
 641         return 0;
 642 }
 643
 644 int sd_lldp_set_callback(sd_lldp *lldp, sd_lldp_cb_t cb, void *userdata) {
 645         assert_return(lldp, -EINVAL);
 646
 647         lldp->cb = cb;
 648         lldp->userdata = userdata;
 649
 650         return 0;
 651 }
 652
 653 void sd_lldp_free(sd_lldp *lldp) {
 654
 655         if (!lldp)
 656                 return;
 657
 658         /* Drop all packets */
 659         lldp_mib_objects_flush(lldp);
 660
 661         lldp_port_free(lldp->port);
 662
 663         hashmap_free(lldp->neighbour_mib);
 664         prioq_free(lldp->by_expiry);
 665
 666         free(lldp);
 667 }
 668
 669 int sd_lldp_new(int ifindex,
 670                 const char *ifname,
 671                 const struct ether_addr *mac,
 672                 sd_lldp **ret) {
 673         _cleanup_lldp_free_ sd_lldp *lldp = NULL;
 674         int r;
 675
 676         assert_return(ret, -EINVAL);
 677         assert_return(ifindex > 0, -EINVAL);
 678         assert_return(ifname, -EINVAL);
 679         assert_return(mac, -EINVAL);
 680
 681         lldp = new0(sd_lldp, 1);
 682         if (!lldp)
 683                 return -ENOMEM;
 684
 685         r = lldp_port_new(ifindex, ifname, mac, lldp, &lldp->port);
 686         if (r < 0)
 687                 return r;
 688
 689         lldp->neighbour_mib = hashmap_new(&chassis_id_hash_ops);
 690         if (!lldp->neighbour_mib)
 691                 return -ENOMEM;
 692
 693         r = prioq_ensure_allocated(&lldp->by_expiry,
 694                                    ttl_expiry_item_prioq_compare_func);
 695         if (r < 0)
 696                 return r;
 697
 698         lldp->rx_state = LLDP_AGENT_RX_WAIT_PORT_OPERATIONAL;
 699
 700         *ret = lldp;
 701         lldp = NULL;
 702
 703         return 0;
 704 }
 705
 706 int sd_lldp_get_packets(sd_lldp *lldp, sd_lldp_packet ***tlvs) {
 707         lldp_neighbour_port *p;
 708         lldp_chassis *c;
 709         Iterator iter;
 710         unsigned count = 0, i;
 711
 712         assert_return(lldp, -EINVAL);
 713         assert_return(tlvs, -EINVAL);
 714
 715         HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
 716                 LIST_FOREACH(port, p, c->ports)
 717                         count++;
 718         }
 719
 720         if (!count) {
 721                 *tlvs = NULL;
 722                 return 0;
 723         }
 724
 725         *tlvs = new(sd_lldp_packet *, count);
 726         if (!*tlvs)
 727                 return -ENOMEM;
 728
 729         i = 0;
 730         HASHMAP_FOREACH(c, lldp->neighbour_mib, iter) {
 731                 LIST_FOREACH(port, p, c->ports)
 732                         (*tlvs)[i++] = sd_lldp_packet_ref(p->packet);
 733         }
 734
 735         return count;
 736 }