1 /*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
4 This file is part of systemd.
6 Copyright (C) 2014 Tom Gundersen
7 Copyright (C) 2014 Susant Sahani
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.
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.
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/>.
23 #include <net/ethernet.h>
24 #include <arpa/inet.h>
26 #include "alloc-util.h"
30 int tlv_section_new(tlv_section
**ret
) {
33 s
= new0(tlv_section
, 1);
42 void tlv_section_free(tlv_section
*m
) {
50 int tlv_packet_new(tlv_packet
**ret
) {
53 m
= new0(tlv_packet
, 1);
57 LIST_HEAD_INIT(m
->sections
);
65 tlv_packet
*sd_lldp_packet_ref(tlv_packet
*m
) {
76 tlv_packet
*sd_lldp_packet_unref(tlv_packet
*m
) {
88 LIST_FOREACH_SAFE(section
, s
, n
, m
->sections
)
95 int tlv_packet_append_bytes(tlv_packet
*m
, const void *data
, size_t data_length
) {
98 assert_return(m
, -EINVAL
);
99 assert_return(data
, -EINVAL
);
100 assert_return(data_length
, -EINVAL
);
102 if (m
->length
+ data_length
> ETHER_MAX_LEN
)
105 p
= m
->pdu
+ m
->length
;
106 memcpy(p
, data
, data_length
);
107 m
->length
+= data_length
;
112 int tlv_packet_append_u8(tlv_packet
*m
, uint8_t data
) {
114 assert_return(m
, -EINVAL
);
116 return tlv_packet_append_bytes(m
, &data
, sizeof(uint8_t));
119 int tlv_packet_append_u16(tlv_packet
*m
, uint16_t data
) {
122 assert_return(m
, -EINVAL
);
126 return tlv_packet_append_bytes(m
, &type
, sizeof(uint16_t));
129 int tlv_packet_append_u32(tlv_packet
*m
, uint32_t data
) {
132 assert_return(m
, -EINVAL
);
136 return tlv_packet_append_bytes(m
, &type
, sizeof(uint32_t));
139 int tlv_packet_append_string(tlv_packet
*m
, char *data
, uint16_t size
) {
141 assert_return(m
, -EINVAL
);
143 return tlv_packet_append_bytes(m
, data
, size
);
146 int lldp_tlv_packet_open_container(tlv_packet
*m
, uint16_t type
) {
148 assert_return(m
, -EINVAL
);
150 m
->container_pos
= m
->pdu
+ m
->length
;
152 return tlv_packet_append_u16(m
, type
<< 9);
155 int lldp_tlv_packet_close_container(tlv_packet
*m
) {
158 assert_return(m
, -EINVAL
);
159 assert_return(m
->container_pos
, -EINVAL
);
161 memcpy(&type
, m
->container_pos
, sizeof(uint16_t));
163 type
|= htons(((m
->pdu
+ m
->length
) - (m
->container_pos
+ 2)) & 0x01ff);
164 memcpy(m
->container_pos
, &type
, sizeof(uint16_t));
169 static inline int tlv_packet_read_internal(tlv_section
*m
, void **data
) {
171 assert_return(m
->read_pos
, -EINVAL
);
178 int tlv_packet_read_u8(tlv_packet
*m
, uint8_t *data
) {
182 assert_return(m
, -EINVAL
);
184 r
= tlv_packet_read_internal(m
->container
, &val
);
188 memcpy(data
, val
, sizeof(uint8_t));
190 m
->container
->read_pos
++;
195 int tlv_packet_read_u16(tlv_packet
*m
, uint16_t *data
) {
200 assert_return(m
, -EINVAL
);
202 r
= tlv_packet_read_internal(m
->container
, &val
);
206 memcpy(&t
, val
, sizeof(uint16_t));
209 m
->container
->read_pos
+= 2;
214 int tlv_packet_read_u32(tlv_packet
*m
, uint32_t *data
) {
219 assert_return(m
, -EINVAL
);
221 r
= tlv_packet_read_internal(m
->container
, &val
);
225 memcpy(&t
, val
, sizeof(uint32_t));
228 m
->container
->read_pos
+= 4;
233 int tlv_packet_read_string(tlv_packet
*m
, char **data
, uint16_t *data_length
) {
237 assert_return(m
, -EINVAL
);
239 r
= tlv_packet_read_internal(m
->container
, &val
);
243 *data
= (char *) val
;
244 *data_length
= m
->container
->data
+ m
->container
->length
- m
->container
->read_pos
;
246 m
->container
->read_pos
+= *data_length
;
251 int tlv_packet_read_bytes(tlv_packet
*m
, uint8_t **data
, uint16_t *data_length
) {
255 assert_return(m
, -EINVAL
);
257 r
= tlv_packet_read_internal(m
->container
, &val
);
261 *data
= (uint8_t *) val
;
262 *data_length
= m
->container
->data
+ m
->container
->length
- m
->container
->read_pos
;
264 m
->container
->read_pos
+= *data_length
;
269 /* parse raw TLV packet */
270 int tlv_packet_parse_pdu(tlv_packet
*m
, uint16_t size
) {
271 tlv_section
*section
, *tail
;
276 assert_return(m
, -EINVAL
);
277 assert_return(size
, -EINVAL
);
281 /* extract Ethernet header */
282 memcpy(&m
->mac
, p
, ETH_ALEN
);
283 p
+= sizeof(struct ether_header
);
285 for (l
= 0; l
<= size
; ) {
286 r
= tlv_section_new(§ion
);
290 memcpy(&t
, p
, sizeof(uint16_t));
292 section
->type
= ntohs(t
) >> 9;
293 section
->length
= ntohs(t
) & 0x01ff;
295 if (section
->type
== LLDP_TYPE_END
|| section
->type
>=_LLDP_TYPE_MAX
) {
296 tlv_section_free(section
);
302 if (section
->type
== LLDP_TYPE_PRIVATE
&&
303 section
->length
>= LLDP_OUI_LEN
+ 1) {
306 section
->subtype
= *p
++;
308 section
->length
-= LLDP_OUI_LEN
+ 1;
309 l
+= LLDP_OUI_LEN
+ 1;
314 LIST_FIND_TAIL(section
, m
->sections
, tail
);
315 LIST_INSERT_AFTER(section
, m
->sections
, tail
, section
);
317 p
+= section
->length
;
318 l
+= (section
->length
+ 2);
324 int lldp_tlv_packet_enter_container(tlv_packet
*m
, uint16_t type
) {
327 assert_return(m
, -EINVAL
);
328 assert_return(type
!= LLDP_TYPE_PRIVATE
, -EINVAL
);
330 LIST_FOREACH(section
, s
, m
->sections
)
338 m
->container
->read_pos
= s
->data
;
339 if (!m
->container
->read_pos
) {
347 int lldp_tlv_packet_enter_container_oui(tlv_packet
*m
, const uint8_t *oui
, uint8_t subtype
) {
350 assert_return(m
, -EINVAL
);
351 assert_return(oui
, -EINVAL
);
353 LIST_FOREACH(section
, s
, m
->sections
) {
354 if (s
->type
== LLDP_TYPE_PRIVATE
&&
356 s
->subtype
== subtype
&&
357 !memcmp(s
->oui
, oui
, LLDP_OUI_LEN
))
366 m
->container
->read_pos
= s
->data
;
367 if (!m
->container
->read_pos
) {
375 int lldp_tlv_packet_exit_container(tlv_packet
*m
) {
376 assert_return(m
, -EINVAL
);
383 static int lldp_tlv_packet_read_u16_tlv(tlv_packet
*tlv
, uint16_t type
, uint16_t *value
) {
386 assert_return(tlv
, -EINVAL
);
388 r
= lldp_tlv_packet_enter_container(tlv
, type
);
392 r
= tlv_packet_read_u16(tlv
, value
);
393 r2
= lldp_tlv_packet_exit_container(tlv
);
395 return r
< 0 ? r
: r2
;
398 static int lldp_tlv_packet_read_string_tlv(tlv_packet
*tlv
, uint16_t type
, char **data
, uint16_t *length
) {
402 assert_return(tlv
, -EINVAL
);
404 r
= lldp_tlv_packet_enter_container(tlv
, type
);
408 r
= tlv_packet_read_string(tlv
, &s
, length
);
415 r2
= lldp_tlv_packet_exit_container(tlv
);
417 return r
< 0 ? r
: r2
;
420 int sd_lldp_packet_read_chassis_id(tlv_packet
*tlv
,
427 assert_return(tlv
, -EINVAL
);
429 r
= lldp_tlv_packet_enter_container(tlv
, LLDP_TYPE_CHASSIS_ID
);
433 r
= tlv_packet_read_u8(tlv
, &subtype
);
438 case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS
:
440 r
= tlv_packet_read_bytes(tlv
, data
, length
);
453 r2
= lldp_tlv_packet_exit_container(tlv
);
455 return r
< 0 ? r
: r2
;
458 int sd_lldp_packet_read_port_id(tlv_packet
*tlv
,
466 assert_return(tlv
, -EINVAL
);
468 r
= lldp_tlv_packet_enter_container(tlv
, LLDP_TYPE_PORT_ID
);
472 r
= tlv_packet_read_u8(tlv
, &subtype
);
477 case LLDP_PORT_SUBTYPE_PORT_COMPONENT
:
478 case LLDP_PORT_SUBTYPE_INTERFACE_ALIAS
:
479 case LLDP_PORT_SUBTYPE_INTERFACE_NAME
:
480 case LLDP_PORT_SUBTYPE_LOCALLY_ASSIGNED
:
482 r
= tlv_packet_read_string(tlv
, &s
, length
);
486 *data
= (uint8_t *) s
;
489 case LLDP_PORT_SUBTYPE_MAC_ADDRESS
:
491 r
= tlv_packet_read_bytes(tlv
, data
, length
);
504 r2
= lldp_tlv_packet_exit_container(tlv
);
506 return r
< 0 ? r
: r2
;
509 int sd_lldp_packet_read_ttl(tlv_packet
*tlv
, uint16_t *ttl
) {
510 return lldp_tlv_packet_read_u16_tlv(tlv
, LLDP_TYPE_TTL
, ttl
);
513 int sd_lldp_packet_read_system_name(tlv_packet
*tlv
,
516 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_SYSTEM_NAME
, data
, length
);
519 int sd_lldp_packet_read_system_description(tlv_packet
*tlv
,
522 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_SYSTEM_DESCRIPTION
, data
, length
);
525 int sd_lldp_packet_read_port_description(tlv_packet
*tlv
,
528 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_PORT_DESCRIPTION
, data
, length
);
531 int sd_lldp_packet_read_system_capability(tlv_packet
*tlv
, uint16_t *data
) {
532 return lldp_tlv_packet_read_u16_tlv(tlv
, LLDP_TYPE_SYSTEM_CAPABILITIES
, data
);
535 int sd_lldp_packet_read_port_vlan_id(tlv_packet
*tlv
, uint16_t *id
) {
538 assert_return(tlv
, -EINVAL
);
540 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID
);
544 r
= tlv_packet_read_u16(tlv
, id
);
545 r2
= lldp_tlv_packet_exit_container(tlv
);
547 return r
< 0 ? r
: r2
;
550 int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet
*tlv
, uint8_t *flags
, uint16_t *id
) {
553 assert_return(tlv
, -EINVAL
);
555 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID
);
559 r
= tlv_packet_read_u8(tlv
, flags
);
561 r
= tlv_packet_read_u16(tlv
, id
);
563 r2
= lldp_tlv_packet_exit_container(tlv
);
565 return r
< 0 ? r
: r2
;
568 int sd_lldp_packet_read_vlan_name(tlv_packet
*tlv
, uint16_t *vlan_id
, char **name
, uint16_t *length
) {
572 assert_return(tlv
, -EINVAL
);
574 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME
);
578 r
= tlv_packet_read_u16(tlv
, vlan_id
);
580 r
= tlv_packet_read_u8(tlv
, &len
);
582 r
= tlv_packet_read_string(tlv
, name
, length
);
584 if (r
>= 0 && len
< *length
)
587 r2
= lldp_tlv_packet_exit_container(tlv
);
589 return r
< 0 ? r
: r2
;
592 int sd_lldp_packet_read_management_vid(tlv_packet
*tlv
, uint16_t *id
) {
595 assert_return(tlv
, -EINVAL
);
597 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID
);
601 r
= tlv_packet_read_u16(tlv
, id
);
602 r2
= lldp_tlv_packet_exit_container(tlv
);
604 return r
< 0 ? r
: r2
;
607 int sd_lldp_packet_read_link_aggregation(sd_lldp_packet
*tlv
, uint8_t *status
, uint32_t *id
) {
610 assert_return(tlv
, -EINVAL
);
612 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION
);
616 r
= tlv_packet_read_u8(tlv
, status
);
618 r
= tlv_packet_read_u32(tlv
, id
);
620 r2
= lldp_tlv_packet_exit_container(tlv
);
622 return r
< 0 ? r
: r2
;
625 int sd_lldp_packet_get_destination_type(tlv_packet
*tlv
, int *dest
) {
626 assert_return(tlv
, -EINVAL
);
627 assert_return(dest
, -EINVAL
);
629 /* 802.1AB-2009, Table 7-1 */
630 if (!memcmp(&tlv
->mac
, LLDP_MAC_NEAREST_BRIDGE
, ETH_ALEN
))
631 *dest
= SD_LLDP_DESTINATION_TYPE_NEAREST_BRIDGE
;
632 else if (!memcmp(&tlv
->mac
, LLDP_MAC_NEAREST_NON_TPMR_BRIDGE
, ETH_ALEN
))
633 *dest
= SD_LLDP_DESTINATION_TYPE_NEAREST_NON_TPMR_BRIDGE
;
634 else if (!memcmp(&tlv
->mac
, LLDP_MAC_NEAREST_CUSTOMER_BRIDGE
, ETH_ALEN
))
635 *dest
= SD_LLDP_DESTINATION_TYPE_NEAREST_CUSTOMER_BRIDGE
;