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