--- libaitio/src/bpf.c	2013/07/09 00:35:36	1.3
+++ libaitio/src/bpf.c	2013/10/21 21:12:42	1.4
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@elwix.org>
 *
 * $Author: misho $
-* $Id: bpf.c,v 1.3 2013/07/09 00:35:36 misho Exp $
+* $Id: bpf.c,v 1.4 2013/10/21 21:12:42 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -169,19 +169,44 @@ 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 (whdr && ioctl(eth, BIOCSHDRCMPLT, &n) == -1) {
+		LOGERR;
+		close(eth);
+		return -1;
+	}
+	if (wdlt && ioctl(eth, BIOCSDLT, &wdlt) == -1) {
+		LOGERR;
+		close(eth);
+		return -1;
+	}
+	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 : 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__
@@ -211,12 +236,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 +244,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;
 }
 
@@ -337,8 +339,10 @@ 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__
 		rlen = nextZCbuf(eth, (struct bpf_zbuf*) zcbuf, buf, buflen);
@@ -365,4 +369,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;
 }