Context Navigation

source: lib/protocols_l2.c @ c876f29

develop

Last change on this file since c876f29 was c876f29, checked in by Jacob Van Walraven <jcv9@…>, 2 years ago
Resolve merge conflict attempt 2
Property mode set to `100644`
File size: 28.0 KB

Line
1	/*
2	*
3	* Copyright (c) 2007-2016 The University of Waikato, Hamilton, New Zealand.
4	* All rights reserved.
5	*
6	* This file is part of libtrace.
7	*
8	* This code has been developed by the University of Waikato WAND
9	* research group. For further information please see http://www.wand.net.nz/
10	*
11	* libtrace is free software; you can redistribute it and/or modify
12	* it under the terms of the GNU Lesser General Public License as published by
13	* the Free Software Foundation; either version 3 of the License, or
14	* (at your option) any later version.
15	*
16	* libtrace is distributed in the hope that it will be useful,
17	* but WITHOUT ANY WARRANTY; without even the implied warranty of
18	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19	* GNU Lesser General Public License for more details.
20	*
21	* You should have received a copy of the GNU Lesser General Public License
22	* along with this program. If not, see <http://www.gnu.org/licenses/>.
23	*
24	*
25	*/
26	#include "libtrace_int.h"
27	#include "libtrace.h"
28	#include "protocols.h"
29	#include <stdlib.h>
30	#include <string.h>
31
32
33	/* This file contains all the protocol decoding functions for layer 2
34	* (and 2.5) protocols. This includes functions for accessing MAC addresses.
35	*
36	* Supported protocols include (but are not limited to):
37	* Ethernet
38	* 802.11
39	* 802.1q (vlan)
40	* MPLS
41	* PPPoE
42	* LLCSnap
43	* ATM
44	*/
45
46
47	/* Returns the payload from 802.3 ethernet. Type optionally returned in
48	* "type" in host byte order. This will return a vlan header.
49	*/
50	void trace_get_payload_from_ethernet(void ethernet,
51	uint16_t *type,
52	uint32_t *remaining)
53	{
54	libtrace_ether_t eth = (libtrace_ether_t)ethernet;
55
56	if (remaining) {
57	if (remaining < sizeof(eth)) {
58	*remaining = 0;
59	return NULL;
60	}
61	remaining-=sizeof(eth);
62	}
63
64	if (type)
65	*type = ntohs(eth->ether_type);
66
67	return (void)((char )eth + sizeof(*eth));
68	}
69
70	/* Skip any 802.1q headers if necessary
71	* type is now output only (why check it if we don't need to?)
72	*/
73	void trace_get_payload_from_vlan(void ethernet, uint16_t *type,
74	uint32_t *remaining)
75	{
76	libtrace_8021q_t vlanhdr = (libtrace_8021q_t )ethernet;
77
78	if (remaining) {
79	if (*remaining < sizeof(libtrace_8021q_t)) {
80	*remaining = 0;
81	return NULL;
82	}
83
84	remaining=remaining-sizeof(libtrace_8021q_t);
85	}
86
87	if (type)
88	*type = ntohs(vlanhdr->vlan_ether_type);
89
90	return (void)((char )ethernet + sizeof(*vlanhdr));
91
92	}
93
94	int trace_destroy_layer2_headers(libtrace_layer2_headers_t *headers) {
95	if (headers == NULL) {
96	fprintf(stderr, "NULL libtrace_layer2_headers_t passed into "
97	"trace_destroy_layer2_headers()\n");
98	return -1;
99	}
100
101	if (headers->header != NULL) {
102	free(headers->header);
103	}
104	free(headers);
105	return 1;
106	}
107	libtrace_layer2_headers_t trace_get_layer2_headers(libtrace_packet_t packet) {
108
109	char *ptr;
110	libtrace_linktype_t linktype;
111	uint32_t remaining;
112	uint16_t ethertype;
113	libtrace_layer2_headers_t *r;
114
115	if (packet == NULL) {
116	fprintf(stderr, "NULL packet passed into trace_get_layer2_headers()\n");
117	return NULL;
118	}
119	if (packet->trace == NULL) {
120	fprintf(stderr, "Packet contains a NULL trace in trace_get_layer2_headers()\n");
121	return NULL;
122	}
123
124	/* jump to layer 2 */
125	ptr = trace_get_layer2(packet, &linktype, &remaining);
126	/* packet does not contain layer2 */
127	if (ptr == NULL) {
128	return NULL;
129	}
130
131	/* allocate memory for the result */
132	r = calloc(1, sizeof(libtrace_layer2_headers_t));
133	if (r == NULL) {
134	trace_set_err(packet->trace, TRACE_ERR_OUT_OF_MEMORY,
135	"Unable to allocate memory in trace_get_layer2_headers()\n");
136	return NULL;
137	}
138	r->header = calloc(1, sizeof(libtrace_layer2_header_t));
139	if (r->header == NULL) {
140	trace_set_err(packet->trace, TRACE_ERR_OUT_OF_MEMORY,
141	"Unable to allocate memory in trace_get_layer2_headers()\n");
142	free(r);
143	return NULL;
144	}
145
146	/* get the first layer2 header */
147	ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &remaining);
148
149	while (remaining != 0 && ptr != NULL) {
150
151	if (ethertype == TRACE_ETHERTYPE_LOOPBACK \|\|
152	ethertype == TRACE_ETHERTYPE_IP \|\|
153	ethertype == TRACE_ETHERTYPE_ARP \|\|
154	ethertype == TRACE_ETHERTYPE_RARP \|\|
155	ethertype == TRACE_ETHERTYPE_8021Q \|\|
156	ethertype == TRACE_ETHERTYPE_IPV6 \|\|
157	ethertype == TRACE_ETHERTYPE_8021QS \|\|
158	ethertype == TRACE_ETHERTYPE_MPLS \|\|
159	ethertype == TRACE_ETHERTYPE_MPLS_MC \|\|
160	ethertype == TRACE_ETHERTYPE_PPP_DISC \|\|
161	ethertype == TRACE_ETHERTYPE_PPP_SES) {
162
163	/* Set the bitmask */
164	switch (ethertype) {
165	case (TRACE_ETHERTYPE_LOOPBACK):
166	r->bitmask \|= TRACE_BITMASK_LOOPBACK;
167	break;
168	case (TRACE_ETHERTYPE_IP):
169	r->bitmask \|= TRACE_BITMASK_IP;
170	break;
171	case (TRACE_ETHERTYPE_ARP):
172	r->bitmask \|= TRACE_BITMASK_ARP;
173	break;
174	case (TRACE_ETHERTYPE_RARP):
175	r->bitmask \|= TRACE_BITMASK_RARP;
176	break;
177	case (TRACE_ETHERTYPE_8021Q):
178	r->bitmask \|= TRACE_BITMASK_8021Q;
179	break;
180	case (TRACE_ETHERTYPE_IPV6):
181	r->bitmask \|= TRACE_BITMASK_IPV6;
182	break;
183	case (TRACE_ETHERTYPE_8021QS):
184	r->bitmask \|= TRACE_BITMASK_8021QS;
185	break;
186	case (TRACE_ETHERTYPE_MPLS):
187	r->bitmask \|= TRACE_BITMASK_MPLS;
188	break;
189	case (TRACE_ETHERTYPE_MPLS_MC):
190	r->bitmask \|= TRACE_BITMASK_MPLS_MC;
191	break;
192	case (TRACE_ETHERTYPE_PPP_DISC):
193	r->bitmask \|= TRACE_BITMASK_PPP_DISC;
194	break;
195	case (TRACE_ETHERTYPE_PPP_SES):
196	r->bitmask \|= TRACE_BITMASK_PPP_SES;
197	break;
198	}
199
200	r->header = realloc(r->header,
201	sizeof(libtrace_layer2_header_t)*(r->num+1));
202	r->header[r->num].ethertype = ethertype;
203	r->header[r->num++].data = ptr;
204	}
205
206	/* if the last ethertype was IP stop */
207	if (ethertype == TRACE_ETHERTYPE_IP \|\| ethertype == TRACE_ETHERTYPE_IPV6) {
208	break;
209	}
210
211	/* get the next header */
212	ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &remaining);
213	}
214
215	/* If no results were found free memory now and just return NULL */
216	if (r->num) {
217	free(r->header);
218	free(r);
219	return NULL;
220	}
221
222	return r;
223	}
224
225	uint16_t trace_get_outermost_vlan(libtrace_packet_t packet, uint8_t *vlanptr,
226	uint32_t *remaining) {
227
228	uint8_t *ptr;
229	libtrace_linktype_t linktype;
230	uint32_t rem;
231	uint16_t vlanid = VLAN_NOT_FOUND;
232	uint16_t ethertype = 0;
233
234	if (!packet) {
235	fprintf(stderr, "NULL packet passed into trace_get_outermost_vlan()\n");
236	*vlanptr = NULL;
237	*remaining = rem;
238	return vlanid;
239	}
240
241	ptr = trace_get_layer2(packet, &linktype, &rem);
242	/* No layer 2 */
243	if (ptr == NULL) {
244	*vlanptr = NULL;
245	*remaining = rem;
246	return vlanid;
247	}
248
249	while (ethertype != TRACE_ETHERTYPE_8021Q && ethertype != TRACE_ETHERTYPE_8021QS) {
250
251	if (rem == 0 \|\| ptr == NULL \|\| ethertype == TRACE_ETHERTYPE_IP \|\|
252	ethertype == TRACE_ETHERTYPE_IPV6) {
253
254	*vlanptr = NULL;
255	*remaining = rem;
256	return vlanid;
257	}
258
259	/* get the next layer 2 header */
260	ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &rem);
261	}
262
263	/* found a vlan header */
264	uint32_t val = ntohl((uint32_t )ptr);
265	/* the id portion is only 12 bits */
266	vlanid = (((val >> 16) << 4) >> 4);
267
268	*remaining = rem;
269	*vlanptr = ptr;
270	return vlanid;
271	}
272
273	uint32_t trace_get_outermost_mpls(libtrace_packet_t packet, uint8_t *mplsptr,
274	uint32_t *remaining) {
275
276	uint8_t *ptr;
277	uint32_t mplslabel = MPLS_NOT_FOUND;
278	libtrace_linktype_t linktype;
279	uint32_t rem;
280	uint16_t ethertype = 0;
281
282	if (!packet) {
283	fprintf(stderr, "NULL packet passed into trace_get_outermost_mpls()\n");
284	*remaining = 0;
285	*mplsptr = NULL;
286	return mplslabel;
287	}
288
289	ptr = trace_get_layer2(packet, &linktype, &rem);
290	/* No layer2 */
291	if (ptr == NULL) {
292	*remaining = rem;
293	*mplsptr = NULL;
294	return mplslabel;
295	}
296
297	/* loop over the packet until we find a mpls label */
298	while (ethertype != TRACE_ETHERTYPE_MPLS) {
299	if (rem == 0 \|\| ptr == NULL) {
300
301	*remaining = rem;
302	*mplsptr = NULL;
303	return mplslabel;
304	}
305
306	/* get next layer2 header */
307	ptr = trace_get_payload_from_layer2(ptr, linktype, &ethertype, &rem);
308	}
309
310	uint32_t val = ntohl((uint32_t )ptr);
311	mplslabel = val >> 12;
312
313	*remaining = rem;
314	*mplsptr = ptr;
315	return mplslabel;
316	}
317
318	libtrace_packet_t trace_strip_packet(libtrace_packet_t packet) {
319
320	libtrace_ether_t *ethernet;
321	libtrace_linktype_t linktype;
322	uint16_t ethertype;
323	uint32_t remaining;
324	char *nextpayload;
325	uint16_t finalethertype = 0;
326	uint16_t caplen, removed = 0;
327	char *dest;
328	uint8_t done = 0;
329	uint32_t oldrem;
330
331	/* For now, this will just work for Ethernet packets. */
332	ethernet = (libtrace_ether_t *)trace_get_layer2(packet,
333	&linktype, &remaining);
334
335	if (linktype != TRACE_TYPE_ETH) {
336	return packet;
337	}
338
339	/* No headers to strip, return the original packet */
340	if (ethernet->ether_type == TRACE_ETHERTYPE_IP \|\|
341	ethernet->ether_type == TRACE_ETHERTYPE_IPV6) {
342	return packet;
343	}
344
345	if (remaining <= sizeof(libtrace_ether_t))
346	return packet;
347
348	caplen = trace_get_capture_length(packet);
349	ethertype = ntohs(ethernet->ether_type);
350	dest = ((char *)ethernet) + sizeof(libtrace_ether_t);
351	nextpayload = dest;
352	remaining -= sizeof(libtrace_ether_t);
353
354	/* I'd normally use trace_get_layer3 here, but it works out faster
355	* to do it this way (mostly less function call overhead).
356	*
357	* XXX This approach is going to just strip everything between the
358	* Ethernet and IP headers -- is there a use case where someone
359	* might want to selectively strip headers?
360	*/
361	while (!done) {
362
363	if (nextpayload == NULL \|\| remaining == 0)
364	break;
365
366	oldrem = remaining;
367	switch (ethertype) {
368
369	case TRACE_ETHERTYPE_8021Q:
370	nextpayload = (char *)trace_get_payload_from_vlan(
371	nextpayload,
372	&ethertype, &remaining);
373	removed += (oldrem - remaining);
374	break;
375
376	case TRACE_ETHERTYPE_MPLS:
377	nextpayload = (char *)trace_get_payload_from_mpls(
378	nextpayload,
379	&ethertype, &remaining);
380	removed += (oldrem - remaining);
381	break;
382	case TRACE_ETHERTYPE_PPP_SES:
383	nextpayload = (char *)trace_get_payload_from_pppoe(
384	nextpayload,
385	&ethertype, &remaining);
386	removed += (oldrem - remaining);
387	break;
388
389	case TRACE_ETHERTYPE_IP:
390	case TRACE_ETHERTYPE_IPV6:
391	default:
392	if (finalethertype == 0)
393	finalethertype = ethertype;
394	done = true;
395	break;
396	}
397	}
398
399	if (nextpayload != NULL && removed > 0) {
400
401	ethernet->ether_type = ntohs(finalethertype);
402	trace_set_capture_length(packet, caplen - removed);
403	memmove(nextpayload - (dest - (char *)packet->payload),
404	packet->payload,
405	(dest - (char *)packet->payload));
406	packet->payload = nextpayload - (dest - (char *)packet->payload);
407	packet->cached.l2_header = NULL;
408	}
409
410	return packet;
411
412	}
413
414	/* Skip any MPLS headers if necessary, guessing what the next type is
415	* type is input/output. If the next type is "ethernet" this will
416	* return a type of 0x0000.
417	*/
418	void trace_get_payload_from_mpls(void ethernet, uint16_t *type,
419	uint32_t *remaining) {
420	/* Ensure supplied type is not NULL */
421	if (!type) {
422	fprintf(stderr, "NULL type passed into trace_get_payload_from_mpls()\n");
423	return NULL;
424	}
425
426	if ((((char*)ethernet)[2]&0x01)==0) {
427	/* The MPLS Stack bit is set */
428	*type = TRACE_ETHERTYPE_MPLS;
429	}
430	else {
431	if (!remaining \|\| *remaining>=5) {
432	switch (((char*)ethernet)[4]&0xF0) {
433	case 0x40: /* IPv4 */
434	*type = TRACE_ETHERTYPE_IP;
435	break;
436	case 0x60: /* IPv6 */
437	*type = TRACE_ETHERTYPE_IPV6;
438	break;
439	default: /* VPLS */
440	/* Ethernet */
441	*type = 0;
442	}
443	}
444	}
445	ethernet=(char*)ethernet+4;
446	if (remaining) {
447	if (*remaining<4)
448	return NULL;
449	else
450	*remaining-=4;
451	}
452
453
454	return ethernet;
455	}
456
457	static void trace_get_payload_from_llcsnap(void link,
458	uint16_t type, uint32_t remaining)
459	{
460	/* 64 byte capture. */
461	libtrace_llcsnap_t llc = (libtrace_llcsnap_t)link;
462
463	if (remaining) {
464	if (*remaining < sizeof(libtrace_llcsnap_t)) {
465	*remaining = 0;
466	return NULL;
467	}
468	*remaining-=(sizeof(libtrace_llcsnap_t));
469	}
470
471	llc = (libtrace_llcsnap_t)((char )llc);
472
473	if (type) *type = ntohs(llc->type);
474
475	return (void)((char)llc+sizeof(*llc));
476	}
477
478
479	static void trace_get_payload_from_80211(void link, uint16_t type, uint32_t remaining)
480	{
481	libtrace_80211_t *wifi;
482	uint16_t eth; / ethertype */
483	int8_t extra = 0; /* how many QoS bytes to skip */
484
485	if (remaining && *remaining < sizeof(libtrace_80211_t)) {
486	*remaining = 0;
487	return NULL;
488	}
489
490	wifi=(libtrace_80211_t*)link;
491
492	/* Data packet? */
493	if (wifi->type != 2) {
494	return NULL;
495	}
496
497	/* If FromDS and ToDS are both set then we have a four-address
498	* frame. Otherwise we have a three-address frame */
499	if (!(wifi->to_ds && wifi->from_ds))
500	extra -= 6;
501
502	/* Indicates QoS field present, see IEEE802.11e-2005 pg 21 */
503	if (wifi->subtype & 0x8)
504	extra += 2;
505
506	if (remaining && remaining < sizeof(eth)) {
507	*remaining = 0;
508	return NULL;
509	}
510
511	eth=(uint16_t )((char)wifi+sizeof(*wifi)+extra);
512
513	if (*eth == 0xaaaa)
514	/* Payload contains an 802.2 LLC/SNAP frame */
515	return trace_get_payload_from_llcsnap((void *)eth, type, remaining);
516
517	/* Otherwise we assume an Ethernet II frame */
518	if (type) type=ntohs(eth);
519	if (remaining) remaining = remaining - sizeof(libtrace_80211_t) - extra - sizeof(*eth);
520
521	return (void)((char)eth+sizeof(*eth));
522	}
523
524	static void trace_get_payload_from_ppp(void link,
525	uint16_t type, uint32_t remaining)
526	{
527	/* 64 byte capture. */
528	libtrace_ppp_t ppp = (libtrace_ppp_t)link;
529
530	if (remaining) {
531	if (*remaining < sizeof(libtrace_ppp_t)) {
532	*remaining = 0;
533	return NULL;
534	}
535	*remaining-=sizeof(libtrace_ppp_t);
536	}
537
538	if (type) {
539	switch(ntohs(ppp->protocol)) {
540	case 0x0021: *type = TRACE_ETHERTYPE_IP; break;
541	case 0x0057: *type = TRACE_ETHERTYPE_IPV6; break;
542	/* If it isn't IP, then it is probably PPP control and
543	* I can't imagine anyone caring about that too much
544	*/
545	default: *type = 0; break;
546	}
547	}
548
549
550	return (void)((char )ppp+sizeof(*ppp));
551	}
552
553	void trace_get_payload_from_pppoe(void link, uint16_t *type,
554	uint32_t *remaining) {
555	/* Ensure type supplied is not NULL */
556	if (!type) {
557	fprintf(stderr, "NULL type passed into trace_get_payload_from_pppoe()\n");
558	return NULL;
559	}
560
561	if (remaining) {
562	if (*remaining < sizeof(libtrace_pppoe_t)) {
563	*remaining = 0;
564	return NULL;
565	}
566	*remaining -= sizeof(libtrace_pppoe_t);
567	}
568
569	/* PPPoE is always followed by PPP */
570	return trace_get_payload_from_ppp(link + sizeof(libtrace_pppoe_t),
571	type, remaining);
572	}
573
574	/* Header for CHDLC framing */
575	typedef struct libtrace_chdlc_t {
576	uint8_t address; /** 0xF0 for unicast, 0xF8 for multicast */
577	uint8_t control; /** Always 0x00 */
578	uint16_t ethertype;
579	} libtrace_chdlc_t;
580
581	/* Header for PPP in HDLC-like framing */
582	typedef struct libtrace_ppp_hdlc_t {
583	uint8_t address; /** Always should be 0xff */
584	uint8_t control; /** Always should be 0x03 */
585	uint16_t protocol;
586	} libtrace_ppp_hdlc_t;
587
588	static void trace_get_payload_from_chdlc(void link, uint16_t *type,
589	uint32_t *remaining) {
590
591	libtrace_chdlc_t chdlc = (libtrace_chdlc_t )link;
592
593	if (remaining) {
594	if (*remaining < sizeof(libtrace_chdlc_t)) {
595	*remaining = 0;
596	return NULL;
597	}
598	*remaining -= sizeof(libtrace_chdlc_t);
599	}
600
601	if (type) {
602	*type = ntohs(chdlc->ethertype);
603	}
604
605	return (void )((char )chdlc + sizeof(*chdlc));
606
607	}
608
609	static void trace_get_payload_from_ppp_hdlc(void link,
610	uint16_t type, uint32_t remaining)
611	{
612	libtrace_ppp_hdlc_t ppp_hdlc = (libtrace_ppp_hdlc_t)link;
613
614	if (remaining) {
615	if (*remaining < sizeof(libtrace_ppp_hdlc_t)) {
616	*remaining = 0;
617	return NULL;
618	}
619	*remaining-=sizeof(libtrace_ppp_hdlc_t);
620	}
621
622	if (type) {
623	/* http://www.iana.org/assignments/ppp-numbers */
624
625	switch(ntohs(ppp_hdlc->protocol)) {
626	case 0x0021: /* IP */
627	*type = TRACE_ETHERTYPE_IP;
628	break;
629	case 0x0057: /* IPV6 */
630	*type = TRACE_ETHERTYPE_IPV6;
631	break;
632	case 0xc021: /* Link Control Protocol */
633	type = 0; / No ethertype for this */
634	break;
635
636	default:
637	printf("Unknown chdlc type: %04x\n",
638	ntohs(ppp_hdlc->protocol));
639	type = 0; / Unknown */
640	}
641	}
642
643
644	return (void)((char )ppp_hdlc+sizeof(*ppp_hdlc));
645	}
646
647	void trace_get_payload_from_link(void link, libtrace_linktype_t linktype,
648	uint16_t ethertype, uint32_t remaining)
649	{
650	void *l = NULL;
651
652	do {
653	l = trace_get_payload_from_meta(link, &linktype, remaining);
654	if (l != NULL) {
655	link=l;
656	}
657	} while (l != NULL);
658
659	return trace_get_payload_from_layer2(link,linktype,ethertype,remaining);
660
661	}
662
663	DLLEXPORT void trace_get_layer2(const libtrace_packet_t packet,
664	libtrace_linktype_t *linktype,
665	uint32_t *remaining)
666	{
667	uint32_t dummyrem;
668	void *meta = NULL;
669
670	if (!packet) {
671	fprintf(stderr, "NULL packet passed into trace_get_layer2()\n");
672	return NULL;
673	}
674	if (!linktype) {
675	fprintf(stderr, "NULL linktype passed into trace_get_layer2()\n");
676	return NULL;
677	}
678
679	if (remaining == NULL)
680	remaining = &dummyrem;
681
682	if (packet->cached.l2_header) {
683	/* Use cached values */
684	*linktype = packet->cached.link_type;
685	*remaining = packet->cached.l2_remaining;
686	return packet->cached.l2_header;
687	}
688
689	/* Code looks a bit inefficient, but I'm actually trying to avoid
690	* calling trace_get_packet_buffer more than once like we used to.
691	*/
692	meta = trace_get_packet_buffer(packet, linktype, remaining);
693	if (meta == NULL) {
694	return NULL;
695	}
696
697	/* If there are no meta-data headers, we just return the start of the
698	* packet buffer, along with the linktype, etc.
699	*/
700	switch(*linktype) {
701	/* meta points to a layer 2 header! */
702	case TRACE_TYPE_HDLC_POS:
703	case TRACE_TYPE_ETH:
704	case TRACE_TYPE_ATM:
705	case TRACE_TYPE_80211:
706	case TRACE_TYPE_NONE:
707	case TRACE_TYPE_POS:
708	case TRACE_TYPE_AAL5:
709	case TRACE_TYPE_DUCK:
710	case TRACE_TYPE_LLCSNAP:
711	case TRACE_TYPE_PPP:
712	case TRACE_TYPE_METADATA:
713	case TRACE_TYPE_NONDATA:
714	case TRACE_TYPE_OPENBSD_LOOP:
715	((libtrace_packet_t*)packet)->cached.l2_header = meta;
716	((libtrace_packet_t)packet)->cached.l2_remaining = remaining;
717	return meta;
718	case TRACE_TYPE_LINUX_SLL:
719	case TRACE_TYPE_80211_RADIO:
720	case TRACE_TYPE_80211_PRISM:
721	case TRACE_TYPE_PFLOG:
722	case TRACE_TYPE_ERF_META:
723	case TRACE_TYPE_PCAPNG_META:
724	case TRACE_TYPE_ETSILI:
725	break;
726	case TRACE_TYPE_UNKNOWN:
727	case TRACE_TYPE_CONTENT_INVALID:
728	return NULL;
729	}
730
731	/* If there are meta-data headers, we need to skip over them until we
732	* find a non-meta data header and return that.
733	*/
734	for(;;) {
735	void *nexthdr = trace_get_payload_from_meta(meta,
736	linktype, remaining);
737
738	if (nexthdr == NULL) {
739	switch (*linktype) {
740	/* meta points to a layer 2 header! */
741	case TRACE_TYPE_HDLC_POS:
742	case TRACE_TYPE_ETH:
743	case TRACE_TYPE_ATM:
744	case TRACE_TYPE_80211:
745	case TRACE_TYPE_NONE:
746	case TRACE_TYPE_POS:
747	case TRACE_TYPE_AAL5:
748	case TRACE_TYPE_DUCK:
749	case TRACE_TYPE_LLCSNAP:
750	case TRACE_TYPE_PPP:
751	case TRACE_TYPE_METADATA:
752	case TRACE_TYPE_NONDATA:
753	case TRACE_TYPE_OPENBSD_LOOP:
754	((libtrace_packet_t*)packet)->cached.l2_header = meta;
755	((libtrace_packet_t)packet)->cached.l2_remaining = remaining;
756	return meta;
757	case TRACE_TYPE_LINUX_SLL:
758	case TRACE_TYPE_80211_RADIO:
759	case TRACE_TYPE_80211_PRISM:
760	case TRACE_TYPE_PFLOG:
761	case TRACE_TYPE_ERF_META:
762	case TRACE_TYPE_PCAPNG_META:
763	case TRACE_TYPE_ETSILI:
764	break;
765	case TRACE_TYPE_UNKNOWN:
766	case TRACE_TYPE_CONTENT_INVALID:
767	return NULL;
768	}
769
770	/* Otherwise, we must have hit the end of the packet */
771	return NULL;
772	}
773
774
775	meta = nexthdr;
776	}
777
778	}
779
780	DLLEXPORT
781	void trace_get_payload_from_atm(void link,
782	uint8_t type, uint32_t remaining)
783	{
784	libtrace_atm_capture_cell_t *cell;
785	if (remaining && *remaining<sizeof(libtrace_atm_capture_cell_t)) {
786	*remaining = 0;
787	return NULL;
788	}
789	cell=(libtrace_atm_capture_cell_t*)link;
790
791	if (type)
792	*type=cell->pt;
793
794	if (remaining)
795	*remaining-=sizeof(libtrace_atm_capture_cell_t);
796
797	return ((char*)link)+sizeof(libtrace_atm_capture_cell_t);
798	}
799
800
801
802	DLLEXPORT void trace_get_payload_from_layer2(void link,
803	libtrace_linktype_t linktype,
804	uint16_t *ethertype,
805	uint32_t *remaining)
806	{
807	void *l;
808
809	if (linktype == TRACE_TYPE_UNKNOWN \|\|
810	linktype == TRACE_TYPE_CONTENT_INVALID) {
811	fprintf(stderr, "Unable to determine linktype for packet\n");
812	return NULL;
813	}
814
815	if (link == NULL) {
816	return NULL;
817	}
818
819	switch(linktype) {
820	/* Packet Metadata headers, not layer2 headers */
821	case TRACE_TYPE_80211_PRISM:
822	case TRACE_TYPE_80211_RADIO:
823	case TRACE_TYPE_PFLOG:
824	case TRACE_TYPE_LINUX_SLL:
825	case TRACE_TYPE_ETSILI:
826	return NULL;
827
828	/* duck packets have no payload! */
829	case TRACE_TYPE_DUCK:
830	return NULL;
831
832	/* The payload is in these packets does
833	not correspond to a genuine link-layer
834	*/
835	case TRACE_TYPE_METADATA:
836	case TRACE_TYPE_NONDATA:
837	case TRACE_TYPE_PCAPNG_META:
838	case TRACE_TYPE_ERF_META:
839	case TRACE_TYPE_CONTENT_INVALID:
840	case TRACE_TYPE_UNKNOWN:
841	return NULL;
842
843	case TRACE_TYPE_80211:
844	return trace_get_payload_from_80211(link,ethertype,remaining);
845	case TRACE_TYPE_ETH:
846	return trace_get_payload_from_ethernet(link,ethertype,remaining);
847	case TRACE_TYPE_NONE:
848	if (*remaining == 0) {
849	return NULL;
850	}
851
852	if (((char)link&0xF0) == 0x40)
853	ethertype=TRACE_ETHERTYPE_IP; / IPv4 */
854	else if (((char)link&0xF0) == 0x60)
855	ethertype=TRACE_ETHERTYPE_IPV6; / IPv6 */
856	else
857	return NULL; /* No idea */
858	return link; /* I love the simplicity */
859	case TRACE_TYPE_PPP:
860	return trace_get_payload_from_ppp(link,ethertype,remaining);
861	case TRACE_TYPE_ATM:
862	l=trace_get_payload_from_atm(link,NULL,remaining);
863	/* FIXME: We shouldn't skip llcsnap here, we should
864	* return an ethertype for it (somehow)
865	*/
866	return (l ? trace_get_payload_from_llcsnap(l,
867	ethertype, remaining):NULL);
868	case TRACE_TYPE_LLCSNAP:
869	return trace_get_payload_from_llcsnap(link,ethertype,remaining);
870
871	case TRACE_TYPE_HDLC_POS:
872	return trace_get_payload_from_chdlc(link,ethertype,
873	remaining);
874	case TRACE_TYPE_POS:
875	return trace_get_payload_from_ppp_hdlc(link,ethertype,
876	remaining);
877	/* TODO: Unsupported */
878	case TRACE_TYPE_AAL5:
879	return NULL;
880
881	case TRACE_TYPE_OPENBSD_LOOP:
882	if (*remaining <= 4) {
883	return NULL;
884	}
885	link = link + 4; /* Loopback header is 4 bytes */
886	if (((char)link&0xF0) == 0x40)
887	ethertype=TRACE_ETHERTYPE_IP; / IPv4 */
888	else if (((char)link&0xF0) == 0x60)
889	ethertype=TRACE_ETHERTYPE_IPV6; / IPv6 */
890	else
891	return NULL;
892	return link; /* I love the simplicity */
893
894
895	}
896	return NULL;
897
898	}
899
900	/* Take a pointer to the start of an IEEE 802.11 MAC frame and return a pointer
901	* to the source MAC address.
902	* If the frame does not contain a sender address, e.g. ACK frame, return NULL.
903	* If the frame is a 4-address WDS frame, return TA, i.e. addr2.
904	* NB: This function decodes the 802.11 header, so it assumes that there are no
905	* bit-errors. If there are, all bets are off.
906	*/
907	static
908	uint8_t get_source_mac_from_wifi(void wifi) {
909	struct libtrace_80211_t *w;
910	if (wifi == NULL) return NULL;
911	w = (struct libtrace_80211_t *) wifi;
912
913	/* If the frame is of type CTRL */
914	if (w->type == 0x1)
915	/* If bit 2 of the subtype field is zero, this indicates that
916	* there is no transmitter address, i.e. the frame is either an
917	* ACK or a CTS frame */
918	if ((w->subtype & 0x2) == 0)
919	return NULL;
920
921	/* Always return the address of the transmitter, i.e. address 2 */
922	return (uint8_t *) &w->mac2;
923	}
924
925	DLLEXPORT uint8_t trace_get_source_mac(libtrace_packet_t packet) {
926	/* Ensure the supplied packet is not NULL */
927	if (!packet) {
928	fprintf(stderr, "NULL packet passed into trace_get_source_mac()\n");
929	return NULL;
930	}
931
932	void *link;
933	uint32_t remaining;
934	libtrace_linktype_t linktype;
935	link = trace_get_layer2(packet,&linktype,&remaining);
936
937	if (!link)
938	return NULL;
939
940	switch (linktype) {
941	case TRACE_TYPE_ETH:
942	return (uint8_t )&(((libtrace_ether_t)link)->ether_shost);
943	case TRACE_TYPE_80211:
944	return get_source_mac_from_wifi(link);
945	/* These packets don't have MAC addresses */
946	case TRACE_TYPE_POS:
947	case TRACE_TYPE_NONE:
948	case TRACE_TYPE_HDLC_POS:
949	case TRACE_TYPE_PFLOG:
950	case TRACE_TYPE_ATM:
951	case TRACE_TYPE_DUCK:
952	case TRACE_TYPE_METADATA:
953	case TRACE_TYPE_AAL5:
954	case TRACE_TYPE_LLCSNAP:
955	case TRACE_TYPE_PPP:
956	case TRACE_TYPE_NONDATA:
957	case TRACE_TYPE_OPENBSD_LOOP:
958	case TRACE_TYPE_ERF_META:
959	case TRACE_TYPE_PCAPNG_META:
960	case TRACE_TYPE_UNKNOWN:
961	case TRACE_TYPE_CONTENT_INVALID:
962	return NULL;
963
964	/* Metadata headers should already be skipped */
965	case TRACE_TYPE_LINUX_SLL:
966	case TRACE_TYPE_80211_PRISM:
967	case TRACE_TYPE_80211_RADIO:
968	case TRACE_TYPE_ETSILI:
969	fprintf(stderr, "Metadata headers should already be skipped in trace_get_source_mac()\n");
970	return NULL;
971	}
972	fprintf(stderr,"%s not implemented for linktype %i\n", __func__, linktype);
973	return NULL;
974	}
975
976	DLLEXPORT uint8_t trace_get_destination_mac(libtrace_packet_t packet) {
977	/* Ensure the supplied packet is not NULL */
978	if (!packet) {
979	fprintf(stderr, "NULL packet passed into trace_get_destination_mac()\n");
980	return NULL;
981	}
982
983	void *link;
984	libtrace_linktype_t linktype;
985	uint32_t remaining;
986	libtrace_80211_t *wifi;
987	libtrace_ether_t *ethptr;
988
989	link = trace_get_layer2(packet,&linktype,&remaining);
990
991	ethptr = (libtrace_ether_t*)link;
992
993
994	if (!link)
995	return NULL;
996
997	switch (linktype) {
998	case TRACE_TYPE_80211:
999	wifi=(libtrace_80211_t*)link;
1000	return (uint8_t*)&wifi->mac1;
1001	case TRACE_TYPE_ETH:
1002	return (uint8_t*)&ethptr->ether_dhost;
1003	case TRACE_TYPE_POS:
1004	case TRACE_TYPE_NONE:
1005	case TRACE_TYPE_ATM:
1006	case TRACE_TYPE_HDLC_POS:
1007	case TRACE_TYPE_PFLOG:
1008	case TRACE_TYPE_DUCK:
1009	case TRACE_TYPE_METADATA:
1010	case TRACE_TYPE_AAL5:
1011	case TRACE_TYPE_LLCSNAP:
1012	case TRACE_TYPE_PPP:
1013	case TRACE_TYPE_NONDATA:
1014	case TRACE_TYPE_OPENBSD_LOOP:
1015	case TRACE_TYPE_ERF_META:
1016	case TRACE_TYPE_PCAPNG_META:
1017	case TRACE_TYPE_UNKNOWN:
1018	case TRACE_TYPE_CONTENT_INVALID:
1019	/* No MAC address */
1020	return NULL;
1021	/* Metadata headers should already be skipped */
1022	case TRACE_TYPE_LINUX_SLL:
1023	case TRACE_TYPE_80211_PRISM:
1024	case TRACE_TYPE_80211_RADIO:
1025	case TRACE_TYPE_ETSILI:
1026	fprintf(stderr, "Metadata headers should already be skipped in trace_get_destination_mac()\n");
1027	return NULL;
1028	}
1029	fprintf(stderr,"Not implemented\n");
1030	return NULL;
1031	}
1032

Note: See TracBrowser for help on using the repository browser.

Download in other formats: