?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
ring_tx: handle EINTR from sendto
[netsniff-ng-new.git] / proto_lldp.c
blob3d1e8bf6d1fae4d8b7f88ecfc280fabfc115200d
1 /*
2 * netsniff-ng - the packet sniffing beast
3 * Copyright 2012, 2013 Tobias Klauser <tklauser@distanz.ch>
4 * Subject to the GPL, version 2.
5 */
6
7 #include <stdint.h>
8 #include <arpa/inet.h> /* for inet_ntop() */
9 #include <netinet/in.h> /* for ntohs()/ntohl() */
10
11 #include "built_in.h"
12 #include "lookup.h"
13 #include "pkt_buff.h"
14 #include "proto.h"
15 #include "protos.h"
16
17 #define EXTRACT_16BIT(x) ntohs(*((uint16_t *) (x)))
18 #define EXTRACT_32BIT(x) ntohl(*((uint32_t *) (x)))
19
20 #define LLDP_TLV_TYPE(tlv) (((tlv) & 0xFE00) >> 9)
21 #define LLDP_TLV_LENGTH(tlv) ((tlv) & 0x01FF)
22
23 /*
24 * LLDP TLV types
25 */
26 #define LLDP_TLV_END 0
27 #define LLDP_TLV_CHASSIS_ID 1
28 #define LLDP_TLV_PORT_ID 2
29 #define LLDP_TLV_TTL 3
30 #define LLDP_TLV_PORT_DESC 4
31 #define LLDP_TLV_SYSTEM_NAME 5
32 #define LLDP_TLV_SYSTEM_DESC 6
33 #define LLDP_TLV_SYSTEM_CAP 7
34 #define LLDP_TLV_MGMT_ADDR 8
35 #define LLDP_TLV_ORG_SPECIFIC 127
36
37 /*
38 * Chassis ID subtypes
39 */
40 #define LLDP_CHASSIS_SUBTYPE_CHASSIS 1
41 #define LLDP_CHASSIS_SUBTYPE_IF_ALIAS 2
42 #define LLDP_CHASSIS_SUBTYPE_PORT 3
43 #define LLDP_CHASSIS_SUBTYPE_MAC_ADDR 4
44 #define LLDP_CHASSIS_SUBTYPE_NET_ADDR 5
45 #define LLDP_CHASSIS_SUBTYPE_IF_NAME 6
46 #define LLDP_CHASSIS_SUBTYPE_LOCAL 7
47
48 /*
49 * Port ID subtypes
50 */
51 #define LLDP_PORT_SUBTYPE_IF_ALIAS 1
52 #define LLDP_PORT_SUBTYPE_PORT_COMP 2
53 #define LLDP_PORT_SUBTYPE_MAC_ADDR 3
54 #define LLDP_PORT_SUBTYPE_NET_ADDR 4
55 #define LLDP_PORT_SUBTYPE_IF_NAME 5
56 #define LLDP_PORT_SUBTYPE_AGENT_CIRC_ID 6
57 #define LLDP_PORT_SUBTYPE_LOCAL 7
58
59 /*
60 * System capabilits bit masks
61 */
62 #define LLDP_SYSTEM_CAP_OTHER (1 << 0)
63 #define LLDP_SYSTEM_CAP_REPEATER (1 << 1)
64 #define LLDP_SYSTEM_CAP_BRIDGE (1 << 2)
65 #define LLDP_SYSTEM_CAP_WLAN_AP (1 << 3)
66 #define LLDP_SYSTEM_CAP_ROUTER (1 << 4)
67 #define LLDP_SYSTEM_CAP_TELEPHONE (1 << 5)
68 #define LLDP_SYSTEM_CAP_DOCSIS (1 << 6)
69 #define LLDP_SYSTEM_CAP_STATION_ONLY (1 << 7)
70
71 /*
72 * Interface number subtypes (for Management address TLV)
73 */
74 #define LLDP_IFACE_NUM_SUBTYPE_UNKNOWN 1
75 #define LLDP_IFACE_NUM_SUBTYPE_IF_INDEX 2
76 #define LLDP_IFACE_NUM_SUBTYPE_SYS_PORT 3
77
78 /*
79 * IANA address family numbers (only the ones we actually use)
80 * http://www.iana.org/assignments/address-family-numbers/address-family-numbers.txt
81 *
82 * TODO: Move these into own header if there are other users?
83 */
84 #define IANA_AF_IPV4 1
85 #define IANA_AF_IPV6 2
86 #define IANA_AF_802 6
87
88 static int lldp_print_net_addr(const uint8_t *addr, size_t addrlen)
89 {
90 uint8_t af;
91 char buf[64] = {0};
92
93 if (addrlen < 1)
94 return -EINVAL;
95
96 af = *addr++;
97 addrlen--;
98 switch (af) {
99 case IANA_AF_IPV4:
100 if (addrlen < 4)
101 return -EINVAL;
102 if (inet_ntop(AF_INET, addr, buf, sizeof(buf)) == NULL) {
103 tprintf("[MALFORMED ADDR]");
104 return 0;
105 }
106
107 tprintf("%s", buf);
108 break;
109 case IANA_AF_IPV6:
110 if (addrlen < 16)
111 return -EINVAL;
112 if (inet_ntop(AF_INET6, addr, buf, sizeof(buf)) == NULL) {
113 tprintf("[MALFORMED ADDR]");
114 return 0;
115 }
116
117 tprintf("%s", buf);
118 break;
119 case IANA_AF_802:
120 if (addrlen < 6)
121 return -EINVAL;
122 tprintf("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x",
123 addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
124 break;
125 default:
126 tprintf("unknown address family");
127 break;
128 }
129
130 return 0;
131 }
132
133 static inline void lldp_print_cap_one(const char *cap, unsigned int *prev)
134 {
135 tprintf("%s%s", *prev ? ", " : "", cap);
136 (*prev)++;
137 }
138
139 static void lldp_print_cap(uint16_t cap)
140 {
141 unsigned int prev = 0;
142
143 if (cap & LLDP_SYSTEM_CAP_OTHER)
144 lldp_print_cap_one("Other", &prev);
145 if (cap & LLDP_SYSTEM_CAP_REPEATER)
146 lldp_print_cap_one("Repeater", &prev);
147 if (cap & LLDP_SYSTEM_CAP_BRIDGE)
148 lldp_print_cap_one("Bridge", &prev);
149 if (cap & LLDP_SYSTEM_CAP_WLAN_AP)
150 lldp_print_cap_one("WLAN AP", &prev);
151 if (cap & LLDP_SYSTEM_CAP_ROUTER)
152 lldp_print_cap_one("Router", &prev);
153 if (cap & LLDP_SYSTEM_CAP_TELEPHONE)
154 lldp_print_cap_one("Telephone", &prev);
155 if (cap & LLDP_SYSTEM_CAP_DOCSIS)
156 lldp_print_cap_one("DOCSIS", &prev);
157 if (cap & LLDP_SYSTEM_CAP_STATION_ONLY)
158 lldp_print_cap_one("Station only", &prev);
159 }
160
161 static void lldp(struct pkt_buff *pkt)
162 {
163 unsigned int n_tlv = 0;
164 uint8_t subtype, mgmt_alen, mgmt_oidlen;
165 uint16_t tlv_hdr;
166 unsigned int tlv_type, tlv_len;
167 unsigned int len;
168 uint8_t *tlv_info_str;
169 uint16_t sys_cap, en_cap;
170 uint32_t oui;
171
172 len = pkt_len(pkt);
173 if (len == 0)
174 return;
175
176 tprintf(" [ LLDP ");
177
178 while (len >= sizeof(tlv_hdr)) {
179 uint8_t *data = pkt_pull(pkt, sizeof(tlv_hdr));
180 if (data == NULL)
181 goto out_invalid;
182
183 tlv_hdr = EXTRACT_16BIT(data);
184 tlv_type = LLDP_TLV_TYPE(tlv_hdr);
185 tlv_len = LLDP_TLV_LENGTH(tlv_hdr);
186
187 len -= sizeof(tlv_hdr);
188
189 if (tlv_type == LLDP_TLV_END && tlv_len == 0) {
190 /* Chassis ID, Port ID and TTL are mandatory */
191 if (n_tlv < 3)
192 goto out_invalid;
193 else
194 break;
195 }
196 if (len < tlv_len)
197 goto out_invalid;
198
199 switch (tlv_type) {
200 case LLDP_TLV_CHASSIS_ID:
201 /*
202 * The mandatory chassis ID shall be the first TLV and
203 * shall appear exactly once.
204 */
205 if (n_tlv != 0)
206 goto out_invalid;
207
208 tprintf("Chassis ID");
209
210 if (tlv_len < 2)
211 goto out_invalid;
212
213 tlv_info_str = pkt_pull(pkt, tlv_len);
214 if (tlv_info_str == NULL)
215 goto out_invalid;
216
217 subtype = *tlv_info_str++;
218 tprintf(" (Subtype %u => ", subtype);
219
220 switch (subtype) {
221 case LLDP_CHASSIS_SUBTYPE_MAC_ADDR:
222 if (tlv_len < 7)
223 goto out_invalid;
224
225 tprintf("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x",
226 tlv_info_str[0], tlv_info_str[1],
227 tlv_info_str[2], tlv_info_str[3],
228 tlv_info_str[4], tlv_info_str[5]);
229 break;
230 case LLDP_CHASSIS_SUBTYPE_NET_ADDR:
231 if (lldp_print_net_addr(tlv_info_str, tlv_len))
232 goto out_invalid;
233 break;
234 case LLDP_CHASSIS_SUBTYPE_CHASSIS:
235 case LLDP_CHASSIS_SUBTYPE_IF_ALIAS:
236 case LLDP_CHASSIS_SUBTYPE_PORT:
237 case LLDP_CHASSIS_SUBTYPE_IF_NAME:
238 case LLDP_CHASSIS_SUBTYPE_LOCAL:
239 tputs_safe((const char *) tlv_info_str, tlv_len - 1);
240 break;
241 default:
242 tprintf("Reserved");
243 break;
244 }
245
246 tprintf(")");
247 break;
248 case LLDP_TLV_PORT_ID:
249 /*
250 * The mandatory port ID shall be the second TLV and
251 * shall appear exactly once.
252 */
253 if (n_tlv != 1)
254 goto out_invalid;
255
256 tprintf(", Port ID");
257
258 if (tlv_len < 2)
259 goto out_invalid;
260
261 tlv_info_str = pkt_pull(pkt, tlv_len);
262 if (tlv_info_str == NULL)
263 goto out_invalid;
264
265 subtype = *tlv_info_str++;
266 tprintf(" (Subtype %u => ", subtype);
267
268 switch (subtype) {
269 case LLDP_PORT_SUBTYPE_MAC_ADDR:
270 if (tlv_len < 7)
271 goto out_invalid;
272
273 tprintf("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x",
274 tlv_info_str[0], tlv_info_str[1],
275 tlv_info_str[2], tlv_info_str[3],
276 tlv_info_str[4], tlv_info_str[5]);
277 break;
278 case LLDP_PORT_SUBTYPE_NET_ADDR:
279 if (lldp_print_net_addr(tlv_info_str, tlv_len))
280 goto out_invalid;
281 break;
282 case LLDP_PORT_SUBTYPE_IF_ALIAS:
283 case LLDP_PORT_SUBTYPE_PORT_COMP:
284 case LLDP_PORT_SUBTYPE_IF_NAME:
285 case LLDP_PORT_SUBTYPE_AGENT_CIRC_ID:
286 case LLDP_PORT_SUBTYPE_LOCAL:
287 tputs_safe((const char *) tlv_info_str, tlv_len - 1);
288 break;
289 default:
290 tprintf("Reserved");
291 break;
292 }
293
294 tprintf(")");
295 break;
296 case LLDP_TLV_TTL:
297 /*
298 * The mandatory TTL shall be the third TLV and
299 * shall appear exactly once.
300 */
301 if (n_tlv != 2)
302 goto out_invalid;
303
304 tprintf(", TTL");
305
306 if (tlv_len != 2)
307 goto out_invalid;
308
309 tlv_info_str = pkt_pull(pkt, tlv_len);
310 if (tlv_info_str == NULL)
311 goto out_invalid;
312
313 tprintf(" (%u)", EXTRACT_16BIT(tlv_info_str));
314 break;
315 case LLDP_TLV_PORT_DESC:
316 tprintf(", Port desc (");
317
318 tlv_info_str = pkt_pull(pkt, tlv_len);
319 if (tlv_info_str == NULL)
320 tprintf("none");
321 else
322 tputs_safe((const char *) tlv_info_str, tlv_len);
323
324 tprintf(")");
325 break;
326 case LLDP_TLV_SYSTEM_NAME:
327 tprintf(", Sys name (");
328
329 tlv_info_str = pkt_pull(pkt, tlv_len);
330 if (tlv_info_str == NULL)
331 tprintf("none");
332 else
333 tputs_safe((const char *) tlv_info_str, tlv_len);
334
335 tprintf(")");
336 break;
337 case LLDP_TLV_SYSTEM_DESC:
338 tprintf(", Sys desc (");
339
340 tlv_info_str = pkt_pull(pkt, tlv_len);
341 if (tlv_info_str == NULL)
342 tprintf("none");
343 else
344 tputs_safe((const char *) tlv_info_str, tlv_len);
345
346 tprintf(")");
347 break;
348 case LLDP_TLV_SYSTEM_CAP:
349 tprintf(", Sys Cap");
350
351 if (tlv_len != 4)
352 goto out_invalid;
353
354 tlv_info_str = pkt_pull(pkt, tlv_len);
355 if (tlv_info_str == NULL)
356 goto out_invalid;
357
358 sys_cap = EXTRACT_16BIT(tlv_info_str);
359 tlv_info_str += sizeof(uint16_t);
360 en_cap = EXTRACT_16BIT(tlv_info_str);
361
362 tprintf(" (");
363 lldp_print_cap(sys_cap);
364 tprintf(")");
365 tprintf(" Ena Cap (");
366 lldp_print_cap(en_cap);
367 tprintf(")");
368 break;
369 case LLDP_TLV_MGMT_ADDR:
370 tprintf(", Mgmt Addr (");
371
372 if (tlv_len < 9 || tlv_len > 167)
373 goto out_invalid;
374
375 tlv_info_str = pkt_pull(pkt, tlv_len);
376 if (tlv_info_str == NULL)
377 goto out_invalid;
378
379 mgmt_alen = *tlv_info_str;
380 tlv_info_str++;
381 if (tlv_len - 1 < mgmt_alen)
382 goto out_invalid;
383
384 if (lldp_print_net_addr(tlv_info_str, mgmt_alen))
385 goto out_invalid;
386 tlv_info_str += mgmt_alen;
387
388 tprintf(", Iface Subtype %d/", *tlv_info_str);
389 switch (*tlv_info_str) {
390 case LLDP_IFACE_NUM_SUBTYPE_IF_INDEX:
391 tprintf("ifIndex");
392 break;
393 case LLDP_IFACE_NUM_SUBTYPE_SYS_PORT:
394 tprintf("System Port Number");
395 break;
396 default:
397 tprintf("Unknown");
398 break;
399 }
400
401 tlv_info_str++;
402
403 if (tlv_len - mgmt_alen < sizeof(uint32_t))
404 goto out_invalid;
405 tprintf(", Iface Number %u", EXTRACT_32BIT(tlv_info_str));
406
407 tlv_info_str += 4;
408 mgmt_oidlen = *tlv_info_str;
409 if (tlv_len - mgmt_alen - sizeof(uint32_t) < 3 ||
410 tlv_len - mgmt_alen - sizeof(uint32_t) - 3 < mgmt_oidlen)
411 goto out_invalid;
412 if (mgmt_oidlen > 0) {
413 tprintf(", OID ");
414 tputs_safe((const char *) tlv_info_str + 1, mgmt_oidlen);
415 }
416
417 tprintf(")");
418 break;
419 case LLDP_TLV_ORG_SPECIFIC:
420 tprintf(", Org specific");
421
422 if (tlv_len < 4)
423 goto out_invalid;
424
425 tlv_info_str = pkt_pull(pkt, 4);
426 if (tlv_info_str == NULL)
427 goto out_invalid;
428
429 oui = ntohl(*((uint32_t *) tlv_info_str));
430 subtype = oui & 0xff;
431 oui >>= 8;
432 tprintf(" (OUI %s, Subtype %u)", lookup_vendor_str(oui),
433 subtype);
434
435 /* Just eat it up, we don't know how to interpret it */
436 pkt_pull(pkt, tlv_len - 4);
437 break;
438 default:
439 tprintf(", Unknown TLV %u", tlv_type);
440 pkt_pull(pkt, tlv_len);
441 break;
442 }
443
444 n_tlv++;
445 }
446
447 len -= tlv_len;
448
449 tprintf(" ]\n");
450 return;
451
452 out_invalid:
453 tprintf(" %sINVALID%s ]\n", colorize_start_full(black, red),
454 colorize_end());
455 }
456
457 static void lldp_less(struct pkt_buff *pkt)
458 {
459 unsigned int len, n_tlv = 0;
460 unsigned int tlv_type, tlv_len;
461 uint16_t tlv_hdr;
462
463 len = pkt_len(pkt);
464
465 while (len >= sizeof(tlv_hdr)) {
466 uint8_t *data = pkt_pull(pkt, sizeof(tlv_hdr));
467 if (data == NULL)
468 break;
469
470 tlv_hdr = EXTRACT_16BIT(data);
471 tlv_type = LLDP_TLV_TYPE(tlv_hdr);
472 tlv_len = LLDP_TLV_LENGTH(tlv_hdr);
473
474 n_tlv++;
475 len -= sizeof(tlv_hdr);
476
477 if (tlv_type == LLDP_TLV_END || tlv_len == 0)
478 break;
479 if (len < tlv_len)
480 break;
481
482 pkt_pull(pkt, tlv_len);
483
484 len -= tlv_len;
485 }
486
487 tprintf(" %u TLV%s", n_tlv, n_tlv == 1 ? "" : "s");
488 }
489
490 struct protocol lldp_ops = {
491 .key = 0x88cc,
492 .print_full = lldp,
493 .print_less = lldp_less,
494 };
New netsniff-ng tree