Return to bpf.c CVS log

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

version 6.7

#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

#include <aitio.h>

#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 };
#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);

	eth = (struct ether_header *) buffer;

	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], szEA[STRSIZ], 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;
	ether_addr_t ea;

	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);

	printf("io_getmaciface(%s) -> %d\n", szStr, io_getmaciface(szStr, &ea));
	e_ether_ntoa(&ea, szEA, sizeof szEA);
	printf("ethernet address is %s\n", szEA);

#ifdef __FreeBSD__
	dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, 0, (u_int*) &siz, (flg) ? &bz : NULL);
	if (dev == -1)
		dev = io_etherOpen(szStr, O_RDWR | O_NONBLOCK, 42, 0, (u_int*) &siz, NULL);
#else
	dev = io_etherOpen(szStr, O_RDWR, 42, 0, (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;
}