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