--- libaitio/src/bpf.c	2013/06/26 22:48:53	1.2
+++ libaitio/src/bpf.c	2013/10/18 14:21:01	1.3.8.1
@@ -3,7 +3,7 @@
 *  by Michael Pounov <misho@elwix.org>
 *
 * $Author: misho $
-* $Id: bpf.c,v 1.2 2013/06/26 22:48:53 misho Exp $
+* $Id: bpf.c,v 1.3.8.1 2013/10/18 14:21:01 misho Exp $
 *
 **************************************************************************
 The ELWIX and AITNET software is distributed under the following
@@ -135,12 +135,14 @@ 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, int whdr, int wdlt, 
+		u_int *buflen, void **zcbuf)
 {
 	int eth = -1;
 	register int i;
@@ -229,6 +231,11 @@ io_etherOpen(const char *csIface, int flags, int whdr,
 		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);
@@ -321,6 +328,7 @@ 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");
@@ -342,5 +350,66 @@ io_etherRecv(int eth, void * __restrict buf, size_t bu
 #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;
 }