source: lib/protocols_l2.c @ c876f29

develop
Last change on this file since c876f29 was c876f29, checked in by Jacob Van Walraven <jcv9@…>, 21 months 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 */
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        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
780DLLEXPORT
781void *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
802DLLEXPORT 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 */
907static
908uint8_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
925DLLEXPORT 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
976DLLEXPORT 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.