--- libaitio/src/bpf.c	2013/06/26 22:48:53	1.2
+++ libaitio/src/bpf.c	2013/07/09 00:35:36	1.3
@@ -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 2013/07/09 00:35:36 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,19 @@ 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;
 }