--- libaitio/src/bpf.c 2013/06/24 14:43:00 1.1.2.3 +++ libaitio/src/bpf.c 2014/02/08 20:15:58 1.5.4.4 @@ -1,3 +1,48 @@ +/************************************************************************* +* (C) 2013 AITNET ltd - Sofia/Bulgaria - +* by Michael Pounov +* +* $Author: misho $ +* $Id: bpf.c,v 1.5.4.4 2014/02/08 20:15:58 misho Exp $ +* +************************************************************************** +The ELWIX and AITNET software is distributed under the following +terms: + +All of the documentation and software included in the ELWIX and AITNET +Releases is copyrighted by ELWIX - Sofia/Bulgaria + +Copyright 2004 - 2014 + by Michael Pounov . All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. +3. All advertising materials mentioning features or use of this software + must display the following acknowledgement: +This product includes software developed by Michael Pounov +ELWIX - Embedded LightWeight unIX and its contributors. +4. Neither the name of AITNET nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS +OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY +OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF +SUCH DAMAGE. +*/ #include "global.h" @@ -23,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 @@ -32,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 @@ -40,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); @@ -50,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) { @@ -90,18 +172,28 @@ allocZCbuf(u_int len) * @csIface = interface name * @flags = open flags * @whdr = with complete headers + * @wdlt = with data link type * @buflen = buffer length * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL * return: -1 error or >-1 bpf handle */ int -io_etherOpen(const char *csIface, int flags, int whdr, u_int buflen, void **zcbuf) +io_etherOpen(const char *csIface, int flags, u_int whdr, u_int wdlt, + u_int *buflen, void **zcbuf) { - int n = 1, eth = -1; + int eth = -1; register int i; char szStr[STRSIZ]; struct ifreq ifr; + u_int n = 1; +#if !defined(__FreeBSD__) || !defined(ZCBUF_ENABLE) + if (zcbuf) { + io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported"); + return -1; + } +#endif + for (i = 0; i < BPF_DEV_MAX; i++) { memset(szStr, 0, sizeof szStr); snprintf(szStr, sizeof szStr, "/dev/bpf%d", i); @@ -114,54 +206,75 @@ io_etherOpen(const char *csIface, int flags, int whdr, return -1; } - if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) { + if (csIface) + strlcpy(szStr, csIface, sizeof szStr); + else if (io_get1stiface(szStr, sizeof szStr) == -1) { + close(eth); + return -1; + } + + n = wdlt; + if (wdlt && ioctl(eth, BIOCSDLT, &n) == -1) { LOGERR; close(eth); return -1; } + n = 1; if (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) { LOGERR; close(eth); return -1; } - if (buflen) { - if (!zcbuf) { - if (ioctl(eth, BIOCSBLEN, &buflen) == -1) { - LOGERR; - close(eth); - return -1; - } - } else { -#ifdef __FreeBSD__ - n = BPF_BUFMODE_ZBUF; - if (ioctl(eth, BIOCSETBUFMODE, &n) == -1) { - LOGERR; - close(eth); - return -1; - } - if (ioctl(eth, BIOCGETZMAX, &n) == -1) { - LOGERR; - close(eth); - return -1; - } - if (!(*zcbuf = allocZCbuf(MIN(n, buflen)))) { - close(eth); - return -1; - } - if (ioctl(eth, BIOCSETZBUF, (struct bpf_zbuf*) *zcbuf) == -1) { - LOGERR; - io_etherClose(eth, zcbuf); - return -1; - } -#endif - } - } - if (csIface) - strlcpy(szStr, csIface, sizeof szStr); - else if (io_get1stiface(szStr, sizeof szStr) == -1) { - io_etherClose(eth, zcbuf); + if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) { + LOGERR; + close(eth); return -1; } + + if (!zcbuf) { + if (ioctl(eth, BIOCGBLEN, &n) == -1) { + LOGERR; + close(eth); + return -1; + } else + n = (buflen && *buflen) ? *buflen : getpagesize(); + + if (ioctl(eth, BIOCSBLEN, &n) == -1) { + LOGERR; + close(eth); + return -1; + } + if (buflen) + *buflen = n; + } else { +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) + n = BPF_BUFMODE_ZBUF; + if (ioctl(eth, BIOCSETBUFMODE, &n) == -1) { + LOGERR; + close(eth); + return -1; + } + if (ioctl(eth, BIOCGETZMAX, &n) == -1) { + LOGERR; + close(eth); + return -1; + } else + n = (buflen && *buflen) ? *buflen : n; + if (!(*zcbuf = allocZCbuf(n))) { + close(eth); + return -1; + } + if (ioctl(eth, BIOCSETZBUF, *zcbuf) == -1) { + LOGERR; + io_etherClose(eth, zcbuf); + return -1; + } + if (buflen && *buflen) + *buflen = n; +#endif + } + + memset(&ifr, 0, sizeof ifr); strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name); if (ioctl(eth, BIOCSETIF, &ifr) == -1) { LOGERR; @@ -170,4 +283,175 @@ io_etherOpen(const char *csIface, int flags, int whdr, } return eth; +} + +/* + * io_etherSend() - Send packet to bpf + * + * @eth = bpf handle + * @buf = buffer + * @buflen = buffer length + * return: -1 error or !=-1 written bytes + */ +ssize_t +io_etherSend(int eth, const void *buf, size_t buflen) +{ + ssize_t wlen = 0; + + if (!buf || !buflen) { + io_SetErr(EINVAL, "invalid arguments"); + return -1; + } + + wlen = write(eth, buf, buflen); + if (wlen == -1) + LOGERR; + return wlen; +} + +#if defined(__FreeBSD__) && defined(ZCBUF_ENABLE) +static inline void +ackZCbuf(struct bpf_zbuf_header *bzh) +{ + atomic_store_rel_int(&bzh->bzh_user_gen, bzh->bzh_kernel_gen); +} + +static inline int +chkZCbuf(struct bpf_zbuf_header *bzh) +{ + /* return true if userspace owns buffer, and false otherwise. */ + return (bzh->bzh_user_gen != atomic_load_acq_int(&bzh->bzh_kernel_gen)); +} + +static ssize_t +nextZCbuf(int eth, struct bpf_zbuf * __restrict zbuf, void * __restrict buf, size_t buflen) +{ + ssize_t rlen = 0; + struct bpf_zbuf bz; + struct bpf_zbuf_header *bzh; + off_t pos = 0; + + bzh = (struct bpf_zbuf_header *) zbuf->bz_bufa; + if (chkZCbuf(bzh)) { + rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen); + memcpy(buf + pos, zbuf->bz_bufa + sizeof(struct bpf_zbuf_header), rlen); + ackZCbuf(bzh); + pos += rlen; + } + bzh = (struct bpf_zbuf_header *) zbuf->bz_bufb; + if (chkZCbuf(bzh)) { + rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen); + memcpy(buf + pos, zbuf->bz_bufb + sizeof(struct bpf_zbuf_header), rlen); + ackZCbuf(bzh); + pos += rlen; + } + + if (!pos) { + if ((rlen = ioctl(eth, BIOCROTZBUF, &bz)) == -1) + LOGERR; + } else + rlen = pos; + return rlen; +} +#endif + +/* + * io_etherRecv() - Receive packet from bpf + * + * @eth = bpf handle + * @buf = buffer + * @buflen = buffer length + * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL + * return: -1 error or !=-1 readed bytes + */ +ssize_t +io_etherRecv(int eth, void * __restrict buf, size_t buflen, void * __restrict zcbuf) +{ + ssize_t rlen = 0; + struct bpf_hdr *h; + + if (!buf || !buflen) { + io_SetErr(EINVAL, "invalid arguments"); + return -1; + } + + if (!zcbuf) { + rlen = read(eth, buf, buflen); + if (rlen == -1) { + LOGERR; + return -1; + } + } else { +#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); +#else + rlen = -1; + io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported"); +#endif + } + + h = (struct bpf_hdr*) buf; + rlen -= h->bh_hdrlen; + + if (h->bh_caplen != rlen) { + if (h->bh_caplen < rlen) + rlen = h->bh_caplen; + else { + io_SetErr(EIO, "Captured %d bytes should be at most %d bytes", + h->bh_caplen, rlen); + return -1; + } + } + + 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; }