Index: bpf.c
===================================================================
RCS file: /cvs/src/sys/net/bpf.c,v
retrieving revision 1.109
diff -u -p -r1.109 bpf.c
--- bpf.c	23 Sep 2014 00:26:11 -0000	1.109
+++ bpf.c	3 Oct 2014 08:09:40 -0000
@@ -92,7 +92,6 @@ void	bpf_ifname(struct ifnet *, struct i
 void	_bpf_mtap(caddr_t, struct mbuf *, u_int,
 	    void (*)(const void *, void *, size_t));
 void	bpf_mcopy(const void *, void *, size_t);
-void	bpf_mcopy_stripvlan(const void *, void *, size_t);
 int	bpf_movein(struct uio *, u_int, struct mbuf **,
 	    struct sockaddr *, struct bpf_insn *);
 void	bpf_attachd(struct bpf_d *, struct bpf_if *);
@@ -1160,46 +1159,6 @@ bpf_mcopy(const void *src_arg, void *dst
 }
 
 /*
- * Copy an ethernet frame from an mbuf chain into a buffer, strip the
- * vlan header bits
- */
-void
-bpf_mcopy_stripvlan(const void *src_arg, void *dst_arg, size_t len)
-{
-#if NVLAN > 0
-	const struct mbuf		*m;
-	u_int				 count, copied = 0, hdrdone = 0;
-	u_char				*dst;
-	struct ether_vlan_header	*evh;
-
-	m = src_arg;
-	dst = dst_arg;
-	evh = dst_arg;
-	while (len > 0) {
-		if (m == 0)
-			panic("bpf_mcopy_stripvlan");
-		count = min(m->m_len, len);
-		bcopy(mtod(m, caddr_t), (caddr_t)dst, count);
-		m = m->m_next;
-		dst += count;
-		len -= count;
-		copied += count;
-		if (!hdrdone && copied >= sizeof(struct ether_vlan_header) &&
-		    (ntohs(evh->evl_encap_proto) == ETHERTYPE_VLAN ||
-		    ntohs(evh->evl_encap_proto) == ETHERTYPE_QINQ)) {
-			/* move up by 4 bytes, overwrite encap_proto + tag */
-			memmove(&evh->evl_encap_proto, &evh->evl_proto, copied -
-			    offsetof(struct ether_vlan_header, evl_proto));
-			dst -= (offsetof(struct ether_vlan_header, evl_proto) -
-			    offsetof(struct ether_vlan_header,
-			    evl_encap_proto)); /* long expression for "4" */
-			hdrdone = 1;
-		}
-	}
-#endif
-}
-
-/*
  * like bpf_mtap, but copy fn can be given. used by various bpf_mtap*
  */
 void
@@ -1249,13 +1208,6 @@ void
 bpf_mtap(caddr_t arg, struct mbuf *m, u_int direction)
 {
 	_bpf_mtap(arg, m, direction, NULL);
-}
-
-/* like bpf_mtap, but strip the vlan header, leave regular ethernet hdr */
-void
-bpf_mtap_stripvlan(caddr_t arg, struct mbuf *m, u_int direction)
-{
-	_bpf_mtap(arg, m, direction, bpf_mcopy_stripvlan);
 }
 
 /*
Index: bpf.h
===================================================================
RCS file: /cvs/src/sys/net/bpf.h,v
retrieving revision 1.46
diff -u -p -r1.46 bpf.h
--- bpf.h	10 Jul 2014 09:46:29 -0000	1.46
+++ bpf.h	3 Oct 2014 08:09:40 -0000
@@ -272,7 +272,6 @@ struct bpf_dltlist {
 int	 bpf_validate(struct bpf_insn *, int);
 int	 bpf_tap(caddr_t, u_char *, u_int, u_int);
 void	 bpf_mtap(caddr_t, struct mbuf *, u_int);
-void	 bpf_mtap_stripvlan(caddr_t, struct mbuf *, u_int);
 void	 bpf_mtap_hdr(caddr_t, caddr_t, u_int, struct mbuf *, u_int,
 	    void (*)(const void *, void *, size_t));
 void	 bpf_mtap_af(caddr_t, u_int32_t, struct mbuf *, u_int);
Index: if_vlan.c
===================================================================
RCS file: /cvs/src/sys/net/if_vlan.c,v
retrieving revision 1.108
diff -u -p -r1.108 if_vlan.c
--- if_vlan.c	12 Jul 2014 18:44:22 -0000	1.108
+++ if_vlan.c	3 Oct 2014 08:09:40 -0000
@@ -212,9 +212,41 @@ vlan_start(struct ifnet *ifp)
 		}
 
 #if NBPFILTER > 0
-		if (ifp->if_bpf)
-			bpf_mtap_stripvlan(ifp->if_bpf, m, BPF_DIRECTION_OUT);
-#endif
+	if (ifp->if_bpf) {
+		struct mbuf *m0, ma;
+		struct m_hdr mh;
+
+                if ((p->if_capabilities & IFCAP_VLAN_HWTAGGING) &&
+                    (ifv->ifv_type == ETHERTYPE_VLAN))
+			m0 = m;
+                else {
+			u_int off;
+
+			/* find where the inside ethertype sits */
+			m0 = m_getptr(m, offsetof(struct ether_vlan_header,
+			    evl_proto), &off);
+			KASSERT(m0 != NULL);
+
+			/* point to the ethertype after the vlan sub header */
+			mh.mh_flags = 0;
+			mh.mh_data = mtod(m0, caddr_t) + off;
+			mh.mh_len = m0->m_len - off;
+			mh.mh_next = m0->m_next;
+
+			/* copy the ether addresses */
+			ma.m_flags = 0;
+			ma.m_data = ma.m_pktdat;
+			ma.m_len = offsetof(struct ether_header, ether_type);
+			ma.m_next = (struct mbuf *)&mh;
+
+			m_copydata(m, 0, ma.m_len, ma.m_data);
+
+			m0 = &ma;
+		}
+
+		bpf_mtap(ifp->if_bpf, m0, BPF_DIRECTION_OUT);
+	}
+#endif /* NBPFILTER > 0 */
 
 		/*
 		 * Send it, precisely as ether_output() would have.