]> git.proxmox.com Git - systemd.git/blob - src/libsystemd-network/lldp-tlv.c
Merge tag 'upstream/229'
[systemd.git] / 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 }