source: lib/protocols.c @ 9e46ee7

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

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