--- libaitio/example/bpf.c 2013/06/25 09:08:10 1.1 +++ libaitio/example/bpf.c 2013/06/26 22:48:53 1.2 @@ -0,0 +1,375 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __FreeBSD__ +#include +#endif +#ifdef __OpenBSD__ +#include +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef __OpenBSD__ +#include +#endif + +#include + +#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); + + 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); + + 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); + return 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; + void *bz = NULL; + + 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, (u_int*) &siz, (flg) ? &bz : NULL); + if (dev == -1) + dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, (u_int*) &siz, NULL); +#else + dev = io_etherOpen(szStr, O_RDWR, 42, (u_int*) &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, &siz) == -1) { + perror("ioctl(BIOCGBLEN)"); + io_etherClose(dev, &bz); + return 1; + } else + printf("BPF buffer len=%d\n", siz); + + if (*szMap) { + fd = open(szMap, O_RDWR); + if (fd == -1) { + perror("open(map)"); + io_etherClose(dev, &bz); + 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()"); + io_etherClose(dev, &bz); + 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 ((ret = poll(&pfd, 1, -1)) == -1) + break; + + ret = io_etherRecv(dev, buffer, siz, bz); + if (!ret) + continue; + if (ret == -1) + printf("%d) io_etherRecv(%d) #%d - %s\n", i, ret, + io_GetErrno(), io_GetError()); + if (Verbose) { + printf("%d) +readed %d bytes\n", i, ret); + ShowPkt(buffer); + } + } + } else { + pfd.events = POLLOUT; + for (i = 0; i < count; i++) { + if (poll(&pfd, 1, -1) == -1) + break; + + ret = io_etherSend(dev, buffer, siz); + if (ret == -1) + printf("io_etherSend(%d) #%d - %s\n", ret, io_GetErrno(), io_GetError()); + if (Verbose) + printf("+writed %d bytes\n", ret); + } + } + assert(!clock_gettime(CLOCK_REALTIME, &ts_end)); + + 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; +}