Annotation of libaitio/example/bpf.c, revision 1.1.2.1
1.1.2.1 ! misho 1: #include <stdio.h>
! 2: #include <stdlib.h>
! 3: #include <fcntl.h>
! 4: #include <unistd.h>
! 5: #include <string.h>
! 6: #include <errno.h>
! 7: #include <poll.h>
! 8: #include <getopt.h>
! 9: #include <pthread.h>
! 10: #include <assert.h>
! 11: #include <sys/types.h>
! 12: #include <sys/param.h>
! 13: #include <sys/limits.h>
! 14: #include <sys/socket.h>
! 15: #include <sys/ioctl.h>
! 16: #include <sys/mman.h>
! 17: #include <net/if.h>
! 18: #include <net/bpf.h>
! 19: #include <machine/atomic.h>
! 20:
! 21: #ifdef __FreeBSD__
! 22: #include <net/ethernet.h>
! 23: #endif
! 24: #ifdef __OpenBSD__
! 25: #include <net/ethertypes.h>
! 26: #include <netinet/in_systm.h>
! 27: #endif
! 28: #include <net/if_arp.h>
! 29: #include <netinet/in.h>
! 30: #include <netinet/ip.h>
! 31: #include <netinet/ip6.h>
! 32: #include <netinet/udp.h>
! 33: #include <netinet/tcp.h>
! 34: #include <netinet/ip_icmp.h>
! 35: #include <arpa/inet.h>
! 36: #ifdef __OpenBSD__
! 37: #include <netinet/if_ether.h>
! 38: #endif
! 39:
! 40: #ifndef roundup
! 41: #define roundup(x, y) ((((x) + ((y) - 1)) / (y)) * (y)) /* to any y */
! 42: #endif
! 43:
! 44: #define time_spec_sub(vvp, uvp) \
! 45: do { \
! 46: (vvp)->tv_sec -= (uvp)->tv_sec; \
! 47: (vvp)->tv_nsec -= (uvp)->tv_nsec; \
! 48: if ((vvp)->tv_nsec < 0) { \
! 49: (vvp)->tv_sec--; \
! 50: (vvp)->tv_nsec += 1000000000; \
! 51: } \
! 52: } while (0)
! 53:
! 54:
! 55: int Verbose, flg;
! 56:
! 57:
! 58: static void *
! 59: ShowPkt(void *buffer)
! 60: {
! 61: char Proto = 0, szStr[BUFSIZ], szLine[BUFSIZ], szWrk[BUFSIZ], szShow[USHRT_MAX] = { 0 };
! 62: struct bpf_hdr *bpf = buffer;
! 63: #ifdef __FreeBSD__
! 64: struct bpf_zbuf_header *bzh = buffer;
! 65: struct icmphdr *icmp;
! 66: #endif
! 67: #ifdef __OpenBSD__
! 68: struct icmp *icmp;
! 69: #endif
! 70: struct ether_header *eth;
! 71: struct ip *ip;
! 72: struct ip6_hdr *ipv6;
! 73: struct arphdr *arp;
! 74: struct udphdr *udp;
! 75: struct tcphdr *tcp;
! 76:
! 77: assert(buffer);
! 78:
! 79: #ifdef __FreeBSD__
! 80: snprintf(szLine, BUFSIZ, "#Packet length: %d\n>>> Ethernet ...\n", flg ? bzh->bzh_kernel_len : bpf->bh_datalen);
! 81: strlcat(szShow, szLine, USHRT_MAX);
! 82: eth = (struct ether_header *) (buffer + (flg ? bzh->bzh_kernel_len : bpf->bh_hdrlen));
! 83: #else
! 84: snprintf(szLine, BUFSIZ, "#Packet length: %d\n>>> Ethernet ...\n", bpf->bh_datalen);
! 85: strlcat(szShow, szLine, USHRT_MAX);
! 86: eth = (struct ether_header *) (buffer + bpf->bh_hdrlen);
! 87: #endif
! 88:
! 89: switch (ntohs(eth->ether_type)) {
! 90: case ETHERTYPE_ARP:
! 91: strlcpy(szWrk, "(ARP)", sizeof szWrk);
! 92: break;
! 93: case ETHERTYPE_IP:
! 94: strlcpy(szWrk, "(IP)", sizeof szWrk);
! 95: break;
! 96: case ETHERTYPE_IPV6:
! 97: strlcpy(szWrk, "(IPv6)", sizeof szWrk);
! 98: break;
! 99: default:
! 100: memset(szWrk, 0, sizeof szWrk);
! 101: }
! 102: snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_shost[0], eth->ether_shost[1],
! 103: eth->ether_shost[2], eth->ether_shost[3], eth->ether_shost[4], eth->ether_shost[5]);
! 104: snprintf(szLine, BUFSIZ, "ether_type: %04x%s, src_mac: %s, ", ntohs(eth->ether_type), szWrk, szStr);
! 105: strlcat(szShow, szLine, USHRT_MAX);
! 106: snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_dhost[0], eth->ether_dhost[1],
! 107: eth->ether_dhost[2], eth->ether_dhost[3], eth->ether_dhost[4], eth->ether_dhost[5]);
! 108: snprintf(szLine, BUFSIZ, "dst_mac: %s\n", szStr);
! 109: strlcat(szShow, szLine, USHRT_MAX);
! 110:
! 111: switch (ntohs(eth->ether_type)) {
! 112: case ETHERTYPE_ARP:
! 113: arp = (struct arphdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
! 114: strlcat(szShow, "\t>>> ARP ...\n", USHRT_MAX);
! 115:
! 116: switch (ntohs(arp->ar_op)) {
! 117: case ARPOP_REQUEST:
! 118: strlcpy(szStr, "Request", sizeof szStr);
! 119: break;
! 120: case ARPOP_REPLY:
! 121: strlcpy(szStr, "Reply", sizeof szStr);
! 122: break;
! 123: case ARPOP_REVREQUEST:
! 124: strlcpy(szStr, "RevRequest", sizeof szStr);
! 125: break;
! 126: case ARPOP_REVREPLY:
! 127: strlcpy(szStr, "RevReply", sizeof szStr);
! 128: break;
! 129: case ARPOP_INVREQUEST:
! 130: strlcpy(szStr, "InvRequest", sizeof szStr);
! 131: break;
! 132: case ARPOP_INVREPLY:
! 133: strlcpy(szStr, "InvReply", sizeof szStr);
! 134: break;
! 135: default:
! 136: strlcpy(szStr, "*Unknown*", sizeof szStr);
! 137: }
! 138: snprintf(szLine, BUFSIZ, "\tAddr_fmt: %d, Proto_fmt: %d, Addr_len: %d, Proto_len: %d, %s\n",
! 139: ntohs(arp->ar_hrd), ntohs(arp->ar_pro), arp->ar_hln, arp->ar_pln, szStr);
! 140: strlcat(szShow, szLine, USHRT_MAX);
! 141: break;
! 142: case ETHERTYPE_IP:
! 143: ip = (struct ip*) (((caddr_t) eth) + ETHER_HDR_LEN);
! 144: strlcat(szShow, "\t>>> IP ...\n", USHRT_MAX);
! 145:
! 146: snprintf(szStr, BUFSIZ, "Frag_off: %d", ntohs(ip->ip_off) & IP_OFFMASK);
! 147: if (ntohs(ip->ip_off) & IP_RF)
! 148: strlcat(szStr, "|RF", sizeof szStr);
! 149: if (ntohs(ip->ip_off) & IP_DF)
! 150: strlcat(szStr, "|DF", sizeof szStr);
! 151: if (ntohs(ip->ip_off) & IP_MF)
! 152: strlcat(szStr, "|MF", sizeof szStr);
! 153: snprintf(szLine, BUFSIZ, "\tTOS: %02x, TTL: %d, ID: %04x, Cksum: %04x, Len: %d, %s\n",
! 154: ip->ip_tos, ip->ip_ttl, ntohs(ip->ip_id), ntohs(ip->ip_sum),
! 155: ntohs(ip->ip_len), szStr);
! 156: strlcat(szShow, szLine, USHRT_MAX);
! 157: snprintf(szLine, BUFSIZ, "\tProto: %d, Src_addr: %s, Dst_addr: %s\n",
! 158: ip->ip_p, inet_ntop(AF_INET, &ip->ip_src, szStr, BUFSIZ),
! 159: inet_ntop(AF_INET, &ip->ip_dst, szWrk, BUFSIZ));
! 160: strlcat(szShow, szLine, USHRT_MAX);
! 161:
! 162: Proto = ip->ip_p;
! 163: break;
! 164: case ETHERTYPE_IPV6:
! 165: ipv6 = (struct ip6_hdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
! 166: strlcat(szShow, "\t>>> IPv6 ...\n", USHRT_MAX);
! 167:
! 168: snprintf(szLine, BUFSIZ, "\tHLim: %d, Payload_len: %d, Flow: %u\n", ipv6->ip6_hlim,
! 169: htons(ipv6->ip6_plen), htonl(ipv6->ip6_flow) & IPV6_FLOWLABEL_MASK);
! 170: strlcat(szShow, szLine, USHRT_MAX);
! 171: inet_ntop(AF_INET6, &ipv6->ip6_src, szStr, BUFSIZ);
! 172: inet_ntop(AF_INET6, &ipv6->ip6_dst, szWrk, BUFSIZ);
! 173: snprintf(szLine, BUFSIZ, "\tNext: %d, Src_addr: %s, Dst_addr: %s\n", ipv6->ip6_nxt, szStr, szWrk);
! 174: strlcat(szShow, szLine, USHRT_MAX);
! 175:
! 176: Proto = ipv6->ip6_nxt;
! 177: break;
! 178: }
! 179:
! 180: if (Proto && (ntohs(eth->ether_type) == ETHERTYPE_IP || ntohs(eth->ether_type) == ETHERTYPE_IPV6))
! 181: switch (Proto) {
! 182: case IPPROTO_TCP:
! 183: strlcat(szShow, "\t\t>>> TCP ...\n", USHRT_MAX);
! 184: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
! 185: tcp = (struct tcphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
! 186: else
! 187: tcp = (struct tcphdr*) (((caddr_t) ip) + sizeof(struct ip));
! 188:
! 189: snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, seq: %u, ack: %u\n",
! 190: ntohs(tcp->th_sport), ntohs(tcp->th_dport), tcp->th_seq, tcp->th_ack);
! 191: strlcat(szShow, szLine, USHRT_MAX);
! 192: snprintf(szWrk, BUFSIZ, "%d", (u_char) tcp->th_off >> 4);
! 193: if (tcp->th_flags & TH_FIN)
! 194: strlcat(szWrk, "|FIN", sizeof szWrk);
! 195: if (tcp->th_flags & TH_SYN)
! 196: strlcat(szWrk, "|SYN", sizeof szWrk);
! 197: if (tcp->th_flags & TH_RST)
! 198: strlcat(szWrk, "|RST", sizeof szWrk);
! 199: if (tcp->th_flags & TH_PUSH)
! 200: strlcat(szWrk, "|PUSH", sizeof szWrk);
! 201: if (tcp->th_flags & TH_ACK)
! 202: strlcat(szWrk, "|ACK", sizeof szWrk);
! 203: if (tcp->th_flags & TH_URG)
! 204: strlcat(szWrk, "|URG", sizeof szWrk);
! 205: if (tcp->th_flags & TH_ECE)
! 206: strlcat(szWrk, "|ECE", sizeof szWrk);
! 207: if (tcp->th_flags & TH_CWR)
! 208: strlcat(szWrk, "|CWR", sizeof szWrk);
! 209: snprintf(szLine, BUFSIZ, "\t\toff: %s, win: %d, urp: %04x, cksum: %04x\n",
! 210: szWrk, ntohs(tcp->th_win), ntohs(tcp->th_urp), ntohs(tcp->th_sum));
! 211: strlcat(szShow, szLine, USHRT_MAX);
! 212: break;
! 213: case IPPROTO_UDP:
! 214: strlcat(szShow, "\t\t>>> UDP ...\n", USHRT_MAX);
! 215: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
! 216: udp = (struct udphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
! 217: else
! 218: udp = (struct udphdr*) (((caddr_t) ip) + sizeof(struct ip));
! 219:
! 220: snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, len: %d, cksum: %04x\n",
! 221: ntohs(udp->uh_sport), ntohs(udp->uh_dport),
! 222: ntohs(udp->uh_ulen), ntohs(udp->uh_sum));
! 223: strlcat(szShow, szLine, USHRT_MAX);
! 224: break;
! 225: case IPPROTO_ICMP:
! 226: strlcat(szShow, "\t\t>>> ICMP ...\n", USHRT_MAX);
! 227: #ifdef __FreeBSD__
! 228: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
! 229: icmp = (struct icmphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
! 230: else
! 231: icmp = (struct icmphdr*) (((caddr_t) ip) + sizeof(struct ip));
! 232: #endif
! 233: #ifdef __OpenBSD__
! 234: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
! 235: icmp = (struct icmp*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
! 236: else
! 237: icmp = (struct icmp*) (((caddr_t) ip) + sizeof(struct ip));
! 238: #endif
! 239:
! 240: snprintf(szLine, BUFSIZ, "\t\ttype: %d, code: %d: cksum: %04x\n",
! 241: icmp->icmp_type, icmp->icmp_code, ntohs(icmp->icmp_cksum));
! 242: strlcat(szShow, szLine, USHRT_MAX);
! 243: break;
! 244: }
! 245:
! 246: printf("%s===\n", szShow);
! 247: pthread_exit(NULL);
! 248: }
! 249:
! 250: // ----------------------
! 251:
! 252: int
! 253: main(int argc, char **argv)
! 254: {
! 255: u_int n, count = (u_int) -1;
! 256: register int i;
! 257: int dev, fd, ret, siz = 0;
! 258: char szStr[BUFSIZ], szMap[MAXPATHLEN] = { 0 }, *buffer = NULL;
! 259: struct ifreq ifr;
! 260: struct pollfd pfd = { 0 };
! 261: pthread_t tid;
! 262: char ch, mode = 'R';
! 263: struct timespec ts_start, ts_end;
! 264: #ifdef __FreeBSD__
! 265: struct bpf_zbuf *bz;
! 266: #endif
! 267:
! 268: while ((ch = getopt(argc, argv, "hvwzs:p:f:")) != -1)
! 269: switch (ch) {
! 270: case 'w':
! 271: mode = 'W';
! 272: break;
! 273: case 's':
! 274: siz = strtol(optarg, NULL, 0);
! 275: break;
! 276: case 'p':
! 277: count = strtol(optarg, NULL, 0);
! 278: break;
! 279: case 'f':
! 280: strlcpy(szMap, optarg, sizeof szMap);
! 281: break;
! 282: case 'z':
! 283: flg++;
! 284: break;
! 285: case 'v':
! 286: Verbose++;
! 287: break;
! 288: case 'h':
! 289: default:
! 290: return 1;
! 291: }
! 292: argc -= optind;
! 293: argv += optind;
! 294: if (argc < 1)
! 295: return 1;
! 296:
! 297: if (**argv == '/')
! 298: strlcpy(szStr, strrchr(*argv, '/') + 1, sizeof szStr);
! 299: else
! 300: strlcpy(szStr, *argv, sizeof szStr);
! 301:
! 302: #ifdef __FreeBSD_
! 303: dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, &siz, &bz);
! 304: #else
! 305: dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, &siz, NULL);
! 306: #endif
! 307: if (dev == -1) {
! 308: printf("Error:: #%d - %s\n", io_GetErrno(), io_GetError());
! 309: return 1;
! 310: } else
! 311: printf("dev=%d(%s)\n", dev, szStr);
! 312:
! 313: if (ioctl(dev, BIOCGBLEN, &n) == -1) {
! 314: perror("ioctl(BIOCGBLEN)");
! 315: close(dev);
! 316: return 1;
! 317: } else
! 318: printf("BPF buffer len=%d\n", n);
! 319:
! 320: #if 0
! 321: #ifdef __FreeBSD__
! 322: ret = BPF_BUFMODE_ZBUF;
! 323: if (flg && !ioctl(dev, BIOCSETBUFMODE, (u_int*) &ret)) {
! 324: if (ioctl(dev, BIOCGETZMAX, &ret) == -1) {
! 325: perror("ioctl(BIOCGETZMAX)");
! 326: close(dev);
! 327: return 1;
! 328: }
! 329: memset(&bz, 0, sizeof bz);
! 330: bz.bz_buflen = roundup(MIN(n, ret), getpagesize());
! 331: bz.bz_bufa = mmap(NULL, bz.bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
! 332: bz.bz_bufb = mmap(NULL, bz.bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
! 333: if (bz.bz_bufa == MAP_FAILED || bz.bz_bufb == MAP_FAILED) {
! 334: perror("mmap()");
! 335: close(dev);
! 336: return 1;
! 337: }
! 338:
! 339: if (ioctl(dev, BIOCSETZBUF, &bz) == -1) {
! 340: perror("ioctl(BIOCSETZBUF)");
! 341: munmap(bz.bz_bufa, bz.bz_buflen);
! 342: munmap(bz.bz_bufb, bz.bz_buflen);
! 343: close(dev);
! 344: return 1;
! 345: } else
! 346: siz = bz.bz_buflen;
! 347: } else {
! 348: #endif
! 349: flg = 0;
! 350: if (siz) {
! 351: if (ioctl(dev, BIOCSBLEN, &siz) == -1) {
! 352: perror("ioctl(BIOCSBLEN)");
! 353: close(dev);
! 354: return 1;
! 355: }
! 356: } else
! 357: siz = n;
! 358: #ifdef __FreeBSD__
! 359: }
! 360: #endif
! 361: printf("Set buffer len to %d\n", siz);
! 362:
! 363: if (!flg) {
! 364: if (*szMap) {
! 365: fd = open(szMap, O_RDWR);
! 366: if (fd == -1) {
! 367: perror("open(map)");
! 368: close(dev);
! 369: return 1;
! 370: }
! 371: buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_PRIVATE, fd, 0);
! 372: close(fd);
! 373: } else
! 374: buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
! 375: if (buffer == MAP_FAILED) {
! 376: perror("mmap()");
! 377: close(dev);
! 378: return 1;
! 379: }
! 380: }
! 381:
! 382: memset(&ifr, 0, sizeof ifr);
! 383: strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name);
! 384: if (ioctl(dev, BIOCSETIF, (char*) &ifr) == -1) {
! 385: perror("ioctl(BIOCSETIF)");
! 386: close(dev);
! 387: #ifdef __FreeBSD__
! 388: if (flg) {
! 389: munmap(bz.bz_bufa, bz.bz_buflen);
! 390: munmap(bz.bz_bufb, bz.bz_buflen);
! 391: } else
! 392: #endif
! 393: munmap(buffer, siz);
! 394: return 1;
! 395: }
! 396: n = 1;
! 397: if (ioctl(dev, BIOCSHDRCMPLT, &n) == -1) {
! 398: perror("ioctl(BIOCSHDRCMPLT)");
! 399: close(dev);
! 400: #ifdef __FreeBSD__
! 401: if (flg) {
! 402: munmap(bz.bz_bufa, bz.bz_buflen);
! 403: munmap(bz.bz_bufb, bz.bz_buflen);
! 404: } else
! 405: #endif
! 406: munmap(buffer, siz);
! 407: return 1;
! 408: }
! 409: if (ioctl(dev, BIOCIMMEDIATE, &n) == -1) {
! 410: perror("ioctl(BIOCIMMEDIATE)");
! 411: close(dev);
! 412: #ifdef __FreeBSD__
! 413: if (flg) {
! 414: munmap(bz.bz_bufa, bz.bz_buflen);
! 415: munmap(bz.bz_bufb, bz.bz_buflen);
! 416: } else
! 417: #endif
! 418: munmap(buffer, siz);
! 419: return 1;
! 420: }
! 421:
! 422: pfd.fd = dev;
! 423: assert(!clock_gettime(CLOCK_REALTIME, &ts_start));
! 424: if (mode == 'R') {
! 425: pfd.events = POLLIN;
! 426: for (i = 0; i < count; i++) {
! 427: if (poll(&pfd, 1, -1) == -1)
! 428: break;
! 429:
! 430: #ifdef __FreeBSD__
! 431: if (flg) {
! 432: if (!NEXT_zbuf((void**) &buffer, &bz, &ret))
! 433: continue;
! 434: if (Verbose) {
! 435: printf("+readed %d bytes\n", ret);
! 436: pthread_create(&tid, NULL, ShowPkt, buffer);
! 437: }
! 438: } else {
! 439: #endif
! 440: ret = read(dev, buffer, siz);
! 441: if (ret == -1)
! 442: printf("%d) read(%d) #%d - %s\n", i, ret, errno, strerror(errno));
! 443: if (Verbose) {
! 444: printf("%d) +readed %d bytes\n", i, ret);
! 445: pthread_create(&tid, NULL, ShowPkt, buffer);
! 446: }
! 447: #ifdef __FreeBSD__
! 448: }
! 449: #endif
! 450: }
! 451: } else {
! 452: pfd.events = POLLOUT;
! 453: for (i = 0; i < count; i++) {
! 454: #ifdef __FreeBSD__
! 455: if (poll(&pfd, 1, -1) == -1)
! 456: break;
! 457: #endif
! 458:
! 459: ret = write(dev, buffer, siz);
! 460: if (ret == -1)
! 461: printf("write(%d) #%d - %s\n", ret, errno, strerror(errno));
! 462: if (Verbose)
! 463: printf("+writed %d bytes\n", ret);
! 464: }
! 465: }
! 466: assert(!clock_gettime(CLOCK_REALTIME, &ts_end));
! 467: #endif
! 468: // munmap(buffer, siz);
! 469:
! 470: io_etherClose(dev, &bz);
! 471:
! 472: time_spec_sub(&ts_end, &ts_start);
! 473: printf("%d.%09lu for %d iterations\n", ts_end.tv_sec, ts_end.tv_nsec, i);
! 474: ts_end.tv_sec *= 1000000000 / i;
! 475: printf("0.%09lu per/iteration\n", ts_end.tv_sec + ts_end.tv_nsec / i);
! 476: return 0;
! 477: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>