source: lib/protocols.c @ 92b3f87

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

Add supoprt for ethernet encapsulated inside LINUX_SLL frames

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