![[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__
1.1.2.8 misho 82: bpf = (struct bpf_hdr*) (buffer + (flg ? sizeof(struct bpf_zbuf_header) : 0));
83: snprintf(szLine, BUFSIZ, "#Packet length: %d/%d\n>>> Ethernet ...\n", flg ? bzh->bzh_kernel_len : bpf->bh_datalen, bpf->bh_datalen);
1.1.2.1 misho 84: strlcat(szShow, szLine, USHRT_MAX);
1.1.2.8 misho 85: eth = (struct ether_header *) (buffer + (flg ? sizeof(struct bpf_zbuf_header) : 0) + bpf->bh_hdrlen);
1.1.2.1 misho 86: #else
87: snprintf(szLine, BUFSIZ, "#Packet length: %d\n>>> Ethernet ...\n", bpf->bh_datalen);
88: strlcat(szShow, szLine, USHRT_MAX);
89: eth = (struct ether_header *) (buffer + bpf->bh_hdrlen);
90: #endif
91:
92: switch (ntohs(eth->ether_type)) {
93: case ETHERTYPE_ARP:
94: strlcpy(szWrk, "(ARP)", sizeof szWrk);
95: break;
96: case ETHERTYPE_IP:
97: strlcpy(szWrk, "(IP)", sizeof szWrk);
98: break;
99: case ETHERTYPE_IPV6:
100: strlcpy(szWrk, "(IPv6)", sizeof szWrk);
101: break;
102: default:
103: memset(szWrk, 0, sizeof szWrk);
104: }
105: snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_shost[0], eth->ether_shost[1],
106: eth->ether_shost[2], eth->ether_shost[3], eth->ether_shost[4], eth->ether_shost[5]);
107: snprintf(szLine, BUFSIZ, "ether_type: %04x%s, src_mac: %s, ", ntohs(eth->ether_type), szWrk, szStr);
108: strlcat(szShow, szLine, USHRT_MAX);
109: snprintf(szStr, BUFSIZ, "%02x:%02x:%02x:%02x:%02x:%02x", eth->ether_dhost[0], eth->ether_dhost[1],
110: eth->ether_dhost[2], eth->ether_dhost[3], eth->ether_dhost[4], eth->ether_dhost[5]);
111: snprintf(szLine, BUFSIZ, "dst_mac: %s\n", szStr);
112: strlcat(szShow, szLine, USHRT_MAX);
113:
114: switch (ntohs(eth->ether_type)) {
115: case ETHERTYPE_ARP:
116: arp = (struct arphdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
117: strlcat(szShow, "\t>>> ARP ...\n", USHRT_MAX);
118:
119: switch (ntohs(arp->ar_op)) {
120: case ARPOP_REQUEST:
121: strlcpy(szStr, "Request", sizeof szStr);
122: break;
123: case ARPOP_REPLY:
124: strlcpy(szStr, "Reply", sizeof szStr);
125: break;
126: case ARPOP_REVREQUEST:
127: strlcpy(szStr, "RevRequest", sizeof szStr);
128: break;
129: case ARPOP_REVREPLY:
130: strlcpy(szStr, "RevReply", sizeof szStr);
131: break;
132: case ARPOP_INVREQUEST:
133: strlcpy(szStr, "InvRequest", sizeof szStr);
134: break;
135: case ARPOP_INVREPLY:
136: strlcpy(szStr, "InvReply", sizeof szStr);
137: break;
138: default:
139: strlcpy(szStr, "*Unknown*", sizeof szStr);
140: }
141: snprintf(szLine, BUFSIZ, "\tAddr_fmt: %d, Proto_fmt: %d, Addr_len: %d, Proto_len: %d, %s\n",
142: ntohs(arp->ar_hrd), ntohs(arp->ar_pro), arp->ar_hln, arp->ar_pln, szStr);
143: strlcat(szShow, szLine, USHRT_MAX);
144: break;
145: case ETHERTYPE_IP:
146: ip = (struct ip*) (((caddr_t) eth) + ETHER_HDR_LEN);
147: strlcat(szShow, "\t>>> IP ...\n", USHRT_MAX);
148:
149: snprintf(szStr, BUFSIZ, "Frag_off: %d", ntohs(ip->ip_off) & IP_OFFMASK);
150: if (ntohs(ip->ip_off) & IP_RF)
151: strlcat(szStr, "|RF", sizeof szStr);
152: if (ntohs(ip->ip_off) & IP_DF)
153: strlcat(szStr, "|DF", sizeof szStr);
154: if (ntohs(ip->ip_off) & IP_MF)
155: strlcat(szStr, "|MF", sizeof szStr);
156: snprintf(szLine, BUFSIZ, "\tTOS: %02x, TTL: %d, ID: %04x, Cksum: %04x, Len: %d, %s\n",
157: ip->ip_tos, ip->ip_ttl, ntohs(ip->ip_id), ntohs(ip->ip_sum),
158: ntohs(ip->ip_len), szStr);
159: strlcat(szShow, szLine, USHRT_MAX);
160: snprintf(szLine, BUFSIZ, "\tProto: %d, Src_addr: %s, Dst_addr: %s\n",
161: ip->ip_p, inet_ntop(AF_INET, &ip->ip_src, szStr, BUFSIZ),
162: inet_ntop(AF_INET, &ip->ip_dst, szWrk, BUFSIZ));
163: strlcat(szShow, szLine, USHRT_MAX);
164:
165: Proto = ip->ip_p;
166: break;
167: case ETHERTYPE_IPV6:
168: ipv6 = (struct ip6_hdr*) (((caddr_t) eth) + ETHER_HDR_LEN);
169: strlcat(szShow, "\t>>> IPv6 ...\n", USHRT_MAX);
170:
171: snprintf(szLine, BUFSIZ, "\tHLim: %d, Payload_len: %d, Flow: %u\n", ipv6->ip6_hlim,
172: htons(ipv6->ip6_plen), htonl(ipv6->ip6_flow) & IPV6_FLOWLABEL_MASK);
173: strlcat(szShow, szLine, USHRT_MAX);
174: inet_ntop(AF_INET6, &ipv6->ip6_src, szStr, BUFSIZ);
175: inet_ntop(AF_INET6, &ipv6->ip6_dst, szWrk, BUFSIZ);
176: snprintf(szLine, BUFSIZ, "\tNext: %d, Src_addr: %s, Dst_addr: %s\n", ipv6->ip6_nxt, szStr, szWrk);
177: strlcat(szShow, szLine, USHRT_MAX);
178:
179: Proto = ipv6->ip6_nxt;
180: break;
181: }
182:
183: if (Proto && (ntohs(eth->ether_type) == ETHERTYPE_IP || ntohs(eth->ether_type) == ETHERTYPE_IPV6))
184: switch (Proto) {
185: case IPPROTO_TCP:
186: strlcat(szShow, "\t\t>>> TCP ...\n", USHRT_MAX);
187: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
188: tcp = (struct tcphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
189: else
190: tcp = (struct tcphdr*) (((caddr_t) ip) + sizeof(struct ip));
191:
192: snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, seq: %u, ack: %u\n",
193: ntohs(tcp->th_sport), ntohs(tcp->th_dport), tcp->th_seq, tcp->th_ack);
194: strlcat(szShow, szLine, USHRT_MAX);
195: snprintf(szWrk, BUFSIZ, "%d", (u_char) tcp->th_off >> 4);
196: if (tcp->th_flags & TH_FIN)
197: strlcat(szWrk, "|FIN", sizeof szWrk);
198: if (tcp->th_flags & TH_SYN)
199: strlcat(szWrk, "|SYN", sizeof szWrk);
200: if (tcp->th_flags & TH_RST)
201: strlcat(szWrk, "|RST", sizeof szWrk);
202: if (tcp->th_flags & TH_PUSH)
203: strlcat(szWrk, "|PUSH", sizeof szWrk);
204: if (tcp->th_flags & TH_ACK)
205: strlcat(szWrk, "|ACK", sizeof szWrk);
206: if (tcp->th_flags & TH_URG)
207: strlcat(szWrk, "|URG", sizeof szWrk);
208: if (tcp->th_flags & TH_ECE)
209: strlcat(szWrk, "|ECE", sizeof szWrk);
210: if (tcp->th_flags & TH_CWR)
211: strlcat(szWrk, "|CWR", sizeof szWrk);
212: snprintf(szLine, BUFSIZ, "\t\toff: %s, win: %d, urp: %04x, cksum: %04x\n",
213: szWrk, ntohs(tcp->th_win), ntohs(tcp->th_urp), ntohs(tcp->th_sum));
214: strlcat(szShow, szLine, USHRT_MAX);
215: break;
216: case IPPROTO_UDP:
217: strlcat(szShow, "\t\t>>> UDP ...\n", USHRT_MAX);
218: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
219: udp = (struct udphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
220: else
221: udp = (struct udphdr*) (((caddr_t) ip) + sizeof(struct ip));
222:
223: snprintf(szLine, BUFSIZ, "\t\tsrc_port: %d, dst_port: %d, len: %d, cksum: %04x\n",
224: ntohs(udp->uh_sport), ntohs(udp->uh_dport),
225: ntohs(udp->uh_ulen), ntohs(udp->uh_sum));
226: strlcat(szShow, szLine, USHRT_MAX);
227: break;
228: case IPPROTO_ICMP:
229: strlcat(szShow, "\t\t>>> ICMP ...\n", USHRT_MAX);
230: #ifdef __FreeBSD__
231: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
232: icmp = (struct icmphdr*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
233: else
234: icmp = (struct icmphdr*) (((caddr_t) ip) + sizeof(struct ip));
235: #endif
236: #ifdef __OpenBSD__
237: if (ntohs(eth->ether_type) == ETHERTYPE_IPV6)
238: icmp = (struct icmp*) (((caddr_t) ipv6) + sizeof(struct ip6_hdr));
239: else
240: icmp = (struct icmp*) (((caddr_t) ip) + sizeof(struct ip));
241: #endif
242:
243: snprintf(szLine, BUFSIZ, "\t\ttype: %d, code: %d: cksum: %04x\n",
244: icmp->icmp_type, icmp->icmp_code, ntohs(icmp->icmp_cksum));
245: strlcat(szShow, szLine, USHRT_MAX);
246: break;
247: }
248:
249: printf("%s===\n", szShow);
250: pthread_exit(NULL);
251: }
252:
253: // ----------------------
254:
255: int
256: main(int argc, char **argv)
257: {
258: u_int n, count = (u_int) -1;
259: register int i;
260: int dev, fd, ret, siz = 0;
261: char szStr[BUFSIZ], szMap[MAXPATHLEN] = { 0 }, *buffer = NULL;
262: struct ifreq ifr;
263: struct pollfd pfd = { 0 };
264: pthread_t tid;
265: char ch, mode = 'R';
266: struct timespec ts_start, ts_end;
1.1.2.3 misho 267: void *bz = NULL;
1.1.2.1 misho 268:
269: while ((ch = getopt(argc, argv, "hvwzs:p:f:")) != -1)
270: switch (ch) {
271: case 'w':
272: mode = 'W';
273: break;
274: case 's':
275: siz = strtol(optarg, NULL, 0);
276: break;
277: case 'p':
278: count = strtol(optarg, NULL, 0);
279: break;
280: case 'f':
281: strlcpy(szMap, optarg, sizeof szMap);
282: break;
283: case 'z':
284: flg++;
285: break;
286: case 'v':
287: Verbose++;
288: break;
289: case 'h':
290: default:
291: return 1;
292: }
293: argc -= optind;
294: argv += optind;
295: if (argc < 1)
296: return 1;
297:
298: if (**argv == '/')
299: strlcpy(szStr, strrchr(*argv, '/') + 1, sizeof szStr);
300: else
301: strlcpy(szStr, *argv, sizeof szStr);
302:
1.1.2.4 misho 303: #ifdef __FreeBSD__
1.1.2.7 misho 304: dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, (u_int*) &siz, (flg) ? &bz : NULL);
1.1.2.4 misho 305: if (dev == -1)
1.1.2.7 misho 306: dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, (u_int*) &siz, NULL);
1.1.2.1 misho 307: #else
1.1.2.4 misho 308: dev = io_etherOpen(szStr, O_RDWR, 42, (u_int*) &siz, NULL);
1.1.2.1 misho 309: #endif
310: if (dev == -1) {
311: printf("Error:: #%d - %s\n", io_GetErrno(), io_GetError());
312: return 1;
313: } else
314: printf("dev=%d(%s)\n", dev, szStr);
315:
1.1.2.6 misho 316: if (ioctl(dev, BIOCGBLEN, &siz) == -1) {
1.1.2.1 misho 317: perror("ioctl(BIOCGBLEN)");
1.1.2.5 misho 318: io_etherClose(dev, &bz);
1.1.2.1 misho 319: return 1;
320: } else
1.1.2.6 misho 321: printf("BPF buffer len=%d\n", siz);
1.1.2.1 misho 322:
1.1.2.6 misho 323: if (*szMap) {
324: fd = open(szMap, O_RDWR);
325: if (fd == -1) {
326: perror("open(map)");
1.1.2.5 misho 327: io_etherClose(dev, &bz);
1.1.2.1 misho 328: return 1;
329: }
1.1.2.6 misho 330: buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_PRIVATE, fd, 0);
331: close(fd);
332: } else
333: buffer = mmap(NULL, siz, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
334: if (buffer == MAP_FAILED) {
335: perror("mmap()");
336: io_etherClose(dev, &bz);
337: return 1;
1.1.2.1 misho 338: }
339:
340: pfd.fd = dev;
341: assert(!clock_gettime(CLOCK_REALTIME, &ts_start));
342: if (mode == 'R') {
343: pfd.events = POLLIN;
344: for (i = 0; i < count; i++) {
345: if (poll(&pfd, 1, -1) == -1)
346: break;
347:
1.1.2.9 ! misho 348: ret = io_etherRecv(dev, buffer, siz, bz);
1.1.2.6 misho 349: if (ret == -1)
350: printf("%d) io_etherRecv(%d) #%d - %s\n", i, ret,
351: io_GetErrno(), io_GetError());
352: if (Verbose) {
353: printf("%d) +readed %d bytes\n", i, ret);
354: pthread_create(&tid, NULL, ShowPkt, buffer);
355: pthread_join(tid, NULL);
1.1.2.1 misho 356: }
357: }
358: } else {
359: pfd.events = POLLOUT;
360: for (i = 0; i < count; i++) {
361: if (poll(&pfd, 1, -1) == -1)
362: break;
363:
1.1.2.5 misho 364: ret = io_etherSend(dev, buffer, siz);
1.1.2.1 misho 365: if (ret == -1)
1.1.2.5 misho 366: printf("io_etherSend(%d) #%d - %s\n", ret, io_GetErrno(), io_GetError());
1.1.2.1 misho 367: if (Verbose)
368: printf("+writed %d bytes\n", ret);
369: }
370: }
371: assert(!clock_gettime(CLOCK_REALTIME, &ts_end));
372:
1.1.2.6 misho 373: munmap(buffer, siz);
1.1.2.1 misho 374: io_etherClose(dev, &bz);
375:
376: time_spec_sub(&ts_end, &ts_start);
377: printf("%d.%09lu for %d iterations\n", ts_end.tv_sec, ts_end.tv_nsec, i);
378: ts_end.tv_sec *= 1000000000 / i;
379: printf("0.%09lu per/iteration\n", ts_end.tv_sec + ts_end.tv_nsec / i);
380: return 0;
381: }