Annotation of libaitio/src/bpf.c, revision 1.5.4.3

1.2       misho       1: /*************************************************************************
                      2: * (C) 2013 AITNET ltd - Sofia/Bulgaria - <misho@aitnet.org>
                      3: *  by Michael Pounov <misho@elwix.org>
                      4: *
                      5: * $Author: misho $
1.5.4.3 ! misho       6: * $Id: bpf.c,v 1.5.4.2 2014/02/05 02:24:28 misho Exp $
1.2       misho       7: *
                      8: **************************************************************************
                      9: The ELWIX and AITNET software is distributed under the following
                     10: terms:
                     11: 
                     12: All of the documentation and software included in the ELWIX and AITNET
                     13: Releases is copyrighted by ELWIX - Sofia/Bulgaria <info@elwix.org>
                     14: 
1.5.4.2   misho      15: Copyright 2004 - 2014
1.2       misho      16:        by Michael Pounov <misho@elwix.org>.  All rights reserved.
                     17: 
                     18: Redistribution and use in source and binary forms, with or without
                     19: modification, are permitted provided that the following conditions
                     20: are met:
                     21: 1. Redistributions of source code must retain the above copyright
                     22:    notice, this list of conditions and the following disclaimer.
                     23: 2. Redistributions in binary form must reproduce the above copyright
                     24:    notice, this list of conditions and the following disclaimer in the
                     25:    documentation and/or other materials provided with the distribution.
                     26: 3. All advertising materials mentioning features or use of this software
                     27:    must display the following acknowledgement:
                     28: This product includes software developed by Michael Pounov <misho@elwix.org>
                     29: ELWIX - Embedded LightWeight unIX and its contributors.
                     30: 4. Neither the name of AITNET nor the names of its contributors
                     31:    may be used to endorse or promote products derived from this software
                     32:    without specific prior written permission.
                     33: 
                     34: THIS SOFTWARE IS PROVIDED BY AITNET AND CONTRIBUTORS ``AS IS'' AND
                     35: ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
                     36: IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
                     37: ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
                     38: FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
                     39: DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
                     40: OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
                     41: HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
                     42: LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
                     43: OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
                     44: SUCH DAMAGE.
                     45: */
                     46: #include "global.h"
                     47: 
                     48: 
                     49: /*
                     50:  * io_get1stiface() - Get first interface of host
                     51:  *
                     52:  * @szIface = interface string buffer
                     53:  * @iflen = size of interface buffer
                     54:  * return: -1 error or 0 ok
                     55:  */
                     56: int
                     57: io_get1stiface(char *szIface, int iflen)
                     58: {
                     59:        struct ifaddrs *ifa;
                     60: 
                     61:        if (!szIface || !iflen)
                     62:                return -1;
                     63: 
                     64:        getifaddrs(&ifa);
                     65:        strlcpy(szIface, ifa->ifa_name, iflen);
                     66:        freeifaddrs(ifa);
                     67:        return 0;
                     68: }
                     69: 
                     70: /*
1.5       misho      71:  * io_getmaciface() - Get MAC address from interface name
                     72:  *
                     73:  * @csIface = interface name
                     74:  * @ea = ethernet address
                     75:  * return: -1 error, 0 ok or 1 not found
                     76:  */
                     77: int
                     78: io_getmaciface(const char *csIface, ether_addr_t * __restrict ea)
                     79: {
                     80:        struct ifaddrs *ifa, *ifp;
                     81:        struct sockaddr_dl *dl;
                     82:        int ret = 1;
                     83: 
                     84:        if (!csIface || !ea)
                     85:                return -1;
                     86:        else
                     87:                memset(ea, 0, sizeof(ether_addr_t));
                     88: 
                     89:        getifaddrs(&ifa);
                     90:        for (ifp = ifa; ifp; ifp = ifp->ifa_next)
                     91:                if (!strcmp(csIface, ifp->ifa_name) && ifp->ifa_addr && 
                     92:                                ifp->ifa_addr->sa_family == AF_LINK) {
                     93:                        dl = (struct sockaddr_dl*) ifp->ifa_addr;
                     94:                        if ((dl->sdl_type == IFT_ETHER || dl->sdl_type == IFT_L2VLAN || 
                     95:                                        dl->sdl_type == IFT_BRIDGE) && 
                     96:                                        dl->sdl_alen == ETHER_ADDR_LEN) {
                     97:                                memcpy(ea, LLADDR(dl), sizeof(ether_addr_t));
                     98:                                ret = 0;
                     99:                                break;
                    100:                        }
                    101:                }
                    102:        freeifaddrs(ifa);
                    103: 
                    104:        return ret;
                    105: }
                    106: 
                    107: /*
1.2       misho     108:  * io_etherClose() - Close BPF interface
                    109:  *
                    110:  * @eth = bpf handle
                    111:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                    112:  * return: none
                    113:  */
                    114: void
                    115: io_etherClose(int eth, void **zcbuf)
                    116: {
1.5.4.1   misho     117: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     118:        struct bpf_zbuf *zbuf = NULL;
                    119: #endif
                    120: 
                    121:        if (eth > STDERR_FILENO)
                    122:                close(eth);
                    123: 
                    124:        if (zcbuf && *zcbuf) {
1.5.4.1   misho     125: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     126:                zbuf = *zcbuf;
                    127:                munmap(zbuf->bz_bufb, zbuf->bz_buflen);
                    128:                munmap(zbuf->bz_bufa, zbuf->bz_buflen);
                    129:                e_free(*zcbuf);
                    130:                *zcbuf = NULL;
                    131: #endif
                    132:        }
                    133: }
                    134: 
1.5.4.1   misho     135: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     136: static inline struct bpf_zbuf *
                    137: allocZCbuf(u_int len)
                    138: {
                    139:        struct bpf_zbuf *zbuf = NULL;
                    140: 
                    141:        zbuf = e_malloc(sizeof(struct bpf_zbuf));
                    142:        if (!zbuf) {
                    143:                io_SetErr(elwix_GetErrno(), "%s", elwix_GetError());
                    144:                return NULL;
                    145:        } else
                    146:                memset(zbuf, 0, sizeof(struct bpf_zbuf));
                    147: 
                    148:        zbuf->bz_buflen = roundup(len, getpagesize());
                    149:        zbuf->bz_bufa = mmap(NULL, zbuf->bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
                    150:        if (zbuf->bz_bufa == MAP_FAILED) {
                    151:                LOGERR;
                    152:                e_free(zbuf);
                    153:                return NULL;
                    154:        } else
                    155:                memset(zbuf->bz_bufa, 0, zbuf->bz_buflen);
                    156:        zbuf->bz_bufb = mmap(NULL, zbuf->bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
                    157:        if (zbuf->bz_bufb == MAP_FAILED) {
                    158:                LOGERR;
                    159:                munmap(zbuf->bz_bufa, zbuf->bz_buflen);
                    160:                e_free(zbuf);
                    161:                return NULL;
                    162:        } else
                    163:                memset(zbuf->bz_bufb, 0, zbuf->bz_buflen);
                    164: 
                    165:        return zbuf;
                    166: }
                    167: #endif
                    168: 
                    169: /*
                    170:  * io_etherOpen() - Open BPF interface to device
                    171:  *
                    172:  * @csIface = interface name
                    173:  * @flags = open flags
                    174:  * @whdr = with complete headers
1.3       misho     175:  * @wdlt = with data link type
1.2       misho     176:  * @buflen = buffer length
                    177:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                    178:  * return: -1 error or >-1 bpf handle
                    179:  */
                    180: int
1.5.4.2   misho     181: io_etherOpen(const char *csIface, int flags, u_int whdr, u_int wdlt, 
1.3       misho     182:                u_int *buflen, void **zcbuf)
1.2       misho     183: {
                    184:        int eth = -1;
                    185:        register int i;
                    186:        char szStr[STRSIZ];
                    187:        struct ifreq ifr;
                    188:        u_int n = 1;
                    189: 
1.5.4.1   misho     190: #if !defined(__FreeBSD__) || !defined(ZCBUF_ENABLE)
1.2       misho     191:        if (zcbuf) {
                    192:                io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported");
                    193:                return -1;
                    194:        }
                    195: #endif
                    196: 
                    197:        for (i = 0; i < BPF_DEV_MAX; i++) {
                    198:                memset(szStr, 0, sizeof szStr);
                    199:                snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
                    200:                eth = open(szStr, flags);
                    201:                if (eth > STDERR_FILENO)
                    202:                        break;
                    203:        }
                    204:        if (eth < 3) {
                    205:                LOGERR;
                    206:                return -1;
                    207:        }
                    208: 
1.4       misho     209:        if (csIface)
                    210:                strlcpy(szStr, csIface, sizeof szStr);
                    211:        else if (io_get1stiface(szStr, sizeof szStr) == -1) {
                    212:                close(eth);
                    213:                return -1;
                    214:        }
                    215: 
1.5.4.3 ! misho     216:        n = 1;
1.5.4.2   misho     217:        if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) {
1.4       misho     218:                LOGERR;
                    219:                close(eth);
                    220:                return -1;
                    221:        }
1.5.4.2   misho     222:        if (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) {
1.4       misho     223:                LOGERR;
                    224:                close(eth);
                    225:                return -1;
                    226:        }
1.5.4.2   misho     227:        if (wdlt && ioctl(eth, BIOCSDLT, &wdlt) == -1) {
1.4       misho     228:                LOGERR;
                    229:                close(eth);
                    230:                return -1;
                    231:        }
                    232: 
1.2       misho     233:        if (!zcbuf) {
                    234:                if (ioctl(eth, BIOCGBLEN, &n) == -1) {
                    235:                        LOGERR;
                    236:                        close(eth);
                    237:                        return -1;
                    238:                } else
1.4       misho     239:                        n = (buflen && *buflen) ? *buflen : getpagesize();
                    240: 
1.2       misho     241:                if (ioctl(eth, BIOCSBLEN, &n) == -1) {
                    242:                        LOGERR;
                    243:                        close(eth);
                    244:                        return -1;
                    245:                }
1.4       misho     246:                if (buflen)
1.2       misho     247:                        *buflen = n;
                    248:        } else {
1.5.4.1   misho     249: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     250:                n = BPF_BUFMODE_ZBUF;
                    251:                if (ioctl(eth, BIOCSETBUFMODE, &n) == -1) {
                    252:                        LOGERR;
                    253:                        close(eth);
                    254:                        return -1;
                    255:                }
                    256:                if (ioctl(eth, BIOCGETZMAX, &n) == -1) {
                    257:                        LOGERR;
                    258:                        close(eth);
                    259:                        return -1;
                    260:                } else
                    261:                        n = (buflen && *buflen) ? *buflen : n;
                    262:                if (!(*zcbuf = allocZCbuf(n))) {
                    263:                        close(eth);
                    264:                        return -1;
                    265:                }
                    266:                if (ioctl(eth, BIOCSETZBUF, *zcbuf) == -1) {
                    267:                        LOGERR;
                    268:                        io_etherClose(eth, zcbuf);
                    269:                        return -1;
                    270:                }
                    271:                if (buflen && *buflen)
                    272:                        *buflen = n;
                    273: #endif
                    274:        }
                    275: 
                    276:        memset(&ifr, 0, sizeof ifr);
                    277:        strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name);
                    278:        if (ioctl(eth, BIOCSETIF, &ifr) == -1) {
                    279:                LOGERR;
                    280:                io_etherClose(eth, zcbuf);
                    281:                return -1;
                    282:        }
                    283: 
                    284:        return eth;
                    285: }
                    286: 
                    287: /*
                    288:  * io_etherSend() - Send packet to bpf
                    289:  *
                    290:  * @eth = bpf handle
                    291:  * @buf = buffer
                    292:  * @buflen = buffer length
                    293:  * return: -1 error or !=-1 written bytes
                    294:  */
                    295: ssize_t
                    296: io_etherSend(int eth, const void *buf, size_t buflen)
                    297: {
                    298:        ssize_t wlen = 0;
                    299: 
                    300:        if (!buf || !buflen) {
                    301:                io_SetErr(EINVAL, "invalid arguments");
                    302:                return -1;
                    303:        }
                    304: 
                    305:        wlen = write(eth, buf, buflen);
                    306:        if (wlen == -1)
                    307:                LOGERR;
                    308:        return wlen;
                    309: }
                    310: 
1.5.4.1   misho     311: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     312: static inline void
                    313: ackZCbuf(struct bpf_zbuf_header *bzh)
                    314: {
                    315:        atomic_store_rel_int(&bzh->bzh_user_gen, bzh->bzh_kernel_gen);
                    316: }
                    317: 
                    318: static inline int
                    319: chkZCbuf(struct bpf_zbuf_header *bzh)
                    320: {
                    321:        /* return true if userspace owns buffer, and false otherwise. */
                    322:        return (bzh->bzh_user_gen != atomic_load_acq_int(&bzh->bzh_kernel_gen));
                    323: }
                    324: 
                    325: static ssize_t
                    326: nextZCbuf(int eth, struct bpf_zbuf * __restrict zbuf, void * __restrict buf, size_t buflen)
                    327: {
                    328:        ssize_t rlen = 0;
                    329:        struct bpf_zbuf bz;
                    330:        struct bpf_zbuf_header *bzh;
                    331:        off_t pos = 0;
                    332: 
                    333:        bzh = (struct bpf_zbuf_header *) zbuf->bz_bufa;
                    334:        if (chkZCbuf(bzh)) {
                    335:                rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen);
                    336:                memcpy(buf + pos, zbuf->bz_bufa + sizeof(struct bpf_zbuf_header), rlen);
                    337:                ackZCbuf(bzh);
                    338:                pos += rlen;
                    339:        }
                    340:        bzh = (struct bpf_zbuf_header *) zbuf->bz_bufb;
                    341:        if (chkZCbuf(bzh)) {
                    342:                rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen);
                    343:                memcpy(buf + pos, zbuf->bz_bufb + sizeof(struct bpf_zbuf_header), rlen);
                    344:                ackZCbuf(bzh);
                    345:                pos += rlen;
                    346:        }
                    347: 
                    348:        if (!pos) {
                    349:                if ((rlen = ioctl(eth, BIOCROTZBUF, &bz)) == -1)
                    350:                        LOGERR;
                    351:        } else
                    352:                rlen = pos;
                    353:        return rlen;
                    354: }
                    355: #endif
                    356: 
                    357: /*
                    358:  * io_etherRecv() - Receive packet from bpf
                    359:  *
                    360:  * @eth = bpf handle
                    361:  * @buf = buffer
                    362:  * @buflen = buffer length
                    363:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                    364:  * return: -1 error or !=-1 readed bytes
                    365:  */
                    366: ssize_t
                    367: io_etherRecv(int eth, void * __restrict buf, size_t buflen, void * __restrict zcbuf)
                    368: {
                    369:        ssize_t rlen = 0;
1.3       misho     370:        struct bpf_hdr *h;
1.2       misho     371: 
                    372:        if (!buf || !buflen) {
                    373:                io_SetErr(EINVAL, "invalid arguments");
                    374:                return -1;
                    375:        }
                    376: 
                    377:        if (!zcbuf) {
                    378:                rlen = read(eth, buf, buflen);
1.4       misho     379:                if (rlen == -1) {
1.2       misho     380:                        LOGERR;
1.4       misho     381:                        return -1;
                    382:                }
1.2       misho     383:        } else {
1.5.4.1   misho     384: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     385:                rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen);
                    386:                if (!rlen)
                    387:                        rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen);
                    388: #else
                    389:                rlen = -1;
                    390:                io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported");
                    391: #endif
                    392:        }
                    393: 
1.3       misho     394:        h = (struct bpf_hdr*) buf;
                    395:        rlen -= h->bh_hdrlen;
                    396: 
                    397:        if (h->bh_caplen != rlen) {
                    398:                if (h->bh_caplen < rlen)
                    399:                        rlen = h->bh_caplen;
                    400:                else {
                    401:                        io_SetErr(EIO, "Captured %d bytes should be at most %d bytes", 
                    402:                                        h->bh_caplen, rlen);
                    403:                        return -1;
                    404:                }
                    405:        }
                    406: 
                    407:        memmove(buf, buf + h->bh_hdrlen, rlen);
1.2       misho     408:        return rlen;
                    409: }
1.4       misho     410: 
                    411: /*
                    412:  * io_etherFilter() - BPF filter routine
                    413:  *
                    414:  * @eth = bpf handle
                    415:  * @io = filter direction 
                    416:  *  (IO_ETHER_FILTER_PROMISC|IO_ETHER_FILTER_NOTREAD|IO_ETHER_FILTER_READ|IO_ETHER_FILTER_WRITE)
                    417:  * @insn = BPF filter instruction array
                    418:  * @insnlen = Length of BPF filter instruction array
                    419:  * return: -1 error or 0 ok
                    420:  */
                    421: int
                    422: io_etherFilter(int eth, int io, struct bpf_insn * __restrict insn, size_t insnlen)
                    423: {
                    424:        int ret = 0;
                    425:        struct bpf_program fcode = { 0 };
                    426: 
                    427:        if (io != IO_ETHER_FILTER_PROMISC && (!insn || !insnlen)) {
                    428:                io_SetErr(EINVAL, "invalid arguments");
                    429:                return -1;
                    430:        }
                    431: 
                    432:        switch (io) {
                    433:                case IO_ETHER_FILTER_PROMISC:   /* promiscuous mode */
                    434:                        ret = ioctl(eth, BIOCPROMISC, NULL);
                    435:                        break;
                    436:                case IO_ETHER_FILTER_NOTREAD:   /* read not filter */
                    437:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    438:                        fcode.bf_insns = insn;
                    439:                        ret = ioctl(eth, BIOCSETFNR, &fcode);
                    440:                        break;
                    441:                case IO_ETHER_FILTER_READ:      /* read filter */
                    442:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    443:                        fcode.bf_insns = insn;
                    444:                        ret = ioctl(eth, BIOCSETF, &fcode);
                    445:                        break;
                    446:                case IO_ETHER_FILTER_WRITE:     /* write filter */
                    447:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    448:                        fcode.bf_insns = insn;
                    449:                        ret = ioctl(eth, BIOCSETWF, &fcode);
                    450:                        break;
                    451:        }
                    452: 
                    453:        if (ret == -1)
                    454:                LOGERR;
                    455:        return ret;
                    456: }

FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>