libaitio/example/bpf.c - diff

Return to bpf.c CVS log

Up to [ELWIX - Embedded LightWeight unIX -] / libaitio / example

Diff for /libaitio/example/bpf.c between versions 1.1 and 1.1.2.1

-version 1.1, 2013/06/25 09:08:10
+version 1.1.2.1, 2013/06/25 09:08:10
- Line 0
+ Line 1
+ #include <stdio.h>
+ #include <stdlib.h>
+ #include <fcntl.h>
+ #include <unistd.h>
+ #include <string.h>
+ #include <errno.h>
+ #include <poll.h>
+ #include <getopt.h>
+ #include <pthread.h>
+ #include <assert.h>
+ #include <sys/types.h>
+ #include <sys/param.h>
+ #include <sys/limits.h>
+ #include <sys/socket.h>
+ #include <sys/ioctl.h>
+ #include <sys/mman.h>
+ #include <net/if.h>
+ #include <net/bpf.h>
+ #include <machine/atomic.h>
+ #ifdef __FreeBSD__
+ #include <net/ethernet.h>
+ #endif
+ #ifdef __OpenBSD__
+ #include <net/ethertypes.h>
+ #include <netinet/in_systm.h>
+ #endif
+ #include <net/if_arp.h>
+ #include <netinet/in.h>
+ #include <netinet/ip.h>
+ #include <netinet/ip6.h>
+ #include <netinet/udp.h>
+ #include <netinet/tcp.h>
+ #include <netinet/ip_icmp.h>
+ #include <arpa/inet.h>
+ #ifdef __OpenBSD__
+ #include <netinet/if_ether.h>
+ #endif
+ #ifndef roundup
+ #define roundup(x, y)   ((((x) + ((y) - 1)) / (y)) * (y))  /* to any y */
+ #endif
+ #define time_spec_sub(vvp, uvp)                                           \
+         do {                                                            \
+                 (vvp)->tv_sec -= (uvp)->tv_sec;                         \
+                 (vvp)->tv_nsec -= (uvp)->tv_nsec;                       \
+                 if ((vvp)->tv_nsec < 0) {                               \
+                         (vvp)->tv_sec--;                                \
+                         (vvp)->tv_nsec += 1000000000;                   \
+                 }                                                       \
+         } while (0)
+ int Verbose, flg;
+ static void *
+ ShowPkt(void *buffer)
+ {
+         char Proto = 0, szStr[BUFSIZ], szLine[BUFSIZ], szWrk[BUFSIZ], szShow[USHRT_MAX] = { 0 };
+         struct bpf_hdr *bpf = buffer;
+ #ifdef __FreeBSD__
+         struct bpf_zbuf_header *bzh = buffer;
+         struct icmphdr *icmp;
+ #endif
+ #ifdef __OpenBSD__
+         struct icmp *icmp;
+ #endif
+         struct ether_header *eth;
+         struct ip *ip;
+         struct ip6_hdr *ipv6;
+         struct arphdr *arp;
+         struct udphdr *udp;
+         struct tcphdr *tcp;
+         assert(buffer);
+ #ifdef __FreeBSD__
+         snprintf(szLine, BUFSIZ, "#Packet length: %d\n>>> Ethernet ...\n", flg ? bzh->bzh_kernel_len : bpf->bh_datalen);
+         strlcat(szShow, szLine, USHRT_MAX);
+         eth = (struct ether_header *) (buffer + (flg ? bzh->bzh_kernel_len : bpf->bh_hdrlen));
+ #else
+         snprintf(szLine, BUFSIZ, "#Packet length: %d\n>>> Ethernet ...\n", bpf->bh_datalen);
+         strlcat(szShow, szLine, USHRT_MAX);
+         eth = (struct ether_header *) (buffer + bpf->bh_hdrlen);
+ #endif
+         switch (ntohs(eth->ether_type)) {
+                 case ETHERTYPE_ARP:
+                         strlcpy(szWrk, "(ARP)", sizeof szWrk);
+                         break;
+                 case ETHERTYPE_IP:
+                         strlcpy(szWrk, "(IP)", sizeof szWrk);
+                         break;
+                 case ETHERTYPE_IPV6:
+                         strlcpy(szWrk, "(IPv6)", sizeof szWrk);
+                         break;
+                 default:
+                         memset(szWrk, 0, sizeof szWrk);
+         }
+         snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_shost[0], eth->ether_shost[1],
+                         eth->ether_shost[2], eth->ether_shost[3], eth->ether_shost[4], eth->ether_shost[5]);
+         snprintf(szLine, BUFSIZ, "ether_type: %04x%s, src_mac: %s, ", ntohs(eth->ether_type), szWrk, szStr);
+         strlcat(szShow, szLine, USHRT_MAX);
+         snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_dhost[0], eth->ether_dhost[1],
+                         eth->ether_dhost[2], eth->ether_dhost[3], eth->ether_dhost[4], eth->ether_dhost[5]);
+         snprintf(szLine, BUFSIZ, "dst_mac: %s\n", szStr);
+         strlcat(szShow, szLine, USHRT_MAX);
+         switch (ntohs(eth->ether_type)) {
+                 case ETHERTYPE_ARP:
+                         arp = (struct arphdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
+                         strlcat(szShow, "\t>>> ARP ...\n", USHRT_MAX);
+                         switch (ntohs(arp->ar_op)) {
+                                 case ARPOP_REQUEST:
+                                         strlcpy(szStr, "Request", sizeof szStr);
+                                         break;
+                                 case ARPOP_REPLY:
+                                         strlcpy(szStr, "Reply", sizeof szStr);
+                                         break;
+                                 case ARPOP_REVREQUEST:
+                                         strlcpy(szStr, "RevRequest", sizeof szStr);
+                                         break;
+                                 case ARPOP_REVREPLY:
+                                         strlcpy(szStr, "RevReply", sizeof szStr);
+                                         break;
+                                 case ARPOP_INVREQUEST:
+                                         strlcpy(szStr, "InvRequest", sizeof szStr);
+                                         break;
+                                 case ARPOP_INVREPLY:
+                                         strlcpy(szStr, "InvReply", sizeof szStr);
+                                         break;
+                                 default:
+                                         strlcpy(szStr, "*Unknown*", sizeof szStr);
+                         }
+                         snprintf(szLine, BUFSIZ, "\tAddr_fmt: %d, Proto_fmt: %d, Addr_len: %d, Proto_len: %d, %s\n",
+                                         ntohs(arp->ar_hrd), ntohs(arp->ar_pro), arp->ar_hln, arp->ar_pln, szStr);
+                         strlcat(szShow, szLine, USHRT_MAX);
+                         break;
+                 case ETHERTYPE_IP:
+                         ip = (struct ip*) (((caddr_t) eth) + ETHER_HDR_LEN);
+                         strlcat(szShow, "\t>>> IP ...\n", USHRT_MAX);
+                         snprintf(szStr, BUFSIZ, "Frag_off: %d", ntohs(ip->ip_off) & IP_OFFMASK);
+                         if (ntohs(ip->ip_off) & IP_RF)
+                                 strlcat(szStr, "|RF", sizeof szStr);
+                         if (ntohs(ip->ip_off) & IP_DF)
+                                 strlcat(szStr, "|DF", sizeof szStr);
+                         if (ntohs(ip->ip_off) & IP_MF)
+                                 strlcat(szStr, "|MF", sizeof szStr);
+                         snprintf(szLine, BUFSIZ, "\tTOS: %02x, TTL: %d, ID: %04x, Cksum: %04x, Len: %d, %s\n",
+                                         ip->ip_tos, ip->ip_ttl, ntohs(ip->ip_id), ntohs(ip->ip_sum),
+                                         ntohs(ip->ip_len), szStr);
+                         strlcat(szShow, szLine, USHRT_MAX);
+                         snprintf(szLine, BUFSIZ, "\tProto: %d, Src_addr: %s, Dst_addr: %s\n",
+                                         ip->ip_p, inet_ntop(AF_INET, &ip->ip_src, szStr, BUFSIZ),
+                                         inet_ntop(AF_INET, &ip->ip_dst, szWrk, BUFSIZ));
+                         strlcat(szShow, szLine, USHRT_MAX);
+                         Proto = ip->ip_p;
+                         break;
+                 case ETHERTYPE_IPV6:
+                         ipv6 = (struct ip6_hdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
+                         strlcat(szShow, "\t>>> IPv6 ...\n", USHRT_MAX);
+                         snprintf(szLine, BUFSIZ, "\tHLim: %d, Payload_len: %d, Flow: %u\n", ipv6->ip6_hlim,
+                                         htons(ipv6->ip6_plen), htonl(ipv6->ip6_flow) & IPV6_FLOWLABEL_MASK);
+                         strlcat(szShow, szLine, USHRT_MAX);
+                         inet_ntop(AF_INET6, &ipv6->ip6_src, szStr, BUFSIZ);
+                         inet_ntop(AF_INET6, &ipv6->ip6_dst, szWrk, BUFSIZ);
+                         snprintf(szLine, BUFSIZ, "\tNext: %d, Src_addr: %s, Dst_addr: %s\n", ipv6->ip6_nxt, szStr, szWrk);
+                         strlcat(szShow, szLine, USHRT_MAX);
+                         Proto = ipv6->ip6_nxt;
+                         break;
+         }
+         if (Proto && (ntohs(eth->ether_type) == ETHERTYPE_IP || ntohs(eth->ether_type) == ETHERTYPE_IPV6))
+                 switch (Proto) {
+                         case IPPROTO_TCP:
+                                 strlcat(szShow, "\t\t>>> TCP ...\n", USHRT_MAX);
+                                 if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
+                                         tcp = (struct tcphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
+                                 else
+                                         tcp = (struct tcphdr*) (((caddr_t) ip) + sizeof(struct ip));
+                                 snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, seq: %u, ack: %u\n",
+                                                 ntohs(tcp->th_sport), ntohs(tcp->th_dport), tcp->th_seq, tcp->th_ack);
+                                 strlcat(szShow, szLine, USHRT_MAX);
+                                 snprintf(szWrk, BUFSIZ, "%d", (u_char) tcp->th_off >> 4);
+                                 if (tcp->th_flags & TH_FIN)
+                                         strlcat(szWrk, "|FIN", sizeof szWrk);
+                                 if (tcp->th_flags & TH_SYN)
+                                         strlcat(szWrk, "|SYN", sizeof szWrk);
+                                 if (tcp->th_flags & TH_RST)
+                                         strlcat(szWrk, "|RST", sizeof szWrk);
+                                 if (tcp->th_flags & TH_PUSH)
+                                         strlcat(szWrk, "|PUSH", sizeof szWrk);
+                                 if (tcp->th_flags & TH_ACK)
+                                         strlcat(szWrk, "|ACK", sizeof szWrk);
+                                 if (tcp->th_flags & TH_URG)
+                                         strlcat(szWrk, "|URG", sizeof szWrk);
+                                 if (tcp->th_flags & TH_ECE)
+                                         strlcat(szWrk, "|ECE", sizeof szWrk);
+                                 if (tcp->th_flags & TH_CWR)
+                                         strlcat(szWrk, "|CWR", sizeof szWrk);
+                                 snprintf(szLine, BUFSIZ, "\t\toff: %s, win: %d, urp: %04x, cksum: %04x\n",
+                                                 szWrk, ntohs(tcp->th_win), ntohs(tcp->th_urp), ntohs(tcp->th_sum));
+                                 strlcat(szShow, szLine, USHRT_MAX);
+                                 break;
+                         case IPPROTO_UDP:
+                                 strlcat(szShow, "\t\t>>> UDP ...\n", USHRT_MAX);
+                                 if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
+                                         udp = (struct udphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
+                                 else
+                                         udp = (struct udphdr*) (((caddr_t) ip) + sizeof(struct ip));
+                                 snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, len: %d, cksum: %04x\n",
+                                                 ntohs(udp->uh_sport), ntohs(udp->uh_dport),
+                                                 ntohs(udp->uh_ulen), ntohs(udp->uh_sum));
+                                 strlcat(szShow, szLine, USHRT_MAX);
+                                 break;
+                         case IPPROTO_ICMP:
+                                 strlcat(szShow, "\t\t>>> ICMP ...\n", USHRT_MAX);
+ #ifdef __FreeBSD__
+                                 if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
+                                         icmp = (struct icmphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
+                                 else
+                                         icmp = (struct icmphdr*) (((caddr_t) ip) + sizeof(struct ip));
+ #endif
+ #ifdef __OpenBSD__
+                                 if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
+                                         icmp = (struct icmp*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
+                                 else
+                                         icmp = (struct icmp*) (((caddr_t) ip) + sizeof(struct ip));
+ #endif
+                                 snprintf(szLine, BUFSIZ, "\t\ttype: %d, code: %d: cksum: %04x\n",
+                                                 icmp->icmp_type, icmp->icmp_code, ntohs(icmp->icmp_cksum));
+                                 strlcat(szShow, szLine, USHRT_MAX);
+                                 break;
+                 }
+         printf("%s===\n", szShow);
+         pthread_exit(NULL);
+ }
+ // ----------------------
+ int
+ main(int argc, char **argv)
+ {
+         u_int n, count = (u_int) -1;
+         register int i;
+         int dev, fd, ret, siz = 0;
+         char szStr[BUFSIZ], szMap[MAXPATHLEN] = { 0 }, *buffer = NULL;
+         struct ifreq ifr;
+         struct pollfd pfd = { 0 };
+         pthread_t tid;
+         char ch, mode = 'R';
+         struct timespec ts_start, ts_end;
+ #ifdef __FreeBSD__
+         struct bpf_zbuf *bz;
+ #endif
+         while ((ch = getopt(argc, argv, "hvwzs:p:f:")) != -1)
+                 switch (ch) {
+                         case 'w':
+                                 mode = 'W';
+                                 break;
+                         case 's':
+                                 siz = strtol(optarg, NULL, 0);
+                                 break;
+                         case 'p':
+                                 count = strtol(optarg, NULL, 0);
+                                 break;
+                         case 'f':
+                                 strlcpy(szMap, optarg, sizeof szMap);
+                                 break;
+                         case 'z':
+                                 flg++;
+                                 break;
+                         case 'v':
+                                 Verbose++;
+                                 break;
+                         case 'h':
+                         default:
+                                 return 1;
+                 }
+         argc -= optind;
+         argv += optind;
+         if (argc < 1)
+                 return 1;
+         if (**argv == '/')
+                 strlcpy(szStr, strrchr(*argv, '/') + 1, sizeof szStr);
+         else
+                 strlcpy(szStr, *argv, sizeof szStr);
+ #ifdef __FreeBSD_
+         dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, &siz, &bz);
+ #else
+         dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, &siz, NULL);
+ #endif
+         if (dev == -1) {
+                 printf("Error:: #%d - %s\n", io_GetErrno(), io_GetError());
+                 return 1;
+         } else
+                 printf("dev=%d(%s)\n", dev, szStr);
+         if (ioctl(dev, BIOCGBLEN, &n) == -1) {
+                 perror("ioctl(BIOCGBLEN)");
+                 close(dev);
+                 return 1;
+         } else
+                 printf("BPF buffer len=%d\n", n);
+ #if 0
+ #ifdef __FreeBSD__
+         ret = BPF_BUFMODE_ZBUF;
+         if (flg && !ioctl(dev, BIOCSETBUFMODE, (u_int*) &ret)) {
+                 if (ioctl(dev, BIOCGETZMAX, &ret) == -1) {
+                         perror("ioctl(BIOCGETZMAX)");
+                         close(dev);
+                         return 1;
+                 }
+                 memset(&bz, 0, sizeof bz);
+                 bz.bz_buflen = roundup(MIN(n, ret), getpagesize());
+                 bz.bz_bufa = mmap(NULL, bz.bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
+                 bz.bz_bufb = mmap(NULL, bz.bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
+                 if (bz.bz_bufa == MAP_FAILED || bz.bz_bufb == MAP_FAILED) {
+                         perror("mmap()");
+                         close(dev);
+                         return 1;
+                 }
+                 if (ioctl(dev, BIOCSETZBUF, &bz) == -1) {
+                         perror("ioctl(BIOCSETZBUF)");
+                         munmap(bz.bz_bufa, bz.bz_buflen);
+                         munmap(bz.bz_bufb, bz.bz_buflen);
+                         close(dev);
+                         return 1;
+                 } else
+                         siz = bz.bz_buflen;
+         } else {
+ #endif
+                 flg = 0;
+                 if (siz) {
+                         if (ioctl(dev, BIOCSBLEN, &siz) == -1) {
+                                 perror("ioctl(BIOCSBLEN)");
+                                 close(dev);
+                                 return 1;
+                         }
+                 } else
+                         siz = n;
+ #ifdef __FreeBSD__
+         }
+ #endif
+         printf("Set buffer len to %d\n", siz);
+         if (!flg) {
+                 if (*szMap) {
+                         fd = open(szMap, O_RDWR);
+                         if (fd == -1) {
+                                 perror("open(map)");
+                                 close(dev);
+                                 return 1;
+                         }
+                         buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_PRIVATE, fd, 0);
+                         close(fd);
+                 } else
+                         buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
+                 if (buffer == MAP_FAILED) {
+                         perror("mmap()");
+                         close(dev);
+                         return 1;
+                 }
+         }
+         memset(&ifr, 0, sizeof ifr);
+         strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name);
+         if (ioctl(dev, BIOCSETIF, (char*) &ifr) == -1) {
+                 perror("ioctl(BIOCSETIF)");
+                 close(dev);
+ #ifdef __FreeBSD__
+                 if (flg) {
+                         munmap(bz.bz_bufa, bz.bz_buflen);
+                         munmap(bz.bz_bufb, bz.bz_buflen);
+                 } else
+ #endif
+                         munmap(buffer, siz);
+                 return 1;
+         }
+         n = 1;
+         if (ioctl(dev, BIOCSHDRCMPLT, &n) == -1) {
+                 perror("ioctl(BIOCSHDRCMPLT)");
+                 close(dev);
+ #ifdef __FreeBSD__
+                 if (flg) {
+                         munmap(bz.bz_bufa, bz.bz_buflen);
+                         munmap(bz.bz_bufb, bz.bz_buflen);
+                 } else
+ #endif
+                         munmap(buffer, siz);
+                 return 1;
+         }
+         if (ioctl(dev, BIOCIMMEDIATE, &n) == -1) {
+                 perror("ioctl(BIOCIMMEDIATE)");
+                 close(dev);
+ #ifdef __FreeBSD__
+                 if (flg) {
+                         munmap(bz.bz_bufa, bz.bz_buflen);
+                         munmap(bz.bz_bufb, bz.bz_buflen);
+                 } else
+ #endif
+                         munmap(buffer, siz);
+                 return 1;
+         }
+         pfd.fd = dev;
+         assert(!clock_gettime(CLOCK_REALTIME, &ts_start));
+         if (mode == 'R') {
+                 pfd.events = POLLIN;
+                 for (i = 0; i < count; i++) {
+                         if (poll(&pfd, 1, -1) == -1)
+                                 break;
+ #ifdef __FreeBSD__
+                         if (flg) {
+                                 if (!NEXT_zbuf((void**) &buffer, &bz, &ret))
+                                         continue;
+                                 if (Verbose) {
+                                         printf("+readed %d bytes\n", ret);
+                                         pthread_create(&tid, NULL, ShowPkt, buffer);
+                                 }
+                         } else {
+ #endif
+                                 ret = read(dev, buffer, siz);
+                                 if (ret == -1)
+                                         printf("%d) read(%d) #%d - %s\n", i, ret, errno, strerror(errno));
+                                 if (Verbose) {
+                                         printf("%d) +readed %d bytes\n", i, ret);
+                                         pthread_create(&tid, NULL, ShowPkt, buffer);
+                                 }
+ #ifdef __FreeBSD__
+                         }
+ #endif
+                 }
+         } else {
+                 pfd.events = POLLOUT;
+                 for (i = 0; i < count; i++) {
+ #ifdef __FreeBSD__
+                         if (poll(&pfd, 1, -1) == -1)
+                                 break;
+ #endif
+                         ret = write(dev, buffer, siz);
+                         if (ret == -1)
+                                 printf("write(%d) #%d - %s\n", ret, errno, strerror(errno));
+                         if (Verbose)
+                                 printf("+writed %d bytes\n", ret);
+                 }
+         }
+         assert(!clock_gettime(CLOCK_REALTIME, &ts_end));
+ #endif
+ //              munmap(buffer, siz);
+         io_etherClose(dev, &bz);
+         time_spec_sub(&ts_end, &ts_start);
+         printf("%d.%09lu for %d iterations\n", ts_end.tv_sec, ts_end.tv_nsec, i);
+         ts_end.tv_sec *= 1000000000 / i;
+         printf("0.%09lu per/iteration\n", ts_end.tv_sec + ts_end.tv_nsec / i);
+         return 0;
+ }

FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>

Removed from v.1.1
changed lines
	Added in v.1.1.2.1