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>
29 int tlv_section_new(tlv_section **ret) {
32 s = new0(tlv_section, 1);
41 void tlv_section_free(tlv_section *m) {
49 int tlv_packet_new(tlv_packet **ret) {
52 m = new0(tlv_packet, 1);
56 LIST_HEAD_INIT(m->sections);
63 void tlv_packet_free(tlv_packet *m) {
69 LIST_FOREACH_SAFE(section, s, n, m->sections)
75 int tlv_packet_append_bytes(tlv_packet *m, const void *data, size_t data_length) {
78 assert_return(m, -EINVAL);
79 assert_return(data, -EINVAL);
80 assert_return(data_length, -EINVAL);
82 if (m->length + data_length > ETHER_MAX_LEN)
85 p = m->pdu + m->length;
86 memcpy(p, data, data_length);
87 m->length += data_length;
92 int tlv_packet_append_u8(tlv_packet *m, uint8_t data) {
94 assert_return(m, -EINVAL);
96 return tlv_packet_append_bytes(m, &data, sizeof(uint8_t));
99 int tlv_packet_append_u16(tlv_packet *m, uint16_t data) {
102 assert_return(m, -EINVAL);
106 return tlv_packet_append_bytes(m, &type, sizeof(uint16_t));
109 int tlv_packet_append_u32(tlv_packet *m, uint32_t data) {
112 assert_return(m, -EINVAL);
116 return tlv_packet_append_bytes(m, &type, sizeof(uint32_t));
119 int tlv_packet_append_string(tlv_packet *m, char *data, uint16_t size) {
121 assert_return(m, -EINVAL);
123 return tlv_packet_append_bytes(m, data, size);
126 int lldp_tlv_packet_open_container(tlv_packet *m, uint16_t type) {
128 assert_return(m, -EINVAL);
130 m->container_pos = m->pdu + m->length;
132 return tlv_packet_append_u16(m, type << 9);
135 int lldp_tlv_packet_close_container(tlv_packet *m) {
138 assert_return(m, -EINVAL);
139 assert_return(m->container_pos, -EINVAL);
141 memcpy(&type, m->container_pos, sizeof(uint16_t));
143 type |= htons(((m->pdu + m->length) - (m->container_pos + 2)) & 0x01ff);
144 memcpy(m->container_pos, &type, sizeof(uint16_t));
149 static inline int tlv_packet_read_internal(tlv_section *m, void **data) {
151 assert_return(m->read_pos, -EINVAL);
158 int tlv_packet_read_u8(tlv_packet *m, uint8_t *data) {
162 assert_return(m, -EINVAL);
164 r = tlv_packet_read_internal(m->container, &val);
168 memcpy(data, val, sizeof(uint8_t));
170 m->container->read_pos ++;
175 int tlv_packet_read_u16(tlv_packet *m, uint16_t *data) {
180 assert_return(m, -EINVAL);
182 r = tlv_packet_read_internal(m->container, &val);
186 memcpy(&t, val, sizeof(uint16_t));
189 m->container->read_pos += 2;
194 int tlv_packet_read_u32(tlv_packet *m, uint32_t *data) {
199 assert_return(m, -EINVAL);
201 r = tlv_packet_read_internal(m->container, &val);
205 memcpy(&t, val, sizeof(uint32_t));
208 m->container->read_pos += 4;
213 int tlv_packet_read_string(tlv_packet *m, char **data, uint16_t *data_length) {
217 assert_return(m, -EINVAL);
219 r = tlv_packet_read_internal(m->container, &val);
223 *data = (char *) val;
224 *data_length = m->container->length;
226 m->container->read_pos += m->container->length;
231 int tlv_packet_read_bytes(tlv_packet *m, uint8_t **data, uint16_t *data_length) {
235 assert_return(m, -EINVAL);
237 r = tlv_packet_read_internal(m->container, &val);
241 *data = (uint8_t *) val;
242 *data_length = m->container->length;
244 m->container->read_pos += m->container->length;
249 /* parse raw TLV packet */
250 int tlv_packet_parse_pdu(tlv_packet *m, uint16_t size) {
251 tlv_section *section, *tail;
256 assert_return(m, -EINVAL);
257 assert_return(size, -EINVAL);
261 /* extract ethernet herader */
262 memcpy(&m->mac, p, ETH_ALEN);
263 p += sizeof(struct ether_header);
265 for (l = 0; l <= size; ) {
266 r = tlv_section_new(§ion);
270 memcpy(&t, p, sizeof(uint16_t));
272 section->type = ntohs(t) >> 9;
273 section->length = ntohs(t) & 0x01ff;
275 if (section->type == LLDP_TYPE_END || section->type >=_LLDP_TYPE_MAX) {
276 tlv_section_free(section);
283 LIST_FIND_TAIL(section, m->sections, tail);
284 LIST_INSERT_AFTER(section, m->sections, tail, section);
286 p += section->length;
287 l += (section->length + 2);
293 int lldp_tlv_packet_enter_container(tlv_packet *m, uint16_t type) {
296 assert_return(m, -EINVAL);
298 LIST_FOREACH(section, s, m->sections)
306 m->container->read_pos = s->data;
307 if (!m->container->read_pos) {
315 int lldp_tlv_packet_exit_container(tlv_packet *m) {
316 assert_return(m, -EINVAL);