--- libaitio/src/bpf.c 2013/07/01 20:43:10 1.2.2.1 +++ libaitio/src/bpf.c 2014/02/08 18:58:12 1.5.4.3 @@ -3,7 +3,7 @@ * by Michael Pounov * * $Author: misho $ -* $Id: bpf.c,v 1.2.2.1 2013/07/01 20:43:10 misho Exp $ +* $Id: bpf.c,v 1.5.4.3 2014/02/08 18:58:12 misho Exp $ * ************************************************************************** The ELWIX and AITNET software is distributed under the following @@ -12,7 +12,7 @@ terms: All of the documentation and software included in the ELWIX and AITNET Releases is copyrighted by ELWIX - Sofia/Bulgaria -Copyright 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013 +Copyright 2004 - 2014 by Michael Pounov . All rights reserved. Redistribution and use in source and binary forms, with or without @@ -68,6 +68,43 @@ io_get1stiface(char *szIface, int iflen) } /* + * io_getmaciface() - Get MAC address from interface name + * + * @csIface = interface name + * @ea = ethernet address + * return: -1 error, 0 ok or 1 not found + */ +int +io_getmaciface(const char *csIface, ether_addr_t * __restrict ea) +{ + struct ifaddrs *ifa, *ifp; + struct sockaddr_dl *dl; + int ret = 1; + + if (!csIface || !ea) + return -1; + else + memset(ea, 0, sizeof(ether_addr_t)); + + getifaddrs(&ifa); + for (ifp = ifa; ifp; ifp = ifp->ifa_next) + if (!strcmp(csIface, ifp->ifa_name) && ifp->ifa_addr && + ifp->ifa_addr->sa_family == AF_LINK) { + dl = (struct sockaddr_dl*) ifp->ifa_addr; + if ((dl->sdl_type == IFT_ETHER || dl->sdl_type == IFT_L2VLAN || + dl->sdl_type == IFT_BRIDGE) && + dl->sdl_alen == ETHER_ADDR_LEN) { + memcpy(ea, LLADDR(dl), sizeof(ether_addr_t)); + ret = 0; + break; + } + } + freeifaddrs(ifa); + + return ret; +} + +/* * io_etherClose() - Close BPF interface * * @eth = bpf handle @@ -77,7 +114,7 @@ io_get1stiface(char *szIface, int iflen) void io_etherClose(int eth, void **zcbuf) { -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) struct bpf_zbuf *zbuf = NULL; #endif @@ -85,7 +122,7 @@ io_etherClose(int eth, void **zcbuf) close(eth); if (zcbuf && *zcbuf) { -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) zbuf = *zcbuf; munmap(zbuf->bz_bufb, zbuf->bz_buflen); munmap(zbuf->bz_bufa, zbuf->bz_buflen); @@ -95,7 +132,7 @@ io_etherClose(int eth, void **zcbuf) } } -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) static inline struct bpf_zbuf * allocZCbuf(u_int len) { @@ -141,7 +178,7 @@ allocZCbuf(u_int len) * return: -1 error or >-1 bpf handle */ int -io_etherOpen(const char *csIface, int flags, int whdr, int wdlt, +io_etherOpen(const char *csIface, int flags, u_int whdr, u_int wdlt, u_int *buflen, void **zcbuf) { int eth = -1; @@ -150,7 +187,7 @@ io_etherOpen(const char *csIface, int flags, int whdr, struct ifreq ifr; u_int n = 1; -#ifndef __FreeBSD__ +#if !defined(__FreeBSD__) || !defined(ZCBUF_ENABLE) if (zcbuf) { io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported"); return -1; @@ -169,22 +206,47 @@ io_etherOpen(const char *csIface, int flags, int whdr, return -1; } + if (csIface) + strlcpy(szStr, csIface, sizeof szStr); + else if (io_get1stiface(szStr, sizeof szStr) == -1) { + close(eth); + return -1; + } + + n = 1; + if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) { + LOGERR; + close(eth); + return -1; + } + if (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) { + LOGERR; + close(eth); + return -1; + } + if (wdlt && ioctl(eth, BIOCSDLT, &wdlt) == -1) { + LOGERR; + close(eth); + return -1; + } + if (!zcbuf) { if (ioctl(eth, BIOCGBLEN, &n) == -1) { LOGERR; close(eth); return -1; } else - n = (buflen && *buflen) ? *buflen : n; + n = (buflen && *buflen) ? *buflen : getpagesize(); + if (ioctl(eth, BIOCSBLEN, &n) == -1) { LOGERR; close(eth); return -1; } - if (buflen && *buflen) + if (buflen) *buflen = n; } else { -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) n = BPF_BUFMODE_ZBUF; if (ioctl(eth, BIOCSETBUFMODE, &n) == -1) { LOGERR; @@ -211,12 +273,6 @@ io_etherOpen(const char *csIface, int flags, int whdr, #endif } - if (csIface) - strlcpy(szStr, csIface, sizeof szStr); - else if (io_get1stiface(szStr, sizeof szStr) == -1) { - io_etherClose(eth, zcbuf); - return -1; - } memset(&ifr, 0, sizeof ifr); strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name); if (ioctl(eth, BIOCSETIF, &ifr) == -1) { @@ -225,23 +281,6 @@ io_etherOpen(const char *csIface, int flags, int whdr, return -1; } - n = 1; - if (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) { - LOGERR; - io_etherClose(eth, zcbuf); - return -1; - } - if (wdlt && ioctl(eth, BIOCSDLT, &wdlt) == -1) { - LOGERR; - io_etherClose(eth, zcbuf); - return -1; - } - if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) { - LOGERR; - io_etherClose(eth, zcbuf); - return -1; - } - return eth; } @@ -269,7 +308,7 @@ io_etherSend(int eth, const void *buf, size_t buflen) return wlen; } -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) static inline void ackZCbuf(struct bpf_zbuf_header *bzh) { @@ -337,10 +376,12 @@ io_etherRecv(int eth, void * __restrict buf, size_t bu if (!zcbuf) { rlen = read(eth, buf, buflen); - if (rlen == -1) + if (rlen == -1) { LOGERR; + return -1; + } } else { -#ifdef __FreeBSD__ +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen); if (!rlen) rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen); @@ -365,4 +406,51 @@ io_etherRecv(int eth, void * __restrict buf, size_t bu memmove(buf, buf + h->bh_hdrlen, rlen); return rlen; +} + +/* + * io_etherFilter() - BPF filter routine + * + * @eth = bpf handle + * @io = filter direction + * (IO_ETHER_FILTER_PROMISC|IO_ETHER_FILTER_NOTREAD|IO_ETHER_FILTER_READ|IO_ETHER_FILTER_WRITE) + * @insn = BPF filter instruction array + * @insnlen = Length of BPF filter instruction array + * return: -1 error or 0 ok + */ +int +io_etherFilter(int eth, int io, struct bpf_insn * __restrict insn, size_t insnlen) +{ + int ret = 0; + struct bpf_program fcode = { 0 }; + + if (io != IO_ETHER_FILTER_PROMISC && (!insn || !insnlen)) { + io_SetErr(EINVAL, "invalid arguments"); + return -1; + } + + switch (io) { + case IO_ETHER_FILTER_PROMISC: /* promiscuous mode */ + ret = ioctl(eth, BIOCPROMISC, NULL); + break; + case IO_ETHER_FILTER_NOTREAD: /* read not filter */ + fcode.bf_len = insnlen / sizeof(struct bpf_insn); + fcode.bf_insns = insn; + ret = ioctl(eth, BIOCSETFNR, &fcode); + break; + case IO_ETHER_FILTER_READ: /* read filter */ + fcode.bf_len = insnlen / sizeof(struct bpf_insn); + fcode.bf_insns = insn; + ret = ioctl(eth, BIOCSETF, &fcode); + break; + case IO_ETHER_FILTER_WRITE: /* write filter */ + fcode.bf_len = insnlen / sizeof(struct bpf_insn); + fcode.bf_insns = insn; + ret = ioctl(eth, BIOCSETWF, &fcode); + break; + } + + if (ret == -1) + LOGERR; + return ret; }