source: lib/protocols_l2.c @ 5325ff0

develop
Last change on this file since 5325ff0 was 5325ff0, checked in by Jacob van Walraven <jacobvw@…>, 2 years ago

Make sure we dont add the IP header to the layer2_headers structure

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