src/libsystemd-network/lldp-tlv.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 <net/ethernet.h>
  24 #include <arpa/inet.h>
  25
  26 #include "alloc-util.h"
  27 #include "lldp-tlv.h"
  28 #include "macro.h"
  29
  30 int tlv_section_new(tlv_section **ret) {
  31         tlv_section *s;
  32
  33         s = new0(tlv_section, 1);
  34         if (!s)
  35                 return -ENOMEM;
  36
  37         *ret = s;
  38
  39         return 0;
  40 }
  41
  42 void tlv_section_free(tlv_section *m) {
  43
  44         if (!m)
  45                 return;
  46
  47         free(m);
  48 }
  49
  50 int tlv_packet_new(tlv_packet **ret) {
  51         tlv_packet *m;
  52
  53         m = new0(tlv_packet, 1);
  54         if (!m)
  55                 return -ENOMEM;
  56
  57         LIST_HEAD_INIT(m->sections);
  58         m->n_ref = 1;
  59
  60         *ret = m;
  61
  62         return 0;
  63 }
  64
  65 tlv_packet *sd_lldp_packet_ref(tlv_packet *m) {
  66
  67         if (!m)
  68                 return NULL;
  69
  70         assert(m->n_ref > 0);
  71         m->n_ref++;
  72
  73         return m;
  74 }
  75
  76 tlv_packet *sd_lldp_packet_unref(tlv_packet *m) {
  77         tlv_section *s, *n;
  78
  79         if (!m)
  80                 return NULL;
  81
  82         assert(m->n_ref > 0);
  83         m->n_ref--;
  84
  85         if (m->n_ref > 0)
  86                 return m;
  87
  88         LIST_FOREACH_SAFE(section, s, n, m->sections)
  89                 tlv_section_free(s);
  90
  91         free(m);
  92         return NULL;
  93 }
  94
  95 int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
  96         uint8_t *p;
  97
  98         assert_return(m, -EINVAL);
  99         assert_return(data, -EINVAL);
 100         assert_return(data_length, -EINVAL);
 101
 102         if (m->length + data_length > ETHER_MAX_LEN)
 103                 return -ENOMEM;
 104
 105         p = m->pdu + m->length;
 106         memcpy(p, data, data_length);
 107         m->length += data_length;
 108
 109         return 0;
 110 }
 111
 112 int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
 113
 114         assert_return(m, -EINVAL);
 115
 116         return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
 117 }
 118
 119 int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
 120         uint16_t type;
 121
 122         assert_return(m, -EINVAL);
 123
 124         type = htons(data);
 125
 126         return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
 127 }
 128
 129 int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
 130         uint32_t type;
 131
 132         assert_return(m, -EINVAL);
 133
 134         type = htonl(data);
 135
 136         return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
 137 }
 138
 139 int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
 140
 141         assert_return(m, -EINVAL);
 142
 143         return tlv_packet_append_bytes(m, data, size);
 144 }
 145
 146 int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
 147
 148         assert_return(m, -EINVAL);
 149
 150         m->container_pos = m->pdu + m->length;
 151
 152         return tlv_packet_append_u16(m, type << 9);
 153 }
 154
 155 int lldp_tlv_packet_close_container(tlv_packet *m) {
 156         uint16_t type;
 157
 158         assert_return(m, -EINVAL);
 159         assert_return(m->container_pos, -EINVAL);
 160
 161         memcpy(&type, m->container_pos, sizeof(uint16_t));
 162
 163         type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
 164         memcpy(m->container_pos, &type, sizeof(uint16_t));
 165
 166         return 0;
 167 }
 168
 169 static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
 170
 171         assert_return(m->read_pos, -EINVAL);
 172
 173         *data = m->read_pos;
 174
 175         return 0;
 176 }
 177
 178 int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
 179         void *val = NULL;
 180         int r;
 181
 182         assert_return(m, -EINVAL);
 183
 184         r = tlv_packet_read_internal(m->container,  &val);
 185         if (r < 0)
 186                 return r;
 187
 188         memcpy(data, val, sizeof(uint8_t));
 189
 190         m->container->read_pos ++;
 191
 192         return 0;
 193 }
 194
 195 int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
 196         uint16_t t;
 197         void *val = NULL;
 198         int r;
 199
 200         assert_return(m, -EINVAL);
 201
 202         r = tlv_packet_read_internal(m->container, &val);
 203         if (r < 0)
 204                 return r;
 205
 206         memcpy(&t, val, sizeof(uint16_t));
 207         *data = ntohs(t);
 208
 209         m->container->read_pos += 2;
 210
 211         return 0;
 212 }
 213
 214 int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
 215         uint32_t t;
 216         void *val;
 217         int r;
 218
 219         assert_return(m, -EINVAL);
 220
 221         r = tlv_packet_read_internal(m->container, &val);
 222         if (r < 0)
 223                 return r;
 224
 225         memcpy(&t, val, sizeof(uint32_t));
 226         *data = ntohl(t);
 227
 228         m->container->read_pos += 4;
 229
 230         return r;
 231 }
 232
 233 int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
 234         void *val = NULL;
 235         int r;
 236
 237         assert_return(m, -EINVAL);
 238
 239         r = tlv_packet_read_internal(m->container, &val);
 240         if (r < 0)
 241                 return r;
 242
 243         *data = (char *) val;
 244         *data_length = m->container->data + m->container->length - m->container->read_pos;
 245
 246         m->container->read_pos += *data_length;
 247
 248         return 0;
 249 }
 250
 251 int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
 252         void *val = NULL;
 253         int r;
 254
 255         assert_return(m, -EINVAL);
 256
 257         r = tlv_packet_read_internal(m->container, &val);
 258         if (r < 0)
 259                 return r;
 260
 261         *data = (uint8_t *) val;
 262         *data_length = m->container->data + m->container->length - m->container->read_pos;
 263
 264         m->container->read_pos += *data_length;
 265
 266         return 0;
 267 }
 268
 269 /* parse raw TLV packet */
 270 int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
 271         tlv_section *section, *tail;
 272         uint16_t t, l;
 273         uint8_t *p;
 274         int r;
 275
 276         assert_return(m, -EINVAL);
 277         assert_return(size, -EINVAL);
 278
 279         p = m->pdu;
 280
 281         /* extract Ethernet header */
 282         memcpy(&m->mac, p, ETH_ALEN);
 283         p += sizeof(struct ether_header);
 284
 285         for (l = 0; l <= size; ) {
 286                 r = tlv_section_new(&section);
 287                 if (r < 0)
 288                         return r;
 289
 290                 memcpy(&t, p, sizeof(uint16_t));
 291
 292                 section->type = ntohs(t) >> 9;
 293                 section->length = ntohs(t) & 0x01ff;
 294
 295                 if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
 296                         tlv_section_free(section);
 297                         break;
 298                 }
 299
 300                 p += 2;
 301
 302                 if (section->type == LLDP_TYPE_PRIVATE &&
 303                     section->length >= LLDP_OUI_LEN + 1) {
 304                         section->oui = p;
 305                         p += LLDP_OUI_LEN;
 306                         section->subtype = *p++;
 307
 308                         section->length -= LLDP_OUI_LEN + 1;
 309                         l += LLDP_OUI_LEN + 1;
 310                 }
 311
 312                 section->data = p;
 313
 314                 LIST_FIND_TAIL(section, m->sections, tail);
 315                 LIST_INSERT_AFTER(section, m->sections, tail, section);
 316
 317                 p += section->length;
 318                 l += (section->length + 2);
 319         }
 320
 321         return 0;
 322 }
 323
 324 int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
 325         tlv_section *s;
 326
 327         assert_return(m, -EINVAL);
 328         assert_return(type != LLDP_TYPE_PRIVATE, -EINVAL);
 329
 330         LIST_FOREACH(section, s, m->sections)
 331                 if (s->type == type)
 332                         break;
 333         if (!s)
 334                 return -1;
 335
 336         m->container = s;
 337
 338         m->container->read_pos = s->data;
 339         if (!m->container->read_pos) {
 340                 m->container = NULL;
 341                 return -1;
 342         }
 343
 344         return 0;
 345 }
 346
 347 int lldp_tlv_packet_enter_container_oui(tlv_packet *m, const uint8_t *oui, uint8_t subtype) {
 348         tlv_section *s;
 349
 350         assert_return(m, -EINVAL);
 351         assert_return(oui, -EINVAL);
 352
 353         LIST_FOREACH(section, s, m->sections) {
 354                 if (s->type == LLDP_TYPE_PRIVATE &&
 355                     s->oui &&
 356                     s->subtype == subtype &&
 357                     !memcmp(s->oui, oui, LLDP_OUI_LEN))
 358                         break;
 359         }
 360
 361         if (!s)
 362                 return -1;
 363
 364         m->container = s;
 365
 366         m->container->read_pos = s->data;
 367         if (!m->container->read_pos) {
 368                 m->container = NULL;
 369                 return -1;
 370         }
 371
 372         return 0;
 373 }
 374
 375 int lldp_tlv_packet_exit_container(tlv_packet *m) {
 376         assert_return(m, -EINVAL);
 377
 378         m->container = 0;
 379
 380         return 0;
 381 }
 382
 383 static int lldp_tlv_packet_read_u16_tlv(tlv_packet *tlv, uint16_t type, uint16_t *value) {
 384         int r, r2;
 385
 386         assert_return(tlv, -EINVAL);
 387
 388         r = lldp_tlv_packet_enter_container(tlv, type);
 389         if (r < 0)
 390                 goto out;
 391
 392         r = tlv_packet_read_u16(tlv, value);
 393         r2 = lldp_tlv_packet_exit_container(tlv);
 394
 395  out:
 396         return r < 0 ? r : r2;
 397 }
 398
 399 static int lldp_tlv_packet_read_string_tlv(tlv_packet *tlv, uint16_t type, char **data, uint16_t *length) {
 400         char *s;
 401         int r, r2;
 402
 403         assert_return(tlv, -EINVAL);
 404
 405         r = lldp_tlv_packet_enter_container(tlv, type);
 406         if (r < 0)
 407                 return r;
 408
 409         r = tlv_packet_read_string(tlv, &s, length);
 410         if (r < 0)
 411                 goto out;
 412
 413         *data = (char *) s;
 414
 415  out:
 416         r2 = lldp_tlv_packet_exit_container(tlv);
 417
 418         return r < 0 ? r : r2;
 419 }
 420
 421 int sd_lldp_packet_read_chassis_id(tlv_packet *tlv,
 422                                    uint8_t *type,
 423                                    uint8_t **data,
 424                                    uint16_t *length) {
 425         uint8_t subtype;
 426         int r, r2;
 427
 428         assert_return(tlv, -EINVAL);
 429
 430         r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_CHASSIS_ID);
 431         if (r < 0)
 432                 goto out2;
 433
 434         r = tlv_packet_read_u8(tlv, &subtype);
 435         if (r < 0)
 436                 goto out1;
 437
 438         switch (subtype) {
 439         case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS:
 440
 441                 r = tlv_packet_read_bytes(tlv, data, length);
 442                 if (r < 0)
 443                         goto out1;
 444
 445                 break;
 446         default:
 447                 r = -EOPNOTSUPP;
 448                 break;
 449         }
 450
 451         *type = subtype;
 452
 453  out1:
 454         r2 = lldp_tlv_packet_exit_container(tlv);
 455
 456  out2:
 457         return r < 0 ? r : r2;
 458 }
 459
 460 int sd_lldp_packet_read_port_id(tlv_packet *tlv,
 461                                 uint8_t *type,
 462                                 uint8_t **data,
 463                                 uint16_t *length) {
 464         uint8_t subtype;
 465         char *s;
 466         int r, r2;
 467
 468         assert_return(tlv, -EINVAL);
 469
 470         r = lldp_tlv_packet_enter_container(tlv, LLDP_TYPE_PORT_ID);
 471         if (r < 0)
 472                 goto out2;
 473
 474         r = tlv_packet_read_u8(tlv, &subtype);
 475         if (r < 0)
 476                 goto out1;
 477
 478         switch (subtype) {
 479         case LLDP_PORT_SUBTYPE_PORT_COMPONENT:
 480         case LLDP_PORT_SUBTYPE_INTERFACE_ALIAS:
 481         case LLDP_PORT_SUBTYPE_INTERFACE_NAME:
 482         case LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED:
 483
 484                 r = tlv_packet_read_string(tlv, &s, length);
 485                 if (r < 0)
 486                         goto out1;
 487
 488                 *data = (uint8_t *) s;
 489
 490                 break;
 491         case LLDP_PORT_SUBTYPE_MAC_ADDRESS:
 492
 493                 r = tlv_packet_read_bytes(tlv, data, length);
 494                 if (r < 0)
 495                         goto out1;
 496
 497                 break;
 498         default:
 499                 r = -EOPNOTSUPP;
 500                 break;
 501         }
 502
 503         *type = subtype;
 504
 505  out1:
 506         r2 = lldp_tlv_packet_exit_container(tlv);
 507
 508  out2:
 509         return r < 0 ? r : r2;
 510 }
 511
 512 int sd_lldp_packet_read_ttl(tlv_packet *tlv, uint16_t *ttl) {
 513         return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_TTL, ttl);
 514 }
 515
 516 int sd_lldp_packet_read_system_name(tlv_packet *tlv,
 517                                     char **data,
 518                                     uint16_t *length) {
 519         return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_NAME, data, length);
 520 }
 521
 522 int sd_lldp_packet_read_system_description(tlv_packet *tlv,
 523                                            char **data,
 524                                            uint16_t *length) {
 525         return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_SYSTEM_DESCRIPTION, data, length);
 526 }
 527
 528 int sd_lldp_packet_read_port_description(tlv_packet *tlv,
 529                                          char **data,
 530                                          uint16_t *length) {
 531         return lldp_tlv_packet_read_string_tlv(tlv, LLDP_TYPE_PORT_DESCRIPTION, data, length);
 532 }
 533
 534 int sd_lldp_packet_read_system_capability(tlv_packet *tlv, uint16_t *data) {
 535         return lldp_tlv_packet_read_u16_tlv(tlv, LLDP_TYPE_SYSTEM_CAPABILITIES, data);
 536 }
 537
 538 int sd_lldp_packet_read_port_vlan_id(tlv_packet *tlv, uint16_t *id) {
 539         int r, r2;
 540
 541         assert_return(tlv, -EINVAL);
 542
 543         r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID);
 544         if (r < 0)
 545                 goto out;
 546
 547         r = tlv_packet_read_u16(tlv, id);
 548         r2 = lldp_tlv_packet_exit_container(tlv);
 549
 550  out:
 551         return r < 0 ? r : r2;
 552 }
 553
 554 int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet *tlv, uint8_t *flags, uint16_t *id) {
 555         int r, r2;
 556
 557         assert_return(tlv, -EINVAL);
 558
 559         r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID);
 560         if (r < 0)
 561                 goto out;
 562
 563         r = tlv_packet_read_u8(tlv, flags);
 564         if (r >= 0)
 565                 r = tlv_packet_read_u16(tlv, id);
 566
 567         r2 = lldp_tlv_packet_exit_container(tlv);
 568
 569  out:
 570         return r < 0 ? r : r2;
 571 }
 572
 573 int sd_lldp_packet_read_vlan_name(tlv_packet *tlv, uint16_t *vlan_id, char **name, uint16_t *length) {
 574         int r, r2;
 575         uint8_t len = 0;
 576
 577         assert_return(tlv, -EINVAL);
 578
 579         r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME);
 580         if (r < 0)
 581                 goto out;
 582
 583         r = tlv_packet_read_u16(tlv, vlan_id);
 584         if (r >= 0)
 585                 r = tlv_packet_read_u8(tlv, &len);
 586         if (r >= 0)
 587                 r = tlv_packet_read_string(tlv, name, length);
 588
 589         if (r >= 0 && len < *length)
 590                 *length = len;
 591
 592         r2 = lldp_tlv_packet_exit_container(tlv);
 593
 594  out:
 595         return r < 0 ? r : r2;
 596 }
 597
 598 int sd_lldp_packet_read_management_vid(tlv_packet *tlv, uint16_t *id) {
 599         int r, r2;
 600
 601         assert_return(tlv, -EINVAL);
 602
 603         r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID);
 604         if (r < 0)
 605                 goto out;
 606
 607         r = tlv_packet_read_u16(tlv, id);
 608         r2 = lldp_tlv_packet_exit_container(tlv);
 609
 610  out:
 611         return r < 0 ? r : r2;
 612 }
 613
 614 int sd_lldp_packet_read_link_aggregation(sd_lldp_packet *tlv, uint8_t *status, uint32_t *id) {
 615         int r, r2;
 616
 617         assert_return(tlv, -EINVAL);
 618
 619         r = lldp_tlv_packet_enter_container_oui(tlv, LLDP_OUI_802_1, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION);
 620         if (r < 0)
 621                 goto out;
 622
 623         r = tlv_packet_read_u8(tlv, status);
 624         if (r >= 0)
 625                 r = tlv_packet_read_u32(tlv, id);
 626
 627         r2 = lldp_tlv_packet_exit_container(tlv);
 628
 629  out:
 630         return r < 0 ? r : r2;
 631 }
 632
 633 int sd_lldp_packet_get_destination_type(tlv_packet *tlv, int *dest) {
 634         assert_return(tlv, -EINVAL);
 635         assert_return(dest, -EINVAL);
 636
 637         /* 802.1AB-2009, Table 7-1 */
 638         if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_BRIDGE, ETH_ALEN))
 639                 *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE;
 640         else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_NON_TPMR_BRIDGE, ETH_ALEN))
 641                 *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE;
 642         else if (!memcmp(&tlv->mac, LLDP_MAC_NEAREST_CUSTOMER_BRIDGE, ETH_ALEN))
 643                 *dest = SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE;
 644         else
 645                 return -EINVAL;
 646
 647         return 0;
 648 }