source: lib/protocols.c @ a81d2fc

4.0.1-hotfixescachetimestampsdevelopdpdk-ndagetsilivegetfragoffhelplibtrace4ndag_formatpfringrc-4.0.1rc-4.0.2rc-4.0.3rc-4.0.4ringdecrementfixringperformanceringtimestampfixes
Last change on this file since a81d2fc was dd06159, checked in by Perry Lorier <perry@…>, 14 years ago

Add support for cisco hdlc over pos

  • Property mode set to 100644
File size: 27.0 KB
Line 
1/* This file has the various helper functions used to decode various protocols
2 *
3 * $Id$
4 */ 
5#include "libtrace.h"
6#include "libtrace_int.h"
7#include <assert.h>
8#include <stdio.h>
9
10#ifndef WIN32
11#include <net/if_arp.h>
12#endif
13
14#ifndef ARPHRD_ETHER
15#define ARPHRD_ETHER    1               /* Ethernet 10/100Mbps.  */
16#endif
17
18#ifndef ARPHRD_PPP
19#define ARPHRD_PPP      512
20#endif
21
22
23static void *trace_get_payload_from_llcsnap(void *link,
24                uint16_t *type, uint32_t *remaining);
25
26/* Returns the payload from 802.3 ethernet.  Type optionally returned in
27 * "type" in host byte order.  This will return a vlan header.
28 */
29static void *trace_get_payload_from_ethernet(void *ethernet, 
30                uint16_t *type,
31                uint32_t *remaining)
32{
33        libtrace_ether_t *eth = (libtrace_ether_t*)ethernet;
34
35        if (remaining) {
36                if (*remaining < sizeof(*eth))
37                        return NULL;
38                *remaining-=sizeof(*eth);
39        }
40
41        if (type)
42                *type = ntohs(eth->ether_type);
43
44        return (void*)((char *)eth + sizeof(*eth));
45}
46
47/* skip any 802.1q headers if necessary
48 * type is input/output
49 */
50void *trace_get_vlan_payload_from_ethernet_payload(void *ethernet, uint16_t *type,
51                uint32_t *remaining)
52{
53        assert(type && "You must pass a type in!");
54
55        if (*type == 0x8100) {
56                libtrace_8021q_t *vlanhdr = (libtrace_8021q_t *)ethernet;
57
58                if (remaining) {
59                        if (*remaining < sizeof(libtrace_8021q_t))
60                                return NULL;
61
62                        *remaining=*remaining-sizeof(libtrace_8021q_t);
63                }
64
65                *type = ntohs(vlanhdr->vlan_ether_type);
66
67                return (void*)((char *)ethernet + sizeof(*vlanhdr));
68        }
69
70        return ethernet;
71}
72
73/* skip any MPLS headers if necessary, guessing what the next type is
74 * type is input/output.  If the next type is "ethernet" this will
75 * return a type of 0x0000.
76 */
77static void *trace_get_mpls_payload_from_ethernet_payload(void *ethernet,
78                uint16_t *type, uint32_t *remaining)
79{
80        assert(type && "You must pass a type in!");
81
82        if (*type == 0x8847) {
83                if ((((char*)ethernet)[2]&0x01)==0) {
84                        *type = 0x8847;
85                }
86                else {
87                        if (!remaining || *remaining>=5) {
88                                switch (((char*)ethernet)[4]&0xF0) {
89                                        case 0x40:
90                                                *type = 0x0800;
91                                                break;
92                                        case 0x60:
93                                                *type = 0x86DD;
94                                                break;
95                                        default:
96                                                /* Ethernet */
97                                                *type = 0;
98                                }
99                        }
100                }
101                ethernet=(char*)ethernet+4;
102                if (remaining) {
103                        if (*remaining<4)
104                                return NULL;
105                        else
106                                *remaining-=4;
107                }
108
109
110                return ethernet;
111        }
112        else
113                return NULL;
114}
115
116static void *trace_get_payload_from_80211(void *link, uint16_t *type, uint32_t *remaining)
117{
118        libtrace_80211_t *wifi;
119        uint16_t *eth; /* ethertype */
120        int8_t extra = 0; /* how many QoS bytes to skip */
121       
122        if (remaining && *remaining < sizeof(libtrace_80211_t))
123                return NULL;
124
125        wifi=(libtrace_80211_t*)link;
126
127        /* Data packet? */
128        if (wifi->type != 2) {
129                return NULL;
130        }
131
132        /* If FromDS and ToDS are both set then we have a four-address
133         * frame. Otherwise we have a three-address frame */
134        if (!(wifi->to_ds && wifi->from_ds)) 
135                extra -= 6; 
136       
137        /* Indicates QoS field present, see IEEE802.11e-2005 pg 21 */
138        if (wifi->subtype & 0x8) 
139                extra += 2;
140
141        if (remaining && *remaining < sizeof(*eth))
142                return NULL;
143
144        eth=(uint16_t *)((char*)wifi+sizeof(*wifi)+extra);
145       
146        if (*eth == 0xaaaa)
147                /* Payload contains an 802.2 LLC/SNAP frame */
148                return trace_get_payload_from_llcsnap((void *)eth, type, remaining);
149                       
150        /* Otherwise we assume an Ethernet II frame */
151        if (type) *type=ntohs(*eth);
152        if (remaining) *remaining = *remaining - sizeof(libtrace_80211_t) - extra - sizeof(*eth);
153       
154        return (void*)((char*)eth+sizeof(*eth));
155}
156
157/* NB: type is returned as an ARPHRD_ type for SLL*/
158void *trace_get_payload_from_linux_sll(const void *link,
159                uint16_t *type, uint32_t *remaining) 
160{
161        libtrace_sll_header_t *sll;
162
163        sll = (libtrace_sll_header_t*) link;
164
165        if (remaining) {
166                if (*remaining < sizeof(*sll))
167                        return NULL;
168                *remaining-=sizeof(*sll);
169        }
170
171        /* What kind of wacked out header, has this in host order?! */
172        if (type) *type = ntohs(sll->hatype);
173
174        return (void*)((char*)sll+sizeof(*sll));
175
176}
177
178DLLEXPORT
179void *trace_get_payload_from_atm(void *link,
180                uint8_t *type, uint32_t *remaining)
181{
182        libtrace_atm_capture_cell_t *cell;
183        if (remaining && *remaining<sizeof(libtrace_atm_capture_cell_t))
184                return NULL;
185        cell=(libtrace_atm_capture_cell_t*)link;
186
187        if (type)
188                *type=cell->pt;
189
190        if (remaining)
191                *remaining-=sizeof(libtrace_atm_capture_cell_t);
192
193        return ((char*)link)+sizeof(libtrace_atm_capture_cell_t);
194}
195
196static void *trace_get_payload_from_llcsnap(void *link,
197                uint16_t *type, uint32_t *remaining)
198{
199        /* 64 byte capture. */
200        libtrace_llcsnap_t *llc = (libtrace_llcsnap_t*)link;
201
202        if (remaining) {
203                if (*remaining < sizeof(libtrace_llcsnap_t))
204                        return NULL;
205                *remaining-=(sizeof(libtrace_llcsnap_t));
206        }
207
208        llc = (libtrace_llcsnap_t*)((char *)llc);
209
210        if (type) *type = ntohs(llc->type);
211
212        return (void*)((char*)llc+sizeof(*llc));
213}
214
215static void *trace_get_payload_from_ppp(void *link, 
216                uint16_t *type, uint32_t *remaining)
217{
218        /* 64 byte capture. */
219        libtrace_ppp_t *ppp = (libtrace_ppp_t*)link;
220
221        if (remaining) {
222                if (*remaining < sizeof(libtrace_ppp_t))
223                        return NULL;
224                *remaining-=sizeof(libtrace_ppp_t);
225        }
226
227        if (type) {
228                switch(ntohs(ppp->protocol)) {
229                        case 0x0021: *type = 0x0800; break;
230                }
231        }
232
233
234        return (void*)((char *)ppp+sizeof(*ppp));
235}
236
237typedef struct libtrace_chdlc_t {
238        uint8_t address;        /** 0xF0 for unicast, 0xF8 for multicast */
239        uint8_t control;
240        uint16_t ethertype;
241} libtrace_chdlc_t;
242
243static void *trace_get_payload_from_chdlc(void *link, 
244                uint16_t *type, uint32_t *remaining)
245{
246        libtrace_chdlc_t *chdlc = (libtrace_chdlc_t*)link;
247
248        if (remaining) {
249                if (*remaining < sizeof(libtrace_chdlc_t))
250                        return NULL;
251                *remaining-=sizeof(libtrace_chdlc_t);
252        }
253
254        if (type) {
255                *type=ntohs(chdlc->ethertype);
256        }
257
258
259        return (void*)((char *)chdlc+sizeof(*chdlc));
260}
261
262static void *trace_get_payload_from_pflog(void *link,
263                uint16_t *type, uint32_t *remaining)
264{
265        libtrace_pflog_header_t *pflog = (libtrace_pflog_header_t*)link;
266    if (remaining) {
267                if (*remaining<sizeof(*pflog)) 
268                        return NULL;
269                *remaining-=sizeof(*pflog);
270        }
271        if (type) {
272                switch(pflog->af) {
273                        case AF_INET6: *type=0x86DD; break;
274                        case AF_INET:  *type=0x0800; break;
275                        default:
276                                      /* Unknown */
277                                      return NULL;
278                }
279        }
280        return (void*)((char*)pflog+ sizeof(*pflog));
281}
282
283/* Returns the 'payload' of the prism header, which is the 802.11 frame */
284static void *trace_get_payload_from_prism (const void *link,
285                libtrace_linktype_t *type, uint32_t *remaining)
286{
287        if (remaining) {
288                if (*remaining<144) 
289                        return NULL;
290                *remaining-=144;
291        }
292
293        if (type) *type = TRACE_TYPE_80211;
294
295        return (void *) ((char*)link+144);
296}
297
298/* Returns the 'payload' of the radiotap header, which is the 802.11 frame */
299static void *trace_get_payload_from_radiotap (const void *link, 
300                libtrace_linktype_t *type, uint32_t *remaining)
301{
302        struct libtrace_radiotap_t *rtap = (struct libtrace_radiotap_t*)link;
303        uint16_t rtaplen = bswap_le_to_host16(rtap->it_len);
304        if (remaining) {
305                if (*remaining < rtaplen)
306                        return NULL;
307                *remaining -= rtaplen;
308        }
309
310        if (type) *type = TRACE_TYPE_80211;
311
312        return (void*) ((char*)link + rtaplen);
313}
314
315void *trace_get_payload_from_link(void *link, libtrace_linktype_t linktype, 
316                uint16_t *type, uint32_t *remaining)
317{
318        void *l = NULL;
319        uint16_t dummytype;
320       
321        switch(linktype) {
322                case TRACE_TYPE_80211_PRISM:
323                        l = trace_get_payload_from_prism(link,&linktype,remaining);
324                        return (l ? trace_get_payload_from_link(l, TRACE_TYPE_80211, type, remaining) : NULL);
325                case TRACE_TYPE_80211_RADIO:
326                        l = trace_get_payload_from_radiotap(link,&linktype,remaining);
327                        return (l ? trace_get_payload_from_link(l, TRACE_TYPE_80211, type, remaining) : NULL);
328                case TRACE_TYPE_80211:
329                        return trace_get_payload_from_80211(link,type,remaining);
330
331                case TRACE_TYPE_ETH:
332                        return trace_get_payload_from_ethernet(link,type,remaining);
333                case TRACE_TYPE_NONE:
334                        if ((*(char*)link&0xF0) == 0x40)
335                                *type=0x0800;
336                        else if ((*(char*)link&0xF0) == 0x60)
337                                *type=0x86DD;
338                        return link; /* I love the simplicity */
339                case TRACE_TYPE_LINUX_SLL:
340                        l = trace_get_payload_from_linux_sll(link,&dummytype,remaining);
341                        if (type) *type = dummytype;
342                        return (l ? trace_get_payload_from_link(l,
343                                                arphrd_type_to_libtrace(dummytype), type, remaining) : NULL);
344                       
345                case TRACE_TYPE_PFLOG:
346                        return trace_get_payload_from_pflog(link,type,remaining);
347                case TRACE_TYPE_PPP:
348                        return trace_get_payload_from_ppp(link,type,remaining);
349                case TRACE_TYPE_ATM:
350                        l=trace_get_payload_from_atm(link,NULL,remaining);
351                        return (l ? trace_get_payload_from_llcsnap(l,
352                                                type, remaining):NULL);
353                case TRACE_TYPE_DUCK:
354                        return NULL; /* duck packets have no payload! */
355                case TRACE_TYPE_METADATA:
356                        return NULL; /* The payload is in these packets does
357                                        not correspond to a genuine link-layer
358                                        */
359                default:
360                        break;
361        }
362        fprintf(stderr, "Don't understand link layer type %i in trace_get_payload_from_link()\n",
363                linktype);
364        return NULL;
365}
366
367libtrace_ip_t *trace_get_ip(libtrace_packet_t *packet) 
368{
369        uint16_t ethertype;
370        void *ret;
371
372        uint32_t remaining = trace_get_capture_length(packet);
373
374        ret = trace_get_layer3(packet,&ethertype,&remaining);
375
376        if (!ret || ethertype!=0x0800)
377                return NULL;
378
379        /* Not an IPv4 packet */
380        if (((libtrace_ip_t*)ret)->ip_v != 4)
381                return NULL;
382
383        return (libtrace_ip_t*)ret;
384}
385
386libtrace_ip6_t *trace_get_ip6(libtrace_packet_t *packet) 
387{
388        uint16_t ethertype;
389        void *ret;
390
391        uint32_t remaining = trace_get_capture_length(packet);
392
393        ret = trace_get_layer3(packet,&ethertype,&remaining);
394
395        if (!ret || ethertype!=0x86DD)
396                return NULL;
397
398        return (libtrace_ip6_t*)ret;
399}
400
401#define SW_IP_OFFMASK 0xff1f
402
403DLLEXPORT void *trace_get_payload_from_ip(libtrace_ip_t *ipptr, uint8_t *prot,
404                uint32_t *remaining) 
405{
406        void *trans_ptr = 0;
407
408        if ((ntohs(ipptr->ip_off) & SW_IP_OFFMASK) != 0) {
409                return NULL;
410        }
411
412        if (remaining) {
413                if (*remaining<(ipptr->ip_hl*4U)) {
414                        return NULL;
415                }
416                *remaining-=(ipptr->ip_hl * 4);
417        }
418
419        trans_ptr = (void *)((char *)ipptr + (ipptr->ip_hl * 4));
420
421        if (prot) *prot = ipptr->ip_p;
422
423        return trans_ptr;
424}
425
426void *trace_get_payload_from_ip6(libtrace_ip6_t *ipptr, uint8_t *prot,
427                uint32_t *remaining) 
428{
429        void *payload = (char*)ipptr+sizeof(libtrace_ip6_t);
430        uint8_t nxt = ipptr->nxt;
431
432        if (remaining) {
433                if (*remaining<sizeof(libtrace_ip6_t))
434                        return NULL;
435                *remaining-=sizeof(libtrace_ip6_t);
436        }
437
438        while(1) {
439                switch (nxt) {
440                        case 0: /* hop by hop options */
441                        case 43: /* routing */
442                        case 44: /* fragment */
443                        case 50: /* ESP */
444                        case 51: /* AH */
445                        case 60: /* Destination options */
446                                {
447                                        uint16_t len=((libtrace_ip6_ext_t*)payload)->len
448                                        +sizeof(libtrace_ip6_ext_t);
449
450                                        if (remaining) {
451                                                if (*remaining < len) {
452                                                        /* Snap too short */
453                                                        return NULL;
454                                                }
455                                                *remaining-=len;
456                                        }
457
458                                        payload=(char*)payload+len;
459                                        nxt=((libtrace_ip6_ext_t*)payload)->nxt;
460                                        continue;
461                                }
462                        default:
463                                if (prot) *prot=nxt;
464                                return payload;
465                }
466        }
467}
468
469DLLEXPORT void *trace_get_packet_meta(const libtrace_packet_t *packet, 
470                libtrace_linktype_t *linktype,
471                uint32_t *remaining)
472{
473        uint32_t dummyrem;
474
475        assert(packet != NULL);
476        assert(linktype != NULL);
477       
478        if (remaining == NULL) 
479                remaining = &dummyrem;
480       
481        void *pktbuf = trace_get_packet_buffer(packet, linktype, remaining);
482        switch (*linktype) {
483                case TRACE_TYPE_LINUX_SLL:
484                case TRACE_TYPE_80211_RADIO:
485                case TRACE_TYPE_80211_PRISM:
486                        return pktbuf;
487                /* Non metadata packets */
488                case TRACE_TYPE_HDLC_POS:
489                case TRACE_TYPE_ETH:
490                case TRACE_TYPE_ATM:
491                case TRACE_TYPE_80211:
492                case TRACE_TYPE_NONE:
493                case TRACE_TYPE_PFLOG:
494                case TRACE_TYPE_POS:
495                case TRACE_TYPE_AAL5:
496                case TRACE_TYPE_DUCK:
497                case TRACE_TYPE_LLCSNAP:
498                case TRACE_TYPE_PPP:
499                case TRACE_TYPE_METADATA:
500                        return NULL;
501        }
502
503        /* Shouldn't get here */
504        return NULL;
505}
506
507DLLEXPORT void *trace_get_payload_from_meta(const void *meta,
508                libtrace_linktype_t *linktype,
509                uint32_t *remaining)
510{
511        void *nexthdr; 
512        uint16_t arphrd;
513       
514        assert(meta != NULL);
515        assert(linktype != NULL);
516        assert(remaining != NULL);
517       
518        switch(*linktype) {
519                case TRACE_TYPE_LINUX_SLL:
520                        nexthdr = trace_get_payload_from_linux_sll(meta,
521                                        &arphrd, remaining);
522                        *linktype = arphrd_type_to_libtrace(arphrd);
523                        return nexthdr;
524                case TRACE_TYPE_80211_RADIO:
525                        nexthdr = trace_get_payload_from_radiotap(meta,
526                                        linktype, remaining);
527                        return nexthdr;
528                case TRACE_TYPE_80211_PRISM:
529                        nexthdr = trace_get_payload_from_prism(meta,
530                                        linktype, remaining);
531                        return nexthdr;
532                case TRACE_TYPE_HDLC_POS:
533                case TRACE_TYPE_ETH:
534                case TRACE_TYPE_ATM:
535                case TRACE_TYPE_80211:
536                case TRACE_TYPE_NONE:
537                case TRACE_TYPE_PFLOG:
538                case TRACE_TYPE_POS:
539                case TRACE_TYPE_AAL5:
540                case TRACE_TYPE_DUCK:
541                case TRACE_TYPE_LLCSNAP:
542                case TRACE_TYPE_PPP:
543                case TRACE_TYPE_METADATA:
544                        /* In this case, the pointer passed in does not point
545                         * to a metadata header and so we cannot get the
546                         * payload.
547                         */
548                        return NULL;
549        }
550        /* Shouldn't get here */
551        return NULL;
552}
553
554DLLEXPORT void *trace_get_layer2(const libtrace_packet_t *packet,
555                libtrace_linktype_t *linktype,
556                uint32_t *remaining) 
557{
558        uint32_t dummyrem;
559       
560        assert(packet != NULL);
561        assert(linktype != NULL);
562
563        if (remaining == NULL)
564                remaining = &dummyrem;
565       
566        void *meta = trace_get_packet_meta(packet, linktype, remaining);
567
568        /* If there are no meta-data headers, we just return the start of the
569         * packet buffer, along with the linktype, etc.
570         */
571        if (meta == NULL) 
572                return trace_get_packet_buffer(packet, linktype, remaining);
573       
574        /* If there are meta-data headers, we need to skip over them until we
575         * find a non-meta data header and return that.
576         */
577        for(;;) {
578                void *nexthdr = trace_get_payload_from_meta(meta, 
579                                linktype, remaining);
580                if (nexthdr == NULL)
581                        return meta;
582                meta = nexthdr;
583        }
584}
585
586DLLEXPORT void *trace_get_payload_from_layer2(void *link,
587                libtrace_linktype_t linktype,
588                uint16_t *ethertype,
589                uint32_t *remaining)
590{
591        void *l;
592        switch(linktype) {
593                /* Packet Metadata headers, not layer2 headers */
594                case TRACE_TYPE_80211_PRISM:
595                case TRACE_TYPE_80211_RADIO:
596                case TRACE_TYPE_LINUX_SLL:
597                        return NULL;
598
599                /* duck packets have no payload! */
600                case TRACE_TYPE_DUCK:
601                        return NULL;
602
603                /* The payload is in these packets does
604                   not correspond to a genuine link-layer
605                   */
606                case TRACE_TYPE_METADATA:
607                        return NULL;
608
609                case TRACE_TYPE_80211:
610                        return trace_get_payload_from_80211(link,ethertype,remaining);
611                case TRACE_TYPE_ETH:
612                        return trace_get_payload_from_ethernet(link,ethertype,remaining);
613                case TRACE_TYPE_NONE:
614                        if ((*(char*)link&0xF0) == 0x40)
615                                *ethertype=0x0800;
616                        else if ((*(char*)link&0xF0) == 0x60)
617                                *ethertype=0x86DD;
618                        return link; /* I love the simplicity */
619                case TRACE_TYPE_PFLOG:
620                        return trace_get_payload_from_pflog(link,ethertype,remaining);
621                case TRACE_TYPE_PPP:
622                        return trace_get_payload_from_ppp(link,ethertype,remaining);
623                case TRACE_TYPE_ATM:
624                        l=trace_get_payload_from_atm(link,NULL,remaining);
625                        /* FIXME: We shouldn't skip llcsnap here, we should return
626                         * an ethertype for it (somehow)
627                         */
628                        return (l ? trace_get_payload_from_llcsnap(l,
629                                                ethertype, remaining):NULL);
630                case TRACE_TYPE_LLCSNAP:
631                        return trace_get_payload_from_llcsnap(link,ethertype,remaining);
632
633                case TRACE_TYPE_HDLC_POS:
634                        return trace_get_payload_from_chdlc(link,ethertype,
635                                        remaining);
636                /* TODO: Unsupported */
637                case TRACE_TYPE_POS:
638                case TRACE_TYPE_AAL5:
639                        return NULL;
640        }
641        return NULL;
642
643}
644
645DLLEXPORT void *trace_get_layer3(const libtrace_packet_t *packet,
646                uint16_t *ethertype,
647                uint32_t *remaining)
648{
649        void *iphdr;
650        uint16_t dummy_ethertype;
651        void *link;
652        uint32_t dummy_remaining;
653        libtrace_linktype_t linktype;
654
655        if (!ethertype) ethertype=&dummy_ethertype;
656
657        if (!remaining) remaining=&dummy_remaining;
658
659        /* use l3 cache */
660        if (packet->l3_header)
661        {
662                link = trace_get_packet_buffer(packet,&linktype,remaining);
663
664                if (!link)
665                        return NULL;
666
667                *ethertype = packet->l3_ethertype;
668                *remaining -= (packet->l3_header - link);
669
670                return packet->l3_header;
671        }
672
673        link = trace_get_layer2(packet,&linktype,remaining);
674
675        iphdr = trace_get_payload_from_layer2(
676                        link,
677                        linktype,
678                        ethertype,
679                        remaining);
680
681        if (!iphdr)
682                return NULL;
683
684        for(;;) {
685                switch(*ethertype) {
686                case 0x8100: /* VLAN */
687                        iphdr=trace_get_vlan_payload_from_ethernet_payload(
688                                          iphdr,ethertype,NULL);
689                        continue;
690                case 0x8847: /* MPLS */
691                        iphdr=trace_get_mpls_payload_from_ethernet_payload(
692                                          iphdr,ethertype,NULL);
693
694                        if (iphdr && ethertype == 0x0) {
695                                iphdr=trace_get_payload_from_ethernet(
696                                                iphdr,ethertype,NULL);
697                        }
698                        continue;
699                default:
700                        break;
701                }
702
703                break;
704        }
705
706        /* Store values in the cache for later */
707        /* Cast away constness, nasty, but this is just a cache */
708        ((libtrace_packet_t*)packet)->l3_ethertype = *ethertype;
709        ((libtrace_packet_t*)packet)->l3_header = iphdr;
710
711        return iphdr;
712}
713
714DLLEXPORT void *trace_get_transport(const libtrace_packet_t *packet, 
715                uint8_t *proto,
716                uint32_t *remaining
717                ) 
718{
719        uint8_t dummy_proto;
720        uint16_t ethertype;
721        uint32_t dummy_remaining;
722        void *transport;
723
724        if (!proto) proto=&dummy_proto;
725
726        if (!remaining) remaining=&dummy_remaining;
727
728        transport = trace_get_layer3(packet,&ethertype,remaining);
729
730        if (!transport)
731                return NULL;
732
733        switch (ethertype) {
734                case 0x0800: /* IPv4 */
735                        transport=trace_get_payload_from_ip(
736                                (libtrace_ip_t*)transport, proto, remaining);
737                        /* IPv6 */
738                        if (transport && *proto == 41) {
739                                transport=trace_get_payload_from_ip6(
740                                 (libtrace_ip6_t*)transport, proto,remaining);
741                        }
742                        return transport;
743                case 0x86DD: /* IPv6 */
744                        return trace_get_payload_from_ip6(
745                                (libtrace_ip6_t*)transport, proto, remaining);
746                       
747                default:
748                        *proto=0;
749                        return NULL;
750        }
751
752}
753
754DLLEXPORT libtrace_tcp_t *trace_get_tcp(libtrace_packet_t *packet) {
755        uint8_t proto;
756        libtrace_tcp_t *tcp;
757
758        tcp=(libtrace_tcp_t*)trace_get_transport(packet,&proto,NULL);
759
760        if (!tcp || proto != 6)
761                return NULL;
762
763        return (libtrace_tcp_t*)tcp;
764}
765
766DLLEXPORT libtrace_tcp_t *trace_get_tcp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
767{
768        libtrace_tcp_t *tcpptr = 0;
769
770        if (ip->ip_p == 6)  {
771                tcpptr = (libtrace_tcp_t *)
772                        trace_get_payload_from_ip(ip, NULL, remaining);
773        }
774
775        return tcpptr;
776}
777
778DLLEXPORT libtrace_udp_t *trace_get_udp(libtrace_packet_t *packet) {
779        uint8_t proto;
780        libtrace_udp_t *udp;
781
782        udp=(libtrace_udp_t*)trace_get_transport(packet,&proto,NULL);
783
784        if (!udp || proto != 17)
785                return NULL;
786
787        return udp;
788}
789
790DLLEXPORT libtrace_udp_t *trace_get_udp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
791{
792        libtrace_udp_t *udpptr = 0;
793
794        if (ip->ip_p == 17) {
795                udpptr = (libtrace_udp_t *)
796                        trace_get_payload_from_ip(ip, NULL, remaining);
797        }
798
799        return udpptr;
800}
801
802DLLEXPORT libtrace_icmp_t *trace_get_icmp(libtrace_packet_t *packet) {
803        uint8_t proto;
804        libtrace_icmp_t *icmp;
805
806        icmp=(libtrace_icmp_t*)trace_get_transport(packet,&proto,NULL);
807
808        if (!icmp || proto != 1)
809                return NULL;
810
811        return icmp;
812}
813
814DLLEXPORT libtrace_icmp_t *trace_get_icmp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
815{
816        libtrace_icmp_t *icmpptr = 0;
817
818        if (ip->ip_p == 1)  {
819                icmpptr = (libtrace_icmp_t *)trace_get_payload_from_ip(ip, 
820                                NULL, remaining);
821        }
822
823        return icmpptr;
824}
825
826DLLEXPORT void *trace_get_payload_from_udp(libtrace_udp_t *udp, uint32_t *remaining)
827{
828        if (remaining) {
829                if (*remaining < sizeof(libtrace_udp_t))
830                        return NULL;
831                *remaining-=sizeof(libtrace_udp_t);
832        }
833        return (void*)((char*)udp+sizeof(libtrace_udp_t));
834}
835
836DLLEXPORT void *trace_get_payload_from_tcp(libtrace_tcp_t *tcp, uint32_t *remaining)
837{
838        unsigned int dlen = tcp->doff*4;
839        if (remaining) {
840                if (*remaining < dlen)
841                        return NULL;
842                *remaining-=dlen;
843        }
844        return (void *)((char *)tcp+dlen);
845}
846
847DLLEXPORT void *trace_get_payload_from_icmp(libtrace_icmp_t *icmp, uint32_t *remaining)
848{
849        if (remaining) {
850                if (*remaining < sizeof(libtrace_icmp_t))
851                        return NULL;
852                *remaining-=sizeof(libtrace_icmp_t);
853        }
854        return (char*)icmp+sizeof(libtrace_icmp_t);
855}
856
857struct ports_t {
858        uint16_t src;
859        uint16_t dst;
860};
861
862/* Return the client port
863 */
864DLLEXPORT uint16_t trace_get_source_port(const libtrace_packet_t *packet)
865{
866        uint32_t remaining;
867        const struct ports_t *port = 
868                (const struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
869                        NULL, &remaining);
870
871        /* snapped too early */
872        if (remaining<2)
873                return 0;
874
875        if (port)
876                return ntohs(port->src);
877        else
878                return 0;
879}
880
881/* Same as get_source_port except use the destination port */
882DLLEXPORT uint16_t trace_get_destination_port(const libtrace_packet_t *packet)
883{
884        uint32_t remaining;
885        struct ports_t *port = 
886                (struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
887                        NULL, &remaining);
888        /* snapped to early */
889        if (remaining<4)
890                return 0;
891
892        if (port)
893                return ntohs(port->dst);
894        else
895                return 0;
896}
897
898/* Take a pointer to the start of an IEEE 802.11 MAC frame and return a pointer
899 * to the source MAC address. 
900 * If the frame does not contain a sender address, e.g. ACK frame, return NULL.
901 * If the frame is a 4-address WDS frame, return TA, i.e. addr2.
902 * NB: This function decodes the 802.11 header, so it assumes that there are no
903 * bit-errors. If there are, all bets are off.
904 */
905static
906uint8_t *get_source_mac_from_wifi(void *wifi) {
907        if (wifi == NULL) return NULL;
908        struct libtrace_80211_t *w = (struct libtrace_80211_t *) wifi;
909       
910        /* If the frame is of type CTRL */
911        if (w->type == 0x1) 
912                /* If bit 2 of the subtype field is zero, this indicates that
913                 * there is no transmitter address, i.e. the frame is either an
914                 * ACK or a CTS frame */
915                if ((w->subtype & 0x2) == 0)
916                        return NULL;
917
918        /* Always return the address of the transmitter, i.e. address 2 */
919        return (uint8_t *) &w->mac2;
920}
921
922DLLEXPORT uint8_t *trace_get_source_mac(libtrace_packet_t *packet) {
923        void *link;
924        uint32_t remaining;
925        libtrace_linktype_t linktype;
926        assert(packet);
927        link = trace_get_layer2(packet,&linktype,&remaining);
928
929        if (!link)
930                return NULL;
931       
932        switch (linktype) {
933                case TRACE_TYPE_ETH:
934                        return (uint8_t *)&(((libtrace_ether_t*)link)->ether_shost);
935                case TRACE_TYPE_80211:
936                        return get_source_mac_from_wifi(link);
937                /* These packets don't have MAC addresses */
938                case TRACE_TYPE_POS:
939                case TRACE_TYPE_NONE:
940                case TRACE_TYPE_HDLC_POS:
941                case TRACE_TYPE_PFLOG:
942                case TRACE_TYPE_ATM:
943                case TRACE_TYPE_DUCK:
944                case TRACE_TYPE_METADATA:
945                case TRACE_TYPE_AAL5:
946                case TRACE_TYPE_LLCSNAP:
947                case TRACE_TYPE_PPP:
948                        return NULL;
949
950                /* Metadata headers should already be skipped */
951                case TRACE_TYPE_LINUX_SLL:
952                case TRACE_TYPE_80211_PRISM:
953                case TRACE_TYPE_80211_RADIO:
954                        assert(!"Metadata headers should already be skipped");
955                        break;
956        }
957        fprintf(stderr,"%s not implemented for linktype %i\n", __func__, linktype);
958        assert(0);
959        return NULL;
960}
961
962DLLEXPORT uint8_t *trace_get_destination_mac(libtrace_packet_t *packet) 
963{
964        void *link;
965        libtrace_linktype_t linktype;
966        uint32_t remaining;
967
968        link = trace_get_layer2(packet,&linktype,&remaining);
969
970        libtrace_80211_t *wifi;
971        libtrace_ether_t *ethptr = (libtrace_ether_t*)link;
972
973
974        if (!link)
975                return NULL;
976
977        switch (linktype) {
978                case TRACE_TYPE_80211:
979                        wifi=(libtrace_80211_t*)link;
980                        return (uint8_t*)&wifi->mac1;
981                case TRACE_TYPE_80211_RADIO:
982                        wifi=(libtrace_80211_t*)trace_get_payload_from_radiotap(
983                                        link,NULL,NULL);
984                        return (uint8_t*)&wifi->mac1;
985                case TRACE_TYPE_80211_PRISM:
986                        wifi=(libtrace_80211_t*)((char*)link+144);
987                        return (uint8_t*)&wifi->mac1;
988                case TRACE_TYPE_ETH:
989                        return (uint8_t*)&ethptr->ether_dhost;
990                case TRACE_TYPE_POS:
991                case TRACE_TYPE_NONE:
992                case TRACE_TYPE_ATM:
993                case TRACE_TYPE_HDLC_POS:
994                case TRACE_TYPE_LINUX_SLL:
995                case TRACE_TYPE_PFLOG:
996                case TRACE_TYPE_DUCK:
997                case TRACE_TYPE_METADATA:
998                        /* No MAC address */
999                        return NULL;
1000                default:
1001                        break;
1002        }
1003        fprintf(stderr,"Not implemented\n");
1004        assert(0);
1005        return NULL;
1006}
1007
1008DLLEXPORT struct sockaddr *trace_get_source_address(
1009                const libtrace_packet_t *packet, struct sockaddr *addr)
1010{
1011        uint16_t ethertype;
1012        uint32_t remaining;
1013        void *l3;
1014        struct ports_t *ports;
1015        static struct sockaddr_storage dummy;
1016
1017        if (!addr)
1018                addr=(struct sockaddr*)&dummy;
1019
1020        l3 = trace_get_layer3(packet,&ethertype,&remaining);
1021
1022        if (!l3)
1023                return NULL;
1024
1025        switch (ethertype) {
1026                case 0x0800: /* IPv4 */
1027                {
1028                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
1029                        libtrace_ip_t *ip = (libtrace_ip_t*)l3;
1030                        ports = (struct ports_t*)
1031                                trace_get_payload_from_ip(ip,NULL,&remaining);
1032                        addr4->sin_family=AF_INET;
1033                        if (ports && remaining>=sizeof(*ports))
1034                                addr4->sin_port=ports->src;
1035                        else
1036                                addr4->sin_port=0;
1037                        addr4->sin_addr=ip->ip_src;
1038                        return addr;
1039                }
1040                case 0x86DD: /* IPv6 */
1041                {
1042                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
1043                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)l3;
1044                        ports = (struct ports_t*)
1045                                trace_get_payload_from_ip6(ip6,NULL,&remaining);
1046                        addr6->sin6_family=AF_INET6;
1047                        if (ports && remaining>=sizeof(*ports))
1048                                addr6->sin6_port=ports->src;
1049                        else
1050                                addr6->sin6_port=0;
1051                        addr6->sin6_flowinfo=0;
1052                        addr6->sin6_addr=ip6->ip_src;
1053                        return addr;
1054                }
1055                default:
1056                        return NULL;
1057        }
1058}
1059
1060DLLEXPORT struct sockaddr *trace_get_destination_address(
1061                const libtrace_packet_t *packet, struct sockaddr *addr)
1062{
1063        uint16_t ethertype;
1064        uint32_t remaining;
1065        void *l3;
1066        struct ports_t *ports;
1067        static struct sockaddr_storage dummy;
1068
1069        if (!addr)
1070                addr=(struct sockaddr*)&dummy;
1071
1072        l3 = trace_get_layer3(packet,&ethertype,&remaining);
1073
1074        if (!l3)
1075                return NULL;
1076
1077        switch (ethertype) {
1078                case 0x0800: /* IPv4 */
1079                {
1080                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
1081                        libtrace_ip_t *ip = (libtrace_ip_t*)l3;
1082                        ports = (struct ports_t*)
1083                                trace_get_payload_from_ip(ip,NULL,&remaining);
1084                        addr4->sin_family=AF_INET;
1085                        if (ports && remaining>=sizeof(*ports))
1086                                addr4->sin_port=ports->dst;
1087                        else
1088                                addr4->sin_port=0;
1089                        addr4->sin_addr=ip->ip_dst;
1090                        return addr;
1091                }
1092                case 0x86DD: /* IPv6 */
1093                {
1094                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
1095                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)l3;
1096                        ports = (struct ports_t*)
1097                                trace_get_payload_from_ip6(ip6,NULL,&remaining);
1098                        addr6->sin6_family=AF_INET6;
1099                        if (ports && remaining>=sizeof(*ports))
1100                                addr6->sin6_port=ports->dst;
1101                        else
1102                                addr6->sin6_port=0;
1103                        addr6->sin6_flowinfo=0;
1104                        addr6->sin6_addr=ip6->ip_dst;
1105                        return addr;
1106                }
1107                default:
1108                        return NULL;
1109        }
1110}
1111
1112/* parse an ip or tcp option
1113 * @param[in,out] ptr   the pointer to the current option
1114 * @param[in,out] len   the length of the remaining buffer
1115 * @param[out] type     the type of the option
1116 * @param[out] optlen   the length of the option
1117 * @param[out] data     the data of the option
1118 *
1119 * @returns bool true if there is another option (and the fields are filled in)
1120 *               or false if this was the last option.
1121 *
1122 * This updates ptr to point to the next option after this one, and updates
1123 * len to be the number of bytes remaining in the options area.  Type is updated
1124 * to be the code of this option, and data points to the data of this option,
1125 * with optlen saying how many bytes there are.
1126 *
1127 * @note Beware of fragmented packets.
1128 * @author Perry Lorier
1129 */
1130DLLEXPORT int trace_get_next_option(unsigned char **ptr,int *len,
1131                        unsigned char *type,
1132                        unsigned char *optlen,
1133                        unsigned char **data)
1134{
1135        if (*len<=0)
1136                return 0;
1137        *type=**ptr;
1138        switch(*type) {
1139                case 0: /* End of options */
1140                        return 0;
1141                case 1: /* Pad */
1142                        (*ptr)++;
1143                        (*len)--;
1144                        return 1;
1145                default:
1146                        *optlen = *(*ptr+1);
1147                        if (*optlen<2)
1148                                return 0; /* I have no idea wtf is going on
1149                                           * with these packets
1150                                           */
1151                        (*len)-=*optlen;
1152                        (*data)=(*ptr+2);
1153                        (*ptr)+=*optlen;
1154                        if (*len<0)
1155                                return 0;
1156                        return 1;
1157        }
1158        assert(0);
1159}
1160
1161
Note: See TracBrowser for help on using the repository browser.