source: lib/protocols_l2.c @ 692bf9c

develop
Last change on this file since 692bf9c was 692bf9c, checked in by Jacob Van Walraven <jcv9@…>, 21 months ago

Add support to retrieve outermost vlan tag, outermost mpls label, and all layer2 headers

  • Property mode set to 100644
File size: 27.3 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
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
225uint16_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
273uint32_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
318libtrace_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 */
418void *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
457static 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
479static 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
524static 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
553void *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 */
575typedef 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 */
582typedef 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
588static 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
609static 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
647void *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
663DLLEXPORT 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       
693        meta = trace_get_packet_buffer(packet, linktype, remaining);
694
695        /* If there are no meta-data headers, we just return the start of the
696         * packet buffer, along with the linktype, etc.
697         */
698        switch(*linktype) {
699                /* meta points to a layer 2 header! */
700                case TRACE_TYPE_HDLC_POS:
701                case TRACE_TYPE_ETH:
702                case TRACE_TYPE_ATM:
703                case TRACE_TYPE_80211:
704                case TRACE_TYPE_NONE:
705                case TRACE_TYPE_POS:
706                case TRACE_TYPE_AAL5:
707                case TRACE_TYPE_DUCK:
708                case TRACE_TYPE_LLCSNAP:
709                case TRACE_TYPE_PPP:
710                case TRACE_TYPE_METADATA:
711                case TRACE_TYPE_NONDATA:
712                case TRACE_TYPE_OPENBSD_LOOP:
713                        ((libtrace_packet_t*)packet)->cached.l2_header = meta;
714                        ((libtrace_packet_t*)packet)->cached.l2_remaining = *remaining;
715                        return meta;
716                case TRACE_TYPE_LINUX_SLL:
717                case TRACE_TYPE_80211_RADIO:
718                case TRACE_TYPE_80211_PRISM:
719                case TRACE_TYPE_PFLOG:
720                case TRACE_TYPE_ERF_META:
721                case TRACE_TYPE_ETSILI:
722                        break;
723                case TRACE_TYPE_UNKNOWN:
724                case TRACE_TYPE_CONTENT_INVALID:
725                        return NULL;
726        }
727
728        /* If there are meta-data headers, we need to skip over them until we
729         * find a non-meta data header and return that.
730         */
731        for(;;) {
732                void *nexthdr = trace_get_payload_from_meta(meta, 
733                                linktype, remaining);
734               
735                if (nexthdr == NULL) {
736                        switch (*linktype) {
737                                /* meta points to a layer 2 header! */
738                                case TRACE_TYPE_HDLC_POS:
739                                case TRACE_TYPE_ETH:
740                                case TRACE_TYPE_ATM:
741                                case TRACE_TYPE_80211:
742                                case TRACE_TYPE_NONE:
743                                case TRACE_TYPE_POS:
744                                case TRACE_TYPE_AAL5:
745                                case TRACE_TYPE_DUCK:
746                                case TRACE_TYPE_LLCSNAP:
747                                case TRACE_TYPE_PPP:
748                                case TRACE_TYPE_METADATA:
749                                case TRACE_TYPE_NONDATA:
750                                case TRACE_TYPE_OPENBSD_LOOP:
751                                        ((libtrace_packet_t*)packet)->cached.l2_header = meta;
752                                        ((libtrace_packet_t*)packet)->cached.l2_remaining = *remaining;
753                                        return meta;
754                                case TRACE_TYPE_LINUX_SLL:
755                                case TRACE_TYPE_80211_RADIO:
756                                case TRACE_TYPE_80211_PRISM:
757                                case TRACE_TYPE_PFLOG:
758                                case TRACE_TYPE_ERF_META:
759                                case TRACE_TYPE_ETSILI:
760                                        break;
761                                case TRACE_TYPE_UNKNOWN:
762                                case TRACE_TYPE_CONTENT_INVALID:
763                                        return NULL;
764                        }
765                       
766                        /* Otherwise, we must have hit the end of the packet */
767                        return NULL;
768                }
769         
770               
771                meta = nexthdr;
772        }
773
774}
775
776DLLEXPORT
777void *trace_get_payload_from_atm(void *link,
778                uint8_t *type, uint32_t *remaining)
779{
780        libtrace_atm_capture_cell_t *cell;
781        if (remaining && *remaining<sizeof(libtrace_atm_capture_cell_t)) {
782                *remaining = 0;
783                return NULL;
784        }
785        cell=(libtrace_atm_capture_cell_t*)link;
786
787        if (type)
788                *type=cell->pt;
789
790        if (remaining)
791                *remaining-=sizeof(libtrace_atm_capture_cell_t);
792
793        return ((char*)link)+sizeof(libtrace_atm_capture_cell_t);
794}
795
796
797
798DLLEXPORT void *trace_get_payload_from_layer2(void *link,
799                libtrace_linktype_t linktype,
800                uint16_t *ethertype,
801                uint32_t *remaining)
802{
803        void *l;
804
805        if (linktype == TRACE_TYPE_UNKNOWN ||
806                        linktype == TRACE_TYPE_CONTENT_INVALID) {
807                fprintf(stderr, "Unable to determine linktype for packet\n");
808                return NULL;
809        }
810       
811        switch(linktype) {
812                /* Packet Metadata headers, not layer2 headers */
813                case TRACE_TYPE_80211_PRISM:
814                case TRACE_TYPE_80211_RADIO:
815                case TRACE_TYPE_PFLOG:
816                case TRACE_TYPE_LINUX_SLL:
817                case TRACE_TYPE_ETSILI:
818                        return NULL;
819
820                /* duck packets have no payload! */
821                case TRACE_TYPE_DUCK:
822                        return NULL;
823
824                /* The payload is in these packets does
825                   not correspond to a genuine link-layer
826                   */
827                case TRACE_TYPE_METADATA:
828                case TRACE_TYPE_NONDATA:
829                case TRACE_TYPE_ERF_META:
830                case TRACE_TYPE_CONTENT_INVALID:
831                case TRACE_TYPE_UNKNOWN:
832                        return NULL;
833
834                case TRACE_TYPE_80211:
835                        return trace_get_payload_from_80211(link,ethertype,remaining);
836                case TRACE_TYPE_ETH:
837                        return trace_get_payload_from_ethernet(link,ethertype,remaining);
838                case TRACE_TYPE_NONE:
839                        if ((*(char*)link&0xF0) == 0x40)
840                                *ethertype=TRACE_ETHERTYPE_IP;   /* IPv4 */
841                        else if ((*(char*)link&0xF0) == 0x60)
842                                *ethertype=TRACE_ETHERTYPE_IPV6; /* IPv6 */
843                        return link; /* I love the simplicity */
844                case TRACE_TYPE_PPP:
845                        return trace_get_payload_from_ppp(link,ethertype,remaining);
846                case TRACE_TYPE_ATM:
847                        l=trace_get_payload_from_atm(link,NULL,remaining);
848                        /* FIXME: We shouldn't skip llcsnap here, we should
849                         * return an ethertype for it (somehow)
850                         */
851                        return (l ? trace_get_payload_from_llcsnap(l,
852                                                ethertype, remaining):NULL);
853                case TRACE_TYPE_LLCSNAP:
854                        return trace_get_payload_from_llcsnap(link,ethertype,remaining);
855
856                case TRACE_TYPE_HDLC_POS:
857                        return trace_get_payload_from_chdlc(link,ethertype,
858                                        remaining);
859                case TRACE_TYPE_POS:
860                        return trace_get_payload_from_ppp_hdlc(link,ethertype,
861                                        remaining);
862                /* TODO: Unsupported */
863                case TRACE_TYPE_AAL5:
864                        return NULL;
865
866                case TRACE_TYPE_OPENBSD_LOOP:
867                        link = link + 4; /* Loopback header is 4 bytes */
868                        if ((*(char*)link&0xF0) == 0x40)
869                                *ethertype=TRACE_ETHERTYPE_IP;   /* IPv4 */
870                        else if ((*(char*)link&0xF0) == 0x60)
871                                *ethertype=TRACE_ETHERTYPE_IPV6; /* IPv6 */
872                        return link; /* I love the simplicity */
873               
874
875        }
876        return NULL;
877
878}
879
880/* Take a pointer to the start of an IEEE 802.11 MAC frame and return a pointer
881 * to the source MAC address. 
882 * If the frame does not contain a sender address, e.g. ACK frame, return NULL.
883 * If the frame is a 4-address WDS frame, return TA, i.e. addr2.
884 * NB: This function decodes the 802.11 header, so it assumes that there are no
885 * bit-errors. If there are, all bets are off.
886 */
887static
888uint8_t *get_source_mac_from_wifi(void *wifi) {
889        struct libtrace_80211_t *w;
890        if (wifi == NULL) return NULL;
891        w = (struct libtrace_80211_t *) wifi;
892
893        /* If the frame is of type CTRL */
894        if (w->type == 0x1)
895                /* If bit 2 of the subtype field is zero, this indicates that
896                 * there is no transmitter address, i.e. the frame is either an
897                 * ACK or a CTS frame */
898                if ((w->subtype & 0x2) == 0)
899                        return NULL;
900
901        /* Always return the address of the transmitter, i.e. address 2 */
902        return (uint8_t *) &w->mac2;
903}
904
905DLLEXPORT uint8_t *trace_get_source_mac(libtrace_packet_t *packet) {
906        /* Ensure the supplied packet is not NULL */
907        if (!packet) {
908                fprintf(stderr, "NULL packet passed into trace_get_source_mac()\n");
909                return NULL;
910        }
911
912        void *link;
913        uint32_t remaining;
914        libtrace_linktype_t linktype;
915        link = trace_get_layer2(packet,&linktype,&remaining);
916
917        if (!link)
918                return NULL;
919
920        switch (linktype) {
921                case TRACE_TYPE_ETH:
922                        return (uint8_t *)&(((libtrace_ether_t*)link)->ether_shost);
923                case TRACE_TYPE_80211:
924                        return get_source_mac_from_wifi(link);
925                /* These packets don't have MAC addresses */
926                case TRACE_TYPE_POS:
927                case TRACE_TYPE_NONE:
928                case TRACE_TYPE_HDLC_POS:
929                case TRACE_TYPE_PFLOG:
930                case TRACE_TYPE_ATM:
931                case TRACE_TYPE_DUCK:
932                case TRACE_TYPE_METADATA:
933                case TRACE_TYPE_AAL5:
934                case TRACE_TYPE_LLCSNAP:
935                case TRACE_TYPE_PPP:
936                case TRACE_TYPE_NONDATA:
937                case TRACE_TYPE_OPENBSD_LOOP:
938                case TRACE_TYPE_ERF_META:
939                case TRACE_TYPE_UNKNOWN:
940                case TRACE_TYPE_CONTENT_INVALID:
941                        return NULL;
942
943                /* Metadata headers should already be skipped */
944                case TRACE_TYPE_LINUX_SLL:
945                case TRACE_TYPE_80211_PRISM:
946                case TRACE_TYPE_80211_RADIO:
947                case TRACE_TYPE_ETSILI:
948                        fprintf(stderr, "Metadata headers should already be skipped in trace_get_source_mac()\n");
949                        return NULL;
950        }
951        fprintf(stderr,"%s not implemented for linktype %i\n", __func__, linktype);
952        return NULL;
953}
954
955DLLEXPORT uint8_t *trace_get_destination_mac(libtrace_packet_t *packet) {
956        /* Ensure the supplied packet is not NULL */
957        if (!packet) {
958                fprintf(stderr, "NULL packet passed into trace_get_destination_mac()\n");
959                return NULL;
960        }
961
962        void *link;
963        libtrace_linktype_t linktype;
964        uint32_t remaining;
965        libtrace_80211_t *wifi;
966        libtrace_ether_t *ethptr;
967
968        link = trace_get_layer2(packet,&linktype,&remaining);
969
970        ethptr = (libtrace_ether_t*)link;
971
972
973        if (!link)
974                return NULL;
975
976        switch (linktype) {
977                case TRACE_TYPE_80211:
978                        wifi=(libtrace_80211_t*)link;
979                        return (uint8_t*)&wifi->mac1;
980                case TRACE_TYPE_ETH:
981                        return (uint8_t*)&ethptr->ether_dhost;
982                case TRACE_TYPE_POS:
983                case TRACE_TYPE_NONE:
984                case TRACE_TYPE_ATM:
985                case TRACE_TYPE_HDLC_POS:
986                case TRACE_TYPE_PFLOG:
987                case TRACE_TYPE_DUCK:
988                case TRACE_TYPE_METADATA:
989                case TRACE_TYPE_AAL5:
990                case TRACE_TYPE_LLCSNAP:
991                case TRACE_TYPE_PPP:   
992                case TRACE_TYPE_NONDATA:
993                case TRACE_TYPE_OPENBSD_LOOP:
994                case TRACE_TYPE_ERF_META:
995                case TRACE_TYPE_UNKNOWN:
996                case TRACE_TYPE_CONTENT_INVALID:
997                        /* No MAC address */
998                        return NULL;
999                /* Metadata headers should already be skipped */
1000                case TRACE_TYPE_LINUX_SLL:
1001                case TRACE_TYPE_80211_PRISM:
1002                case TRACE_TYPE_80211_RADIO:
1003                case TRACE_TYPE_ETSILI:
1004                        fprintf(stderr, "Metadata headers should already be skipped in trace_get_destination_mac()\n");
1005                        return NULL;
1006        }
1007        fprintf(stderr,"Not implemented\n");
1008        return NULL;
1009}
1010
Note: See TracBrowser for help on using the repository browser.