source: trunk/lib/protocols.c @ 827

Last change on this file since 827 was 827, checked in by perry, 7 years ago

Add new DUCK link type (for duck packets obviously).
Make get_payload_from_link on a duck packet return "NULL" (no payload)
  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
File size: 16.2 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 "wag.h"
8#include <assert.h>
9#include <stdio.h>
10
11#ifndef WIN32
12#include <net/if_arp.h>
13#endif
14
15#ifndef ARPHRD_ETHER
16#define ARPHRD_ETHER    1               /* Ethernet 10/100Mbps.  */
17#endif
18
19#ifndef ARPHRD_PPP
20#define ARPHRD_PPP      512
21#endif
22
23
24/* Returns the payload from 802.3 ethernet.  Type optionally returned in
25 * "type" in host byte order.  This will return a vlan header.
26 */
27static void *trace_get_payload_from_ethernet(void *ethernet, 
28                uint16_t *type,
29                uint32_t *remaining)
30{
31        libtrace_ether_t *eth = (libtrace_ether_t*)ethernet;
32
33        if (remaining) {
34                if (*remaining < sizeof(*eth))
35                        return NULL;
36                *remaining-=sizeof(*eth);
37        }
38
39        if (type)
40                *type = ntohs(eth->ether_type);
41
42        return (void*)((char *)eth + sizeof(*eth));
43}
44
45/* skip any 802.1q headers if necessary
46 * type is input/output
47 */
48static void *trace_get_vlan_payload_from_ethernet_payload(void *ethernet, uint16_t *type,
49                uint32_t *remaining)
50{
51        assert(type && "You must pass a type in!");
52
53        if (*type == 0x8100) {
54                libtrace_8021q_t *vlanhdr = (libtrace_8021q_t *)ethernet;
55
56                if (remaining) {
57                        if (*remaining < sizeof(libtrace_8021q_t))
58                                return NULL;
59
60                        *remaining=*remaining-sizeof(libtrace_8021q_t);
61                }
62
63                *type = ntohs(vlanhdr->vlan_ether_type);
64
65                return (void*)((char *)ethernet + sizeof(*vlanhdr));
66        }
67
68        return ethernet;
69}
70
71static void *trace_get_payload_from_80211(void *link, uint16_t *type, uint32_t *remaining)
72{
73        libtrace_80211_t *wifi;
74        libtrace_802_11_payload_t *eth;
75
76        if (remaining && *remaining < sizeof(libtrace_80211_t))
77                return NULL;
78
79        wifi=(libtrace_80211_t*)link;
80
81        /* Data packet? */
82        if (wifi->type != 2) {
83                return NULL;
84        }
85
86        if (remaining && *remaining < sizeof(*eth))
87                return NULL;
88
89        eth=(libtrace_802_11_payload_t *)((char*)wifi+sizeof(*eth));
90
91        if (*type) *type=eth->type;
92
93        return eth;
94}
95
96static void *trace_get_payload_from_linux_sll(void *link,
97                uint16_t *type, uint32_t *remaining) 
98{
99        libtrace_sll_header_t *sll;
100        void *ret;
101
102        sll = (libtrace_sll_header_t*) link;
103
104        if (remaining) {
105                if (*remaining < sizeof(*sll))
106                        return NULL;
107                *remaining-=sizeof(*sll);
108        }
109
110        if (*type) *type = sll->protocol;
111
112        ret=(void*)((char*)sll+sizeof(*sll));
113
114        switch(sll->hatype) {
115                case ARPHRD_PPP:
116                        break;
117                case ARPHRD_ETHER:
118                        ret=trace_get_payload_from_ethernet(ret,type,remaining);
119        }
120
121        return ret;
122}
123
124static void *trace_get_payload_from_atm(void *link,
125                uint16_t *type, uint32_t *remaining)
126{
127        /* 64 byte capture. */
128        libtrace_llcsnap_t *llc = (libtrace_llcsnap_t*)link;
129
130        if (remaining) {
131                if (*remaining < sizeof(libtrace_llcsnap_t)+4)
132                        return NULL;
133                *remaining-=(sizeof(libtrace_llcsnap_t)+4);
134        }
135
136        /* advance the llc ptr +4 into the link layer.
137         * TODO: need to check what is in these 4 bytes.
138         * don't have time!
139         */
140        llc = (libtrace_llcsnap_t*)((char *)llc + 4);
141
142        if (*type) *type = ntohs(llc->type);
143
144        return (void*)((char*)llc+sizeof(*llc));
145}
146
147static void *trace_get_payload_from_pos(void *link, 
148                uint16_t *type, uint32_t *remaining)
149{
150        /* 64 byte capture. */
151        libtrace_pos_t *pos = (libtrace_pos_t*)link;
152
153        if (remaining) {
154                if (*remaining < sizeof(libtrace_pos_t))
155                        return NULL;
156                *remaining-=sizeof(libtrace_pos_t);
157        }
158
159        if (type) *type = ntohs(pos->ether_type);
160
161        return (void*)((char *)pos+sizeof(*pos));
162}
163
164static void *trace_get_payload_from_pflog(void *link,
165                uint16_t *type, uint32_t *remaining)
166{
167        libtrace_pflog_header_t *pflog = (libtrace_pflog_header_t*)link;
168        if (remaining) {
169                if (*remaining<sizeof(*pflog)) 
170                        return NULL;
171                *remaining-=sizeof(*pflog);
172        }
173        if (type) {
174                switch(pflog->af) {
175                        case AF_INET6: *type=0x86DD; break;
176                        case AF_INET:  *type=0x0800; break;
177                        default:
178                                      /* Unknown */
179                                      return NULL;
180                }
181        }
182        return (void*)((char*)pflog+ sizeof(*pflog));
183}
184
185void *trace_get_payload_from_link(void *link, libtrace_linktype_t linktype, 
186                uint16_t *type, uint32_t *remaining)
187{
188        switch(linktype) {
189                case TRACE_TYPE_80211_PRISM:
190                        return trace_get_payload_from_80211((char*)link+144,
191                                        type,remaining);
192                case TRACE_TYPE_80211:
193                        return trace_get_payload_from_80211(link,type,remaining);
194                case TRACE_TYPE_ETH:
195                        return trace_get_payload_from_ethernet(link,type,remaining);
196                case TRACE_TYPE_NONE:
197                        if ((*(char*)link&0xF0) == 0x40)
198                                *type=0x0800;
199                        else if ((*(char*)link&0xF0) == 0x60)
200                                *type=0x86DD;
201                        return link; /* I love the simplicity */
202                case TRACE_TYPE_LINUX_SLL:
203                        return trace_get_payload_from_linux_sll(link,type,remaining);
204                case TRACE_TYPE_PFLOG:
205                        return trace_get_payload_from_pflog(link,type,remaining);
206                case TRACE_TYPE_POS:
207                        return trace_get_payload_from_pos(link,type,remaining);
208                case TRACE_TYPE_ATM:
209                        return trace_get_payload_from_atm(link,type,remaining);
210                case TRACE_TYPE_DUCK:
211                        return NULL; /* duck packets have no payload! */
212        }
213        fprintf(stderr,"Don't understand link layer type %i in trace_get_payload_from_link()\n",
214                linktype);
215        return NULL;
216}
217
218libtrace_ip_t *trace_get_ip(libtrace_packet_t *packet) 
219{
220        uint16_t type;
221        void *link = trace_get_link(packet);
222        void *ret;
223
224        if (!link)
225                return NULL;
226       
227        ret=trace_get_payload_from_link(
228                        link,
229                        trace_get_link_type(packet),
230                        &type, NULL);
231
232        if (!ret)
233                return NULL;
234
235        ret=trace_get_vlan_payload_from_ethernet_payload(ret,&type,NULL);
236
237        if (!ret || type!=0x0800)
238                return NULL;
239
240        return (libtrace_ip_t*)ret;
241}
242
243libtrace_ip6_t *trace_get_ip6(libtrace_packet_t *packet) 
244{
245        uint16_t type;
246        void *link=trace_get_link(packet);
247        void *ret;
248       
249        if (!link)
250                return NULL;
251
252        ret=trace_get_payload_from_link(
253                        link,
254                        trace_get_link_type(packet),
255                        &type,NULL);
256
257        if (!ret)
258                return NULL;
259
260        ret=trace_get_vlan_payload_from_ethernet_payload(ret,&type,NULL);
261
262        if (!ret || type!=0x86DD)
263                return NULL;
264
265        return (libtrace_ip6_t*)ret;
266}
267
268#define SW_IP_OFFMASK 0xff1f
269
270DLLEXPORT void *trace_get_payload_from_ip(libtrace_ip_t *ipptr, uint8_t *prot,
271                uint32_t *remaining) 
272{
273        void *trans_ptr = 0;
274
275        if ((ipptr->ip_off & SW_IP_OFFMASK) != 0)
276                return NULL;
277
278        if (remaining) {
279                if (*remaining<(ipptr->ip_hl*4U)) {
280                        return NULL;
281                }
282                *remaining-=(ipptr->ip_hl * 4);
283        }
284
285        trans_ptr = (void *)((char *)ipptr + (ipptr->ip_hl * 4));
286
287        if (prot) *prot = ipptr->ip_p;
288
289        return trans_ptr;
290}
291
292void *trace_get_payload_from_ip6(libtrace_ip6_t *ipptr, uint8_t *prot,
293                uint32_t *remaining) 
294{
295        void *payload = (char*)ipptr+sizeof(libtrace_ip6_t);
296        uint8_t nxt = ipptr->nxt;
297
298        if (remaining) {
299                if (*remaining<sizeof(libtrace_ip6_t))
300                        return NULL;
301                *remaining-=sizeof(libtrace_ip6_t);
302        }
303
304        while(1) {
305                switch (nxt) {
306                        case 0: /* hop by hop options */
307                        case 43: /* routing */
308                        case 44: /* fragment */
309                        case 50: /* ESP */
310                        case 51: /* AH */
311                        case 60: /* Destination options */
312                                {
313                                        uint16_t len=((libtrace_ip6_ext_t*)payload)->len
314                                        +sizeof(libtrace_ip6_ext_t);
315
316                                        if (remaining) {
317                                                if (*remaining < len) {
318                                                        /* Snap too short */
319                                                        return NULL;
320                                                }
321                                                *remaining-=len;
322                                        }
323
324                                        payload=(char*)payload+len;
325                                        nxt=((libtrace_ip6_ext_t*)payload)->nxt;
326                                        continue;
327                                }
328                        default:
329                                if (prot) *prot=nxt;
330                                return payload;
331                }
332        }
333}
334
335DLLEXPORT void *trace_get_transport(libtrace_packet_t *packet, 
336                uint8_t *proto,
337                uint32_t *remaining
338                ) 
339{
340        void *transport;
341        uint8_t dummy_proto;
342        uint16_t ethertype;
343        void *link;
344        uint32_t dummy_remaining;
345
346        if (!proto) proto=&dummy_proto;
347
348        if (!remaining) remaining=&dummy_remaining;
349
350        *remaining = trace_get_capture_length(packet);
351
352        link=trace_get_link(packet);
353
354        if (!link)
355                return NULL;
356
357        transport = trace_get_payload_from_link(
358                        link,
359                        trace_get_link_type(packet),
360                        &ethertype,
361                        remaining);
362
363        if (!transport)
364                return NULL;
365
366        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
367                        &ethertype,
368                        remaining);
369
370        if (!transport)
371                return NULL;
372
373        switch (ethertype) {
374                case 0x0800: /* IPv4 */
375                        transport=trace_get_payload_from_ip(
376                                (libtrace_ip_t*)transport, proto, remaining);
377                        /* IPv6 */
378                        if (transport && *proto == 41) {
379                                transport=trace_get_payload_from_ip6(
380                                 (libtrace_ip6_t*)transport, proto,remaining);
381                        }
382                        return transport;
383                case 0x86DD: /* IPv6 */
384                        return trace_get_payload_from_ip6(
385                                (libtrace_ip6_t*)transport, proto, remaining);
386                       
387                default:
388                        *proto=0;
389                        return NULL;
390        }
391
392}
393
394DLLEXPORT libtrace_tcp_t *trace_get_tcp(libtrace_packet_t *packet) {
395        uint8_t proto;
396        libtrace_tcp_t *tcp;
397
398        tcp=(libtrace_tcp_t*)trace_get_transport(packet,&proto,NULL);
399
400        if (!tcp && proto != 6)
401                return NULL;
402
403        return (libtrace_tcp_t*)tcp;
404}
405
406DLLEXPORT libtrace_tcp_t *trace_get_tcp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
407{
408        libtrace_tcp_t *tcpptr = 0;
409
410        if (ip->ip_p == 6)  {
411                tcpptr = (libtrace_tcp_t *)
412                        trace_get_payload_from_ip(ip, NULL, remaining);
413        }
414
415        return tcpptr;
416}
417
418DLLEXPORT libtrace_udp_t *trace_get_udp(libtrace_packet_t *packet) {
419        uint8_t proto;
420        libtrace_udp_t *udp;
421
422        udp=(libtrace_udp_t*)trace_get_transport(packet,&proto,NULL);
423
424        if (proto != 17)
425                return NULL;
426
427        return udp;
428}
429
430DLLEXPORT libtrace_udp_t *trace_get_udp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
431{
432        libtrace_udp_t *udpptr = 0;
433
434        if (ip->ip_p == 17) {
435                udpptr = (libtrace_udp_t *)
436                        trace_get_payload_from_ip(ip, NULL, remaining);
437        }
438
439        return udpptr;
440}
441
442DLLEXPORT libtrace_icmp_t *trace_get_icmp(libtrace_packet_t *packet) {
443        uint8_t proto;
444        libtrace_icmp_t *icmp;
445
446        icmp=(libtrace_icmp_t*)trace_get_transport(packet,&proto,NULL);
447
448        if (!icmp || proto != 1)
449                return NULL;
450
451        return icmp;
452}
453
454DLLEXPORT libtrace_icmp_t *trace_get_icmp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
455{
456        libtrace_icmp_t *icmpptr = 0;
457
458        if (ip->ip_p == 1)  {
459                icmpptr = (libtrace_icmp_t *)trace_get_payload_from_ip(ip, 
460                                NULL, remaining);
461        }
462
463        return icmpptr;
464}
465
466DLLEXPORT void *trace_get_payload_from_udp(libtrace_udp_t *udp, uint32_t *remaining)
467{
468        if (remaining) {
469                if (*remaining < sizeof(libtrace_udp_t))
470                        return NULL;
471                *remaining-=sizeof(libtrace_udp_t);
472        }
473        return (void*)((char*)udp+sizeof(libtrace_udp_t));
474}
475
476DLLEXPORT void *trace_get_payload_from_tcp(libtrace_tcp_t *tcp, uint32_t *remaining)
477{
478        unsigned int dlen = tcp->doff*4;
479        if (remaining) {
480                if (*remaining < dlen)
481                        return NULL;
482                *remaining-=dlen;
483        }
484        return tcp+dlen;
485}
486
487DLLEXPORT void *trace_get_payload_from_icmp(libtrace_icmp_t *icmp, uint32_t *remaining)
488{
489        if (remaining) {
490                if (*remaining < sizeof(libtrace_icmp_t))
491                        return NULL;
492                *remaining-=sizeof(libtrace_icmp_t);
493        }
494        return (char*)icmp+sizeof(libtrace_icmp_t);
495}
496
497struct ports_t {
498        uint16_t src;
499        uint16_t dst;
500};
501
502/* Return the client port
503 */
504DLLEXPORT uint16_t trace_get_source_port(const libtrace_packet_t *packet)
505{
506        uint32_t remaining;
507        struct ports_t *port = 
508                (struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
509                        NULL, &remaining);
510
511        /* snapped too early */
512        if (remaining<2)
513                return 0;
514
515        if (port)
516                return ntohs(port->src);
517        else
518                return 0;
519}
520
521/* Same as get_source_port except use the destination port */
522DLLEXPORT uint16_t trace_get_destination_port(const libtrace_packet_t *packet)
523{
524        uint32_t remaining;
525        struct ports_t *port = 
526                (struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
527                        NULL, &remaining);
528        /* snapped to early */
529        if (remaining<4)
530                return 0;
531
532        if (port)
533                return ntohs(port->dst);
534        else
535                return 0;
536}
537
538
539uint8_t *trace_get_source_mac(libtrace_packet_t *packet) {
540        void *link = trace_get_link(packet);
541        libtrace_80211_t *wifi;
542        libtrace_ether_t *ethptr = (libtrace_ether_t*)link;
543        if (!link)
544                return NULL;
545        switch (trace_get_link_type(packet)) {
546                case TRACE_TYPE_80211:
547                        wifi=(libtrace_80211_t*)link;
548                        return (uint8_t*)&wifi->mac2;
549                case TRACE_TYPE_80211_PRISM:
550                        wifi=(libtrace_80211_t*)((char*)link+144);
551                        return (uint8_t*)&wifi->mac2;
552                case TRACE_TYPE_ETH:
553                        return (uint8_t*)&ethptr->ether_shost;
554                case TRACE_TYPE_POS:
555                case TRACE_TYPE_NONE:
556                case TRACE_TYPE_HDLC_POS:
557                case TRACE_TYPE_LINUX_SLL:
558                case TRACE_TYPE_PFLOG:
559                case TRACE_TYPE_ATM:
560                        return NULL;
561        }
562        fprintf(stderr,"Not implemented\n");
563        assert(0);
564        return NULL;
565}
566
567DLLEXPORT uint8_t *trace_get_destination_mac(libtrace_packet_t *packet) {
568        void *link = trace_get_link(packet);
569        libtrace_80211_t *wifi;
570        libtrace_ether_t *ethptr = (libtrace_ether_t*)link;
571        if (!link)
572                return NULL;
573        switch (trace_get_link_type(packet)) {
574                case TRACE_TYPE_80211:
575                        wifi=(libtrace_80211_t*)link;
576                        return (uint8_t*)&wifi->mac1;
577                case TRACE_TYPE_80211_PRISM:
578                        wifi=(libtrace_80211_t*)((char*)link+144);
579                        return (uint8_t*)&wifi->mac1;
580                case TRACE_TYPE_ETH:
581                        return (uint8_t*)&ethptr->ether_dhost;
582                case TRACE_TYPE_POS:
583                case TRACE_TYPE_NONE:
584                case TRACE_TYPE_ATM:
585                case TRACE_TYPE_HDLC_POS:
586                case TRACE_TYPE_LINUX_SLL:
587                case TRACE_TYPE_PFLOG:
588                        /* No MAC address */
589                        return NULL;
590        }
591        fprintf(stderr,"Not implemented\n");
592        assert(0);
593        return NULL;
594}
595
596DLLEXPORT struct sockaddr *trace_get_source_address(const libtrace_packet_t *packet, 
597                struct sockaddr *addr)
598{
599        uint16_t proto;
600        uint32_t remaining;
601        void *transport;
602        static struct sockaddr_storage dummy;
603
604        if (!addr)
605                addr=(struct sockaddr*)&dummy;
606
607        remaining = trace_get_capture_length(packet);
608
609        transport = trace_get_payload_from_link(
610                        trace_get_link(packet),
611                        trace_get_link_type(packet),
612                        &proto,
613                        &remaining);
614
615        if (!transport)
616                return false;
617
618        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
619                        &proto,
620                        &remaining);
621
622        if (!transport)
623                return false;
624
625        switch (proto) {
626                case 0x0800: /* IPv4 */
627                {
628                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
629                        libtrace_ip_t *ip = (libtrace_ip_t*)transport;
630                        addr4->sin_family=AF_INET;
631                        addr4->sin_port=0;
632                        addr4->sin_addr=ip->ip_src;
633                        return addr;
634                }
635                case 0x86DD: /* IPv6 */
636                {
637                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
638                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)transport;
639                        addr6->sin6_family=AF_INET6;
640                        addr6->sin6_port=0;
641                        addr6->sin6_flowinfo=0;
642                        addr6->sin6_addr=ip6->ip_src;
643                        return addr;
644                }
645                default:
646                        return NULL;
647        }
648}
649
650DLLEXPORT struct sockaddr *trace_get_destination_address(const libtrace_packet_t *packet, 
651                struct sockaddr *addr)
652{
653        uint16_t proto;
654        uint32_t remaining;
655        void *transport;
656        static struct sockaddr_storage dummy;
657
658        if (!addr)
659                addr=(struct sockaddr*)&dummy;
660
661        remaining = trace_get_capture_length(packet);
662
663        transport = trace_get_payload_from_link(
664                        trace_get_link(packet),
665                        trace_get_link_type(packet),
666                        &proto,
667                        &remaining);
668
669        if (!transport)
670                return false;
671
672        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
673                        &proto,
674                        &remaining);
675
676        if (!transport)
677                return false;
678
679        switch (proto) {
680                case 0x0800: /* IPv4 */
681                {
682                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
683                        libtrace_ip_t *ip = (libtrace_ip_t*)transport;
684                        addr4->sin_family=AF_INET;
685                        addr4->sin_port=0;
686                        addr4->sin_addr=ip->ip_dst;
687                        return addr;
688                }
689                case 0x86DD: /* IPv6 */
690                {
691                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
692                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)transport;
693                        addr6->sin6_family=AF_INET6;
694                        addr6->sin6_port=0;
695                        addr6->sin6_flowinfo=0;
696                        addr6->sin6_addr=ip6->ip_dst;
697                        return addr;
698                }
699                default:
700                        return NULL;
701        }
702}
703
704/* parse an ip or tcp option
705 * @param[in,out] ptr   the pointer to the current option
706 * @param[in,out] len   the length of the remaining buffer
707 * @param[out] type     the type of the option
708 * @param[out] optlen   the length of the option
709 * @param[out] data     the data of the option
710 *
711 * @returns bool true if there is another option (and the fields are filled in)
712 *               or false if this was the last option.
713 *
714 * This updates ptr to point to the next option after this one, and updates
715 * len to be the number of bytes remaining in the options area.  Type is updated
716 * to be the code of this option, and data points to the data of this option,
717 * with optlen saying how many bytes there are.
718 *
719 * @note Beware of fragmented packets.
720 * @author Perry Lorier
721 */
722DLLEXPORT int trace_get_next_option(unsigned char **ptr,int *len,
723                        unsigned char *type,
724                        unsigned char *optlen,
725                        unsigned char **data)
726{
727        if (*len<=0)
728                return 0;
729        *type=**ptr;
730        switch(*type) {
731                case 0: /* End of options */
732                        return 0;
733                case 1: /* Pad */
734                        (*ptr)++;
735                        (*len)--;
736                        return 1;
737                default:
738                        *optlen = *(*ptr+1);
739                        if (*optlen<2)
740                                return 0; /* I have no idea wtf is going on
741                                           * with these packets
742                                           */
743                        (*len)-=*optlen;
744                        (*data)=(*ptr+2);
745                        (*ptr)+=*optlen;
746                        if (*len<0)
747                                return 0;
748                        return 1;
749        }
750        assert(0);
751}
752
753
Note: See TracBrowser for help on using the repository browser.