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

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.7     ! misho       6: * $Id: bpf.c,v 1.6.6.3 2014/11/17 22:36:07 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.6       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_etherClose() - Close BPF interface
                     51:  *
                     52:  * @eth = bpf handle
                     53:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                     54:  * return: none
                     55:  */
                     56: void
                     57: io_etherClose(int eth, void **zcbuf)
                     58: {
1.6       misho      59: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho      60:        struct bpf_zbuf *zbuf = NULL;
                     61: #endif
                     62: 
                     63:        if (eth > STDERR_FILENO)
                     64:                close(eth);
                     65: 
                     66:        if (zcbuf && *zcbuf) {
1.6       misho      67: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho      68:                zbuf = *zcbuf;
                     69:                munmap(zbuf->bz_bufb, zbuf->bz_buflen);
                     70:                munmap(zbuf->bz_bufa, zbuf->bz_buflen);
                     71:                e_free(*zcbuf);
                     72:                *zcbuf = NULL;
                     73: #endif
                     74:        }
                     75: }
                     76: 
1.6       misho      77: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho      78: static inline struct bpf_zbuf *
                     79: allocZCbuf(u_int len)
                     80: {
                     81:        struct bpf_zbuf *zbuf = NULL;
                     82: 
                     83:        zbuf = e_malloc(sizeof(struct bpf_zbuf));
                     84:        if (!zbuf) {
                     85:                io_SetErr(elwix_GetErrno(), "%s", elwix_GetError());
                     86:                return NULL;
                     87:        } else
                     88:                memset(zbuf, 0, sizeof(struct bpf_zbuf));
                     89: 
                     90:        zbuf->bz_buflen = roundup(len, getpagesize());
                     91:        zbuf->bz_bufa = mmap(NULL, zbuf->bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
                     92:        if (zbuf->bz_bufa == MAP_FAILED) {
                     93:                LOGERR;
                     94:                e_free(zbuf);
                     95:                return NULL;
                     96:        } else
                     97:                memset(zbuf->bz_bufa, 0, zbuf->bz_buflen);
                     98:        zbuf->bz_bufb = mmap(NULL, zbuf->bz_buflen, PROT_READ | PROT_WRITE, MAP_ANON, -1, 0);
                     99:        if (zbuf->bz_bufb == MAP_FAILED) {
                    100:                LOGERR;
                    101:                munmap(zbuf->bz_bufa, zbuf->bz_buflen);
                    102:                e_free(zbuf);
                    103:                return NULL;
                    104:        } else
                    105:                memset(zbuf->bz_bufb, 0, zbuf->bz_buflen);
                    106: 
                    107:        return zbuf;
                    108: }
                    109: #endif
                    110: 
                    111: /*
                    112:  * io_etherOpen() - Open BPF interface to device
                    113:  *
                    114:  * @csIface = interface name
                    115:  * @flags = open flags
                    116:  * @whdr = with complete headers
1.3       misho     117:  * @wdlt = with data link type
1.2       misho     118:  * @buflen = buffer length
                    119:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                    120:  * return: -1 error or >-1 bpf handle
                    121:  */
                    122: int
1.6       misho     123: io_etherOpen(const char *csIface, int flags, u_int whdr, u_int wdlt, 
1.3       misho     124:                u_int *buflen, void **zcbuf)
1.2       misho     125: {
                    126:        int eth = -1;
                    127:        register int i;
                    128:        char szStr[STRSIZ];
                    129:        struct ifreq ifr;
                    130:        u_int n = 1;
                    131: 
1.6       misho     132: #if !defined(__FreeBSD__) || !defined(ZCBUF_ENABLE)
1.2       misho     133:        if (zcbuf) {
                    134:                io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported");
                    135:                return -1;
                    136:        }
                    137: #endif
                    138: 
                    139:        for (i = 0; i < BPF_DEV_MAX; i++) {
                    140:                memset(szStr, 0, sizeof szStr);
                    141:                snprintf(szStr, sizeof szStr, "/dev/bpf%d", i);
                    142:                eth = open(szStr, flags);
                    143:                if (eth > STDERR_FILENO)
                    144:                        break;
                    145:        }
                    146:        if (eth < 3) {
                    147:                LOGERR;
                    148:                return -1;
                    149:        }
                    150: 
1.4       misho     151:        if (csIface)
                    152:                strlcpy(szStr, csIface, sizeof szStr);
1.7     ! misho     153:        else if (e_get1stiface(szStr, sizeof szStr) == -1) {
1.4       misho     154:                close(eth);
                    155:                return -1;
                    156:        }
                    157: 
                    158:        n = 1;
                    159:        if (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) {
                    160:                LOGERR;
                    161:                close(eth);
                    162:                return -1;
                    163:        }
                    164:        if (ioctl(eth, BIOCIMMEDIATE, &n) == -1) {
                    165:                LOGERR;
                    166:                close(eth);
                    167:                return -1;
                    168:        }
                    169: 
1.2       misho     170:        if (!zcbuf) {
                    171:                if (ioctl(eth, BIOCGBLEN, &n) == -1) {
                    172:                        LOGERR;
                    173:                        close(eth);
                    174:                        return -1;
                    175:                } else
1.4       misho     176:                        n = (buflen && *buflen) ? *buflen : getpagesize();
                    177: 
1.2       misho     178:                if (ioctl(eth, BIOCSBLEN, &n) == -1) {
                    179:                        LOGERR;
                    180:                        close(eth);
                    181:                        return -1;
                    182:                }
1.4       misho     183:                if (buflen)
1.2       misho     184:                        *buflen = n;
                    185:        } else {
1.6       misho     186: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     187:                n = BPF_BUFMODE_ZBUF;
                    188:                if (ioctl(eth, BIOCSETBUFMODE, &n) == -1) {
                    189:                        LOGERR;
                    190:                        close(eth);
                    191:                        return -1;
                    192:                }
                    193:                if (ioctl(eth, BIOCGETZMAX, &n) == -1) {
                    194:                        LOGERR;
                    195:                        close(eth);
                    196:                        return -1;
                    197:                } else
                    198:                        n = (buflen && *buflen) ? *buflen : n;
                    199:                if (!(*zcbuf = allocZCbuf(n))) {
                    200:                        close(eth);
                    201:                        return -1;
                    202:                }
                    203:                if (ioctl(eth, BIOCSETZBUF, *zcbuf) == -1) {
                    204:                        LOGERR;
                    205:                        io_etherClose(eth, zcbuf);
                    206:                        return -1;
                    207:                }
                    208:                if (buflen && *buflen)
                    209:                        *buflen = n;
                    210: #endif
                    211:        }
                    212: 
                    213:        memset(&ifr, 0, sizeof ifr);
                    214:        strlcpy(ifr.ifr_name, szStr, sizeof ifr.ifr_name);
                    215:        if (ioctl(eth, BIOCSETIF, &ifr) == -1) {
                    216:                LOGERR;
                    217:                io_etherClose(eth, zcbuf);
                    218:                return -1;
                    219:        }
                    220: 
1.6       misho     221:        n = wdlt;
                    222:        if (wdlt && ioctl(eth, BIOCSDLT, &n) == -1) {
                    223:                LOGERR;
                    224:                close(eth);
                    225:                return -1;
                    226:        }
                    227: 
1.2       misho     228:        return eth;
                    229: }
                    230: 
                    231: /*
                    232:  * io_etherSend() - Send packet to bpf
                    233:  *
                    234:  * @eth = bpf handle
                    235:  * @buf = buffer
                    236:  * @buflen = buffer length
                    237:  * return: -1 error or !=-1 written bytes
                    238:  */
                    239: ssize_t
                    240: io_etherSend(int eth, const void *buf, size_t buflen)
                    241: {
                    242:        ssize_t wlen = 0;
                    243: 
                    244:        if (!buf || !buflen) {
                    245:                io_SetErr(EINVAL, "invalid arguments");
                    246:                return -1;
                    247:        }
                    248: 
                    249:        wlen = write(eth, buf, buflen);
                    250:        if (wlen == -1)
                    251:                LOGERR;
                    252:        return wlen;
                    253: }
                    254: 
1.6       misho     255: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     256: static inline void
                    257: ackZCbuf(struct bpf_zbuf_header *bzh)
                    258: {
                    259:        atomic_store_rel_int(&bzh->bzh_user_gen, bzh->bzh_kernel_gen);
                    260: }
                    261: 
                    262: static inline int
                    263: chkZCbuf(struct bpf_zbuf_header *bzh)
                    264: {
                    265:        /* return true if userspace owns buffer, and false otherwise. */
                    266:        return (bzh->bzh_user_gen != atomic_load_acq_int(&bzh->bzh_kernel_gen));
                    267: }
                    268: 
                    269: static ssize_t
                    270: nextZCbuf(int eth, struct bpf_zbuf * __restrict zbuf, void * __restrict buf, size_t buflen)
                    271: {
                    272:        ssize_t rlen = 0;
                    273:        struct bpf_zbuf bz;
                    274:        struct bpf_zbuf_header *bzh;
                    275:        off_t pos = 0;
                    276: 
                    277:        bzh = (struct bpf_zbuf_header *) zbuf->bz_bufa;
                    278:        if (chkZCbuf(bzh)) {
                    279:                rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen);
                    280:                memcpy(buf + pos, zbuf->bz_bufa + sizeof(struct bpf_zbuf_header), rlen);
                    281:                ackZCbuf(bzh);
                    282:                pos += rlen;
                    283:        }
                    284:        bzh = (struct bpf_zbuf_header *) zbuf->bz_bufb;
                    285:        if (chkZCbuf(bzh)) {
                    286:                rlen = MIN(atomic_load_acq_int(&bzh->bzh_kernel_len), buflen);
                    287:                memcpy(buf + pos, zbuf->bz_bufb + sizeof(struct bpf_zbuf_header), rlen);
                    288:                ackZCbuf(bzh);
                    289:                pos += rlen;
                    290:        }
                    291: 
                    292:        if (!pos) {
                    293:                if ((rlen = ioctl(eth, BIOCROTZBUF, &bz)) == -1)
                    294:                        LOGERR;
                    295:        } else
                    296:                rlen = pos;
                    297:        return rlen;
                    298: }
                    299: #endif
                    300: 
                    301: /*
                    302:  * io_etherRecv() - Receive packet from bpf
                    303:  *
                    304:  * @eth = bpf handle
                    305:  * @buf = buffer
                    306:  * @buflen = buffer length
                    307:  * @zcbuf = zero copy buffer, if BPF supports it and isn't NULL
                    308:  * return: -1 error or !=-1 readed bytes
                    309:  */
                    310: ssize_t
                    311: io_etherRecv(int eth, void * __restrict buf, size_t buflen, void * __restrict zcbuf)
                    312: {
                    313:        ssize_t rlen = 0;
1.3       misho     314:        struct bpf_hdr *h;
1.2       misho     315: 
                    316:        if (!buf || !buflen) {
                    317:                io_SetErr(EINVAL, "invalid arguments");
                    318:                return -1;
                    319:        }
                    320: 
                    321:        if (!zcbuf) {
                    322:                rlen = read(eth, buf, buflen);
1.4       misho     323:                if (rlen == -1) {
1.2       misho     324:                        LOGERR;
1.4       misho     325:                        return -1;
                    326:                }
1.2       misho     327:        } else {
1.6       misho     328: #if defined(__FreeBSD__) && defined(ZCBUF_ENABLE)
1.2       misho     329:                rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen);
                    330:                if (!rlen)
                    331:                        rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen);
                    332: #else
                    333:                rlen = -1;
                    334:                io_SetErr(ENOTSUP, "bpf zero copy buffer mode is not supported");
                    335: #endif
                    336:        }
                    337: 
1.3       misho     338:        h = (struct bpf_hdr*) buf;
                    339:        rlen -= h->bh_hdrlen;
                    340: 
                    341:        if (h->bh_caplen != rlen) {
                    342:                if (h->bh_caplen < rlen)
                    343:                        rlen = h->bh_caplen;
                    344:                else {
                    345:                        io_SetErr(EIO, "Captured %d bytes should be at most %d bytes", 
                    346:                                        h->bh_caplen, rlen);
                    347:                        return -1;
                    348:                }
                    349:        }
                    350: 
                    351:        memmove(buf, buf + h->bh_hdrlen, rlen);
1.2       misho     352:        return rlen;
                    353: }
1.4       misho     354: 
                    355: /*
                    356:  * io_etherFilter() - BPF filter routine
                    357:  *
                    358:  * @eth = bpf handle
                    359:  * @io = filter direction 
                    360:  *  (IO_ETHER_FILTER_PROMISC|IO_ETHER_FILTER_NOTREAD|IO_ETHER_FILTER_READ|IO_ETHER_FILTER_WRITE)
                    361:  * @insn = BPF filter instruction array
                    362:  * @insnlen = Length of BPF filter instruction array
                    363:  * return: -1 error or 0 ok
                    364:  */
                    365: int
                    366: io_etherFilter(int eth, int io, struct bpf_insn * __restrict insn, size_t insnlen)
                    367: {
                    368:        int ret = 0;
                    369:        struct bpf_program fcode = { 0 };
                    370: 
                    371:        if (io != IO_ETHER_FILTER_PROMISC && (!insn || !insnlen)) {
                    372:                io_SetErr(EINVAL, "invalid arguments");
                    373:                return -1;
                    374:        }
                    375: 
                    376:        switch (io) {
                    377:                case IO_ETHER_FILTER_PROMISC:   /* promiscuous mode */
                    378:                        ret = ioctl(eth, BIOCPROMISC, NULL);
                    379:                        break;
                    380:                case IO_ETHER_FILTER_NOTREAD:   /* read not filter */
                    381:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    382:                        fcode.bf_insns = insn;
                    383:                        ret = ioctl(eth, BIOCSETFNR, &fcode);
                    384:                        break;
                    385:                case IO_ETHER_FILTER_READ:      /* read filter */
                    386:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    387:                        fcode.bf_insns = insn;
                    388:                        ret = ioctl(eth, BIOCSETF, &fcode);
                    389:                        break;
                    390:                case IO_ETHER_FILTER_WRITE:     /* write filter */
                    391:                        fcode.bf_len = insnlen / sizeof(struct bpf_insn);
                    392:                        fcode.bf_insns = insn;
                    393:                        ret = ioctl(eth, BIOCSETWF, &fcode);
                    394:                        break;
                    395:        }
                    396: 
                    397:        if (ret == -1)
                    398:                LOGERR;
                    399:        return ret;
                    400: }

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