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
);
396 return r
< 0 ? r
: r2
;
399 static int lldp_tlv_packet_read_string_tlv(tlv_packet
*tlv
, uint16_t type
, char **data
, uint16_t *length
) {
403 assert_return(tlv
, -EINVAL
);
405 r
= lldp_tlv_packet_enter_container(tlv
, type
);
409 r
= tlv_packet_read_string(tlv
, &s
, length
);
416 r2
= lldp_tlv_packet_exit_container(tlv
);
418 return r
< 0 ? r
: r2
;
421 int sd_lldp_packet_read_chassis_id(tlv_packet
*tlv
,
428 assert_return(tlv
, -EINVAL
);
430 r
= lldp_tlv_packet_enter_container(tlv
, LLDP_TYPE_CHASSIS_ID
);
434 r
= tlv_packet_read_u8(tlv
, &subtype
);
439 case LLDP_CHASSIS_SUBTYPE_MAC_ADDRESS
:
441 r
= tlv_packet_read_bytes(tlv
, data
, length
);
454 r2
= lldp_tlv_packet_exit_container(tlv
);
457 return r
< 0 ? r
: r2
;
460 int sd_lldp_packet_read_port_id(tlv_packet
*tlv
,
468 assert_return(tlv
, -EINVAL
);
470 r
= lldp_tlv_packet_enter_container(tlv
, LLDP_TYPE_PORT_ID
);
474 r
= tlv_packet_read_u8(tlv
, &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
:
484 r
= tlv_packet_read_string(tlv
, &s
, length
);
488 *data
= (uint8_t *) s
;
491 case LLDP_PORT_SUBTYPE_MAC_ADDRESS
:
493 r
= tlv_packet_read_bytes(tlv
, data
, length
);
506 r2
= lldp_tlv_packet_exit_container(tlv
);
509 return r
< 0 ? r
: r2
;
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
);
516 int sd_lldp_packet_read_system_name(tlv_packet
*tlv
,
519 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_SYSTEM_NAME
, data
, length
);
522 int sd_lldp_packet_read_system_description(tlv_packet
*tlv
,
525 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_SYSTEM_DESCRIPTION
, data
, length
);
528 int sd_lldp_packet_read_port_description(tlv_packet
*tlv
,
531 return lldp_tlv_packet_read_string_tlv(tlv
, LLDP_TYPE_PORT_DESCRIPTION
, data
, length
);
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
);
538 int sd_lldp_packet_read_port_vlan_id(tlv_packet
*tlv
, uint16_t *id
) {
541 assert_return(tlv
, -EINVAL
);
543 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_PORT_VLAN_ID
);
547 r
= tlv_packet_read_u16(tlv
, id
);
548 r2
= lldp_tlv_packet_exit_container(tlv
);
551 return r
< 0 ? r
: r2
;
554 int sd_lldp_packet_read_port_protocol_vlan_id(sd_lldp_packet
*tlv
, uint8_t *flags
, uint16_t *id
) {
557 assert_return(tlv
, -EINVAL
);
559 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_PORT_PROTOCOL_VLAN_ID
);
563 r
= tlv_packet_read_u8(tlv
, flags
);
565 r
= tlv_packet_read_u16(tlv
, id
);
567 r2
= lldp_tlv_packet_exit_container(tlv
);
570 return r
< 0 ? r
: r2
;
573 int sd_lldp_packet_read_vlan_name(tlv_packet
*tlv
, uint16_t *vlan_id
, char **name
, uint16_t *length
) {
577 assert_return(tlv
, -EINVAL
);
579 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_VLAN_NAME
);
583 r
= tlv_packet_read_u16(tlv
, vlan_id
);
585 r
= tlv_packet_read_u8(tlv
, &len
);
587 r
= tlv_packet_read_string(tlv
, name
, length
);
589 if (r
>= 0 && len
< *length
)
592 r2
= lldp_tlv_packet_exit_container(tlv
);
595 return r
< 0 ? r
: r2
;
598 int sd_lldp_packet_read_management_vid(tlv_packet
*tlv
, uint16_t *id
) {
601 assert_return(tlv
, -EINVAL
);
603 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_MANAGEMENT_VID
);
607 r
= tlv_packet_read_u16(tlv
, id
);
608 r2
= lldp_tlv_packet_exit_container(tlv
);
611 return r
< 0 ? r
: r2
;
614 int sd_lldp_packet_read_link_aggregation(sd_lldp_packet
*tlv
, uint8_t *status
, uint32_t *id
) {
617 assert_return(tlv
, -EINVAL
);
619 r
= lldp_tlv_packet_enter_container_oui(tlv
, LLDP_OUI_802_1
, LLDP_OUI_SUBTYPE_802_1_LINK_AGGREGATION
);
623 r
= tlv_packet_read_u8(tlv
, status
);
625 r
= tlv_packet_read_u32(tlv
, id
);
627 r2
= lldp_tlv_packet_exit_container(tlv
);
630 return r
< 0 ? r
: r2
;
633 int sd_lldp_packet_get_destination_type(tlv_packet
*tlv
, int *dest
) {
634 assert_return(tlv
, -EINVAL
);
635 assert_return(dest
, -EINVAL
);
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
;