source: trunk/lib/protocols.c @ 820

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

Cover destination protocol too
  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
File size: 16.1 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        }
211        fprintf(stderr,"Don't understand link layer type %i in trace_get_payload_from_link()\n",
212                linktype);
213        return NULL;
214}
215
216libtrace_ip_t *trace_get_ip(libtrace_packet_t *packet) 
217{
218        uint16_t type;
219        void *link = trace_get_link(packet);
220        void *ret;
221
222        if (!link)
223                return NULL;
224       
225        ret=trace_get_payload_from_link(
226                        link,
227                        trace_get_link_type(packet),
228                        &type, NULL);
229
230        if (!ret)
231                return NULL;
232
233        ret=trace_get_vlan_payload_from_ethernet_payload(ret,&type,NULL);
234
235        if (!ret || type!=0x0800)
236                return NULL;
237
238        return (libtrace_ip_t*)ret;
239}
240
241libtrace_ip6_t *trace_get_ip6(libtrace_packet_t *packet) 
242{
243        uint16_t type;
244        void *link=trace_get_link(packet);
245        void *ret;
246       
247        if (!link)
248                return NULL;
249
250        ret=trace_get_payload_from_link(
251                        link,
252                        trace_get_link_type(packet),
253                        &type,NULL);
254
255        if (!ret)
256                return NULL;
257
258        ret=trace_get_vlan_payload_from_ethernet_payload(ret,&type,NULL);
259
260        if (!ret || type!=0x86DD)
261                return NULL;
262
263        return (libtrace_ip6_t*)ret;
264}
265
266#define SW_IP_OFFMASK 0xff1f
267
268DLLEXPORT void *trace_get_payload_from_ip(libtrace_ip_t *ipptr, uint8_t *prot,
269                uint32_t *remaining) 
270{
271        void *trans_ptr = 0;
272
273        if ((ipptr->ip_off & SW_IP_OFFMASK) != 0)
274                return NULL;
275
276        if (remaining) {
277                if (*remaining<(ipptr->ip_hl*4U)) {
278                        return NULL;
279                }
280                *remaining-=(ipptr->ip_hl * 4);
281        }
282
283        trans_ptr = (void *)((char *)ipptr + (ipptr->ip_hl * 4));
284
285        if (prot) *prot = ipptr->ip_p;
286
287        return trans_ptr;
288}
289
290void *trace_get_payload_from_ip6(libtrace_ip6_t *ipptr, uint8_t *prot,
291                uint32_t *remaining) 
292{
293        void *payload = (char*)ipptr+sizeof(libtrace_ip6_t);
294        uint8_t nxt = ipptr->nxt;
295
296        if (remaining) {
297                if (*remaining<sizeof(libtrace_ip6_t))
298                        return NULL;
299                *remaining-=sizeof(libtrace_ip6_t);
300        }
301
302        while(1) {
303                switch (nxt) {
304                        case 0: /* hop by hop options */
305                        case 43: /* routing */
306                        case 44: /* fragment */
307                        case 50: /* ESP */
308                        case 51: /* AH */
309                        case 60: /* Destination options */
310                                {
311                                        uint16_t len=((libtrace_ip6_ext_t*)payload)->len
312                                        +sizeof(libtrace_ip6_ext_t);
313
314                                        if (remaining) {
315                                                if (*remaining < len) {
316                                                        /* Snap too short */
317                                                        return NULL;
318                                                }
319                                                *remaining-=len;
320                                        }
321
322                                        payload=(char*)payload+len;
323                                        nxt=((libtrace_ip6_ext_t*)payload)->nxt;
324                                        continue;
325                                }
326                        default:
327                                if (prot) *prot=nxt;
328                                return payload;
329                }
330        }
331}
332
333DLLEXPORT void *trace_get_transport(libtrace_packet_t *packet, 
334                uint8_t *proto,
335                uint32_t *remaining
336                ) 
337{
338        void *transport;
339        uint8_t dummy_proto;
340        uint16_t ethertype;
341        void *link;
342        uint32_t dummy_remaining;
343
344        if (!proto) proto=&dummy_proto;
345
346        if (!remaining) remaining=&dummy_remaining;
347
348        *remaining = trace_get_capture_length(packet);
349
350        link=trace_get_link(packet);
351
352        if (!link)
353                return NULL;
354
355        transport = trace_get_payload_from_link(
356                        link,
357                        trace_get_link_type(packet),
358                        &ethertype,
359                        remaining);
360
361        if (!transport)
362                return NULL;
363
364        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
365                        &ethertype,
366                        remaining);
367
368        if (!transport)
369                return NULL;
370
371        switch (ethertype) {
372                case 0x0800: /* IPv4 */
373                        transport=trace_get_payload_from_ip(
374                                (libtrace_ip_t*)transport, proto, remaining);
375                        /* IPv6 */
376                        if (transport && *proto == 41) {
377                                transport=trace_get_payload_from_ip6(
378                                 (libtrace_ip6_t*)transport, proto,remaining);
379                        }
380                        return transport;
381                case 0x86DD: /* IPv6 */
382                        return trace_get_payload_from_ip6(
383                                (libtrace_ip6_t*)transport, proto, remaining);
384                       
385                default:
386                        *proto=0;
387                        return NULL;
388        }
389
390}
391
392DLLEXPORT libtrace_tcp_t *trace_get_tcp(libtrace_packet_t *packet) {
393        uint8_t proto;
394        libtrace_tcp_t *tcp;
395
396        tcp=(libtrace_tcp_t*)trace_get_transport(packet,&proto,NULL);
397
398        if (!tcp && proto != 6)
399                return NULL;
400
401        return (libtrace_tcp_t*)tcp;
402}
403
404DLLEXPORT libtrace_tcp_t *trace_get_tcp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
405{
406        libtrace_tcp_t *tcpptr = 0;
407
408        if (ip->ip_p == 6)  {
409                tcpptr = (libtrace_tcp_t *)
410                        trace_get_payload_from_ip(ip, NULL, remaining);
411        }
412
413        return tcpptr;
414}
415
416DLLEXPORT libtrace_udp_t *trace_get_udp(libtrace_packet_t *packet) {
417        uint8_t proto;
418        libtrace_udp_t *udp;
419
420        udp=(libtrace_udp_t*)trace_get_transport(packet,&proto,NULL);
421
422        if (proto != 17)
423                return NULL;
424
425        return udp;
426}
427
428DLLEXPORT libtrace_udp_t *trace_get_udp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
429{
430        libtrace_udp_t *udpptr = 0;
431
432        if (ip->ip_p == 17) {
433                udpptr = (libtrace_udp_t *)
434                        trace_get_payload_from_ip(ip, NULL, remaining);
435        }
436
437        return udpptr;
438}
439
440DLLEXPORT libtrace_icmp_t *trace_get_icmp(libtrace_packet_t *packet) {
441        uint8_t proto;
442        libtrace_icmp_t *icmp;
443
444        icmp=(libtrace_icmp_t*)trace_get_transport(packet,&proto,NULL);
445
446        if (!icmp || proto != 1)
447                return NULL;
448
449        return icmp;
450}
451
452DLLEXPORT libtrace_icmp_t *trace_get_icmp_from_ip(libtrace_ip_t *ip, uint32_t *remaining)
453{
454        libtrace_icmp_t *icmpptr = 0;
455
456        if (ip->ip_p == 1)  {
457                icmpptr = (libtrace_icmp_t *)trace_get_payload_from_ip(ip, 
458                                NULL, remaining);
459        }
460
461        return icmpptr;
462}
463
464DLLEXPORT void *trace_get_payload_from_udp(libtrace_udp_t *udp, uint32_t *remaining)
465{
466        if (remaining) {
467                if (*remaining < sizeof(libtrace_udp_t))
468                        return NULL;
469                *remaining-=sizeof(libtrace_udp_t);
470        }
471        return (void*)((char*)udp+sizeof(libtrace_udp_t));
472}
473
474DLLEXPORT void *trace_get_payload_from_tcp(libtrace_tcp_t *tcp, uint32_t *remaining)
475{
476        unsigned int dlen = tcp->doff*4;
477        if (remaining) {
478                if (*remaining < dlen)
479                        return NULL;
480                *remaining-=dlen;
481        }
482        return tcp+dlen;
483}
484
485DLLEXPORT void *trace_get_payload_from_icmp(libtrace_icmp_t *icmp, uint32_t *remaining)
486{
487        if (remaining) {
488                if (*remaining < sizeof(libtrace_icmp_t))
489                        return NULL;
490                *remaining-=sizeof(libtrace_icmp_t);
491        }
492        return (char*)icmp+sizeof(libtrace_icmp_t);
493}
494
495struct ports_t {
496        uint16_t src;
497        uint16_t dst;
498};
499
500/* Return the client port
501 */
502DLLEXPORT uint16_t trace_get_source_port(const libtrace_packet_t *packet)
503{
504        uint32_t remaining;
505        struct ports_t *port = 
506                (struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
507                        NULL, &remaining);
508
509        /* snapped too early */
510        if (remaining<2)
511                return 0;
512
513        if (port)
514                return ntohs(port->src);
515        else
516                return 0;
517}
518
519/* Same as get_source_port except use the destination port */
520DLLEXPORT uint16_t trace_get_destination_port(const libtrace_packet_t *packet)
521{
522        uint32_t remaining;
523        struct ports_t *port = 
524                (struct ports_t*)trace_get_transport((libtrace_packet_t*)packet,
525                        NULL, &remaining);
526        /* snapped to early */
527        if (remaining<4)
528                return 0;
529
530        if (port)
531                return ntohs(port->dst);
532        else
533                return 0;
534}
535
536
537uint8_t *trace_get_source_mac(libtrace_packet_t *packet) {
538        void *link = trace_get_link(packet);
539        libtrace_80211_t *wifi;
540        libtrace_ether_t *ethptr = (libtrace_ether_t*)link;
541        if (!link)
542                return NULL;
543        switch (trace_get_link_type(packet)) {
544                case TRACE_TYPE_80211:
545                        wifi=(libtrace_80211_t*)link;
546                        return (uint8_t*)&wifi->mac2;
547                case TRACE_TYPE_80211_PRISM:
548                        wifi=(libtrace_80211_t*)((char*)link+144);
549                        return (uint8_t*)&wifi->mac2;
550                case TRACE_TYPE_ETH:
551                        return (uint8_t*)&ethptr->ether_shost;
552                case TRACE_TYPE_POS:
553                case TRACE_TYPE_NONE:
554                case TRACE_TYPE_HDLC_POS:
555                case TRACE_TYPE_LINUX_SLL:
556                case TRACE_TYPE_PFLOG:
557                case TRACE_TYPE_ATM:
558                        return NULL;
559        }
560        fprintf(stderr,"Not implemented\n");
561        assert(0);
562        return NULL;
563}
564
565DLLEXPORT uint8_t *trace_get_destination_mac(libtrace_packet_t *packet) {
566        void *link = trace_get_link(packet);
567        libtrace_80211_t *wifi;
568        libtrace_ether_t *ethptr = (libtrace_ether_t*)link;
569        if (!link)
570                return NULL;
571        switch (trace_get_link_type(packet)) {
572                case TRACE_TYPE_80211:
573                        wifi=(libtrace_80211_t*)link;
574                        return (uint8_t*)&wifi->mac1;
575                case TRACE_TYPE_80211_PRISM:
576                        wifi=(libtrace_80211_t*)((char*)link+144);
577                        return (uint8_t*)&wifi->mac1;
578                case TRACE_TYPE_ETH:
579                        return (uint8_t*)&ethptr->ether_dhost;
580                case TRACE_TYPE_POS:
581                case TRACE_TYPE_NONE:
582                case TRACE_TYPE_ATM:
583                case TRACE_TYPE_HDLC_POS:
584                case TRACE_TYPE_LINUX_SLL:
585                case TRACE_TYPE_PFLOG:
586                        /* No MAC address */
587                        return NULL;
588        }
589        fprintf(stderr,"Not implemented\n");
590        assert(0);
591        return NULL;
592}
593
594DLLEXPORT struct sockaddr *trace_get_source_address(const libtrace_packet_t *packet, 
595                struct sockaddr *addr)
596{
597        uint16_t proto;
598        uint32_t remaining;
599        void *transport;
600        static struct sockaddr_storage dummy;
601
602        if (!addr)
603                addr=(struct sockaddr*)&dummy;
604
605        remaining = trace_get_capture_length(packet);
606
607        transport = trace_get_payload_from_link(
608                        trace_get_link(packet),
609                        trace_get_link_type(packet),
610                        &proto,
611                        &remaining);
612
613        if (!transport)
614                return false;
615
616        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
617                        &proto,
618                        &remaining);
619
620        if (!transport)
621                return false;
622
623        switch (proto) {
624                case 0x0800: /* IPv4 */
625                {
626                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
627                        libtrace_ip_t *ip = (libtrace_ip_t*)transport;
628                        addr4->sin_family=AF_INET;
629                        addr4->sin_port=0;
630                        addr4->sin_addr=ip->ip_src;
631                        return addr;
632                }
633                case 0x86DD: /* IPv6 */
634                {
635                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
636                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)transport;
637                        addr6->sin6_family=AF_INET6;
638                        addr6->sin6_port=0;
639                        addr6->sin6_flowinfo=0;
640                        addr6->sin6_addr=ip6->ip_src;
641                        return addr;
642                }
643                default:
644                        return NULL;
645        }
646}
647
648DLLEXPORT struct sockaddr *trace_get_destination_address(const libtrace_packet_t *packet, 
649                struct sockaddr *addr)
650{
651        uint16_t proto;
652        uint32_t remaining;
653        void *transport;
654        static struct sockaddr_storage dummy;
655
656        if (!addr)
657                addr=(struct sockaddr*)&dummy;
658
659        remaining = trace_get_capture_length(packet);
660
661        transport = trace_get_payload_from_link(
662                        trace_get_link(packet),
663                        trace_get_link_type(packet),
664                        &proto,
665                        &remaining);
666
667        if (!transport)
668                return false;
669
670        transport = trace_get_vlan_payload_from_ethernet_payload(transport,
671                        &proto,
672                        &remaining);
673
674        if (!transport)
675                return false;
676
677        switch (proto) {
678                case 0x0800: /* IPv4 */
679                {
680                        struct sockaddr_in *addr4=(struct sockaddr_in*)addr;
681                        libtrace_ip_t *ip = (libtrace_ip_t*)transport;
682                        addr4->sin_family=AF_INET;
683                        addr4->sin_port=0;
684                        addr4->sin_addr=ip->ip_dst;
685                        return addr;
686                }
687                case 0x86DD: /* IPv6 */
688                {
689                        struct sockaddr_in6 *addr6=(struct sockaddr_in6*)addr;
690                        libtrace_ip6_t *ip6 = (libtrace_ip6_t*)transport;
691                        addr6->sin6_family=AF_INET6;
692                        addr6->sin6_port=0;
693                        addr6->sin6_flowinfo=0;
694                        addr6->sin6_addr=ip6->ip_dst;
695                        return addr;
696                }
697                default:
698                        return NULL;
699        }
700}
701
702/* parse an ip or tcp option
703 * @param[in,out] ptr   the pointer to the current option
704 * @param[in,out] len   the length of the remaining buffer
705 * @param[out] type     the type of the option
706 * @param[out] optlen   the length of the option
707 * @param[out] data     the data of the option
708 *
709 * @returns bool true if there is another option (and the fields are filled in)
710 *               or false if this was the last option.
711 *
712 * This updates ptr to point to the next option after this one, and updates
713 * len to be the number of bytes remaining in the options area.  Type is updated
714 * to be the code of this option, and data points to the data of this option,
715 * with optlen saying how many bytes there are.
716 *
717 * @note Beware of fragmented packets.
718 * @author Perry Lorier
719 */
720DLLEXPORT int trace_get_next_option(unsigned char **ptr,int *len,
721                        unsigned char *type,
722                        unsigned char *optlen,
723                        unsigned char **data)
724{
725        if (*len<=0)
726                return 0;
727        *type=**ptr;
728        switch(*type) {
729                case 0: /* End of options */
730                        return 0;
731                case 1: /* Pad */
732                        (*ptr)++;
733                        (*len)--;
734                        return 1;
735                default:
736                        *optlen = *(*ptr+1);
737                        if (*optlen<2)
738                                return 0; /* I have no idea wtf is going on
739                                           * with these packets
740                                           */
741                        (*len)-=*optlen;
742                        (*data)=(*ptr+2);
743                        (*ptr)+=*optlen;
744                        if (*len<0)
745                                return 0;
746                        return 1;
747        }
748        assert(0);
749}
750
751
Note: See TracBrowser for help on using the repository browser.