From e6b5847cec36d34d78ec0337eee5cbf5cfbd80e5 Mon Sep 17 00:00:00 2001
From: Sepherosa Ziehau <sephe@dragonflybsd.org>
Date: Fri, 16 May 2008 13:19:12 +0000
Subject: [PATCH] Unify vlan_input() and vlan_input_tag(): - For device drivers
 that support hardware vlan tag extraction, mbuf's   M_VLANTAG is turned on
 and vlan tag is saved in mbuf.m_pkthdr.ether_vlantag - At the very beginning
 of ether_input_chain(), if the packet's ether type is   vlan and hardware
 does not extract vlan tag, vlan_ether_decap() is called to   do software vlan
 tag extraction. - Instead of BPF_MTAP(), ETHER_BPF_MTAP() is used in
 ether_input_chain() to   deliver possible vlan tagging information to the bpf
 listeners. - Ether header is restored before calling vlan_input(), so under
 most cases,   extra ether header copy is avoided.  vlan_input() does nothing
 more than   finding vlan interface and looping back the packet to
 ether_input_chain()   with vlan interface as input interface.

Ideas-from: FreeBSD
---
 sys/dev/netif/bce/if_bce.c   |  12 ++--
 sys/dev/netif/bge/if_bge.c   |   8 +--
 sys/dev/netif/em/if_em.c     |  17 +++---
 sys/dev/netif/nge/if_nge.c   |  12 ++--
 sys/dev/netif/re/if_re.c     |  10 ++--
 sys/dev/netif/stge/if_stge.c |  10 ++--
 sys/dev/netif/ti/if_ti.c     |  15 ++---
 sys/dev/netif/txp/if_txp.c   |  11 ++--
 sys/dev/netif/vge/if_vge.c   |  12 ++--
 sys/net/ethernet.h           |  17 +-----
 sys/net/if_ethersubr.c       |  51 +++++++++++-----
 sys/net/vlan/if_vlan.c       | 113 ++++++++++-------------------------
 sys/net/vlan/if_vlan_ether.c |  27 ++++++++-
 sys/net/vlan/if_vlan_ether.h |   3 +-
 14 files changed, 154 insertions(+), 164 deletions(-)

diff --git a/sys/dev/netif/bce/if_bce.c b/sys/dev/netif/bce/if_bce.c
index 81f34538c6..f0560c1dfb 100644
--- a/sys/dev/netif/bce/if_bce.c
+++ b/sys/dev/netif/bce/if_bce.c
@@ -28,7 +28,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/dev/bce/if_bce.c,v 1.31 2007/05/16 23:34:11 davidch Exp $
- * $DragonFly: src/sys/dev/netif/bce/if_bce.c,v 1.4 2008/05/14 11:59:18 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/bce/if_bce.c,v 1.5 2008/05/16 13:19:11 sephe Exp $
  */
 
 /*
@@ -3925,10 +3925,12 @@ bce_rx_int_next_rx:
 			DBPRINT(sc, BCE_VERBOSE_RECV,
 				"%s(): Passing received frame up.\n", __func__);
 
-			if (status & L2_FHDR_STATUS_L2_VLAN_TAG)
-				VLAN_INPUT_TAG(m, l2fhdr->l2_fhdr_vlan_tag);
-			else
-				ifp->if_input(ifp, m);
+			if (status & L2_FHDR_STATUS_L2_VLAN_TAG) {
+				m->m_flags |= M_VLANTAG;
+				m->m_pkthdr.ether_vlantag =
+					l2fhdr->l2_fhdr_vlan_tag;
+			}
+			ifp->if_input(ifp, m);
 
 			DBRUNIF(1, sc->rx_mbuf_alloc--);
 		}
diff --git a/sys/dev/netif/bge/if_bge.c b/sys/dev/netif/bge/if_bge.c
index a577138ebe..a6da7d2e79 100644
--- a/sys/dev/netif/bge/if_bge.c
+++ b/sys/dev/netif/bge/if_bge.c
@@ -31,7 +31,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/dev/bge/if_bge.c,v 1.3.2.39 2005/07/03 03:41:18 silby Exp $
- * $DragonFly: src/sys/dev/netif/bge/if_bge.c,v 1.91 2008/05/14 11:59:18 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/bge/if_bge.c,v 1.92 2008/05/16 13:19:11 sephe Exp $
  *
  */
 
@@ -2303,11 +2303,11 @@ bge_rxeof(struct bge_softc *sc)
 		 * to vlan_input() instead of ether_input().
 		 */
 		if (have_tag) {
-			VLAN_INPUT_TAG(m, vlan_tag);
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag = vlan_tag;
 			have_tag = vlan_tag = 0;
-		} else {
-			ifp->if_input(ifp, m);
 		}
+		ifp->if_input(ifp, m);
 	}
 
 	if (stdcnt > 0) {
diff --git a/sys/dev/netif/em/if_em.c b/sys/dev/netif/em/if_em.c
index 9aebb86f69..e1a4c2e32c 100644
--- a/sys/dev/netif/em/if_em.c
+++ b/sys/dev/netif/em/if_em.c
@@ -64,7 +64,7 @@
  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  * 
- * $DragonFly: src/sys/dev/netif/em/if_em.c,v 1.71 2008/05/14 11:59:19 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/em/if_em.c,v 1.72 2008/05/16 13:19:11 sephe Exp $
  * $FreeBSD$
  */
 /*
@@ -3114,17 +3114,16 @@ em_rxeof(struct adapter *adapter, int count)
 				em_receive_checksum(adapter, current_desc,
 						    adapter->fmp);
 				if (current_desc->status & E1000_RXD_STAT_VP) {
-					VLAN_INPUT_TAG(adapter->fmp,
-						       (current_desc->special & 
-							E1000_RXD_SPC_VLAN_MASK));
-				} else {
+					adapter->fmp->m_flags |= M_VLANTAG;
+					adapter->fmp->m_pkthdr.ether_vlantag =
+						(current_desc->special &
+						 E1000_RXD_SPC_VLAN_MASK);
+				}
 #ifdef ETHER_INPUT_CHAIN
-					ether_input_chain(ifp, adapter->fmp,
-							  chain);
+				ether_input_chain(ifp, adapter->fmp, chain);
 #else
-					ifp->if_input(ifp, adapter->fmp);
+				ifp->if_input(ifp, adapter->fmp);
 #endif
-				}
 				adapter->fmp = NULL;
 				adapter->lmp = NULL;
 			}
diff --git a/sys/dev/netif/nge/if_nge.c b/sys/dev/netif/nge/if_nge.c
index df0f62e03e..4ba86bc238 100644
--- a/sys/dev/netif/nge/if_nge.c
+++ b/sys/dev/netif/nge/if_nge.c
@@ -31,7 +31,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/dev/nge/if_nge.c,v 1.13.2.13 2003/02/05 22:03:57 mbr Exp $
- * $DragonFly: src/sys/dev/netif/nge/if_nge.c,v 1.47 2008/05/14 11:59:21 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/nge/if_nge.c,v 1.48 2008/05/16 13:19:12 sephe Exp $
  */
 
 /*
@@ -1287,10 +1287,12 @@ nge_rxeof(struct nge_softc *sc)
 		 * If we received a packet with a vlan tag, pass it
 		 * to vlan_input() instead of ether_input().
 		 */
-		if (extsts & NGE_RXEXTSTS_VLANPKT)
-			VLAN_INPUT_TAG(m, extsts & NGE_RXEXTSTS_VTCI);
-		else
-			ifp->if_input(ifp, m);
+		if (extsts & NGE_RXEXTSTS_VLANPKT) {
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag =
+				(extsts & NGE_RXEXTSTS_VTCI);
+		}
+		ifp->if_input(ifp, m);
 	}
 
 	sc->nge_cdata.nge_rx_prod = i;
diff --git a/sys/dev/netif/re/if_re.c b/sys/dev/netif/re/if_re.c
index 78c89794be..510193f942 100644
--- a/sys/dev/netif/re/if_re.c
+++ b/sys/dev/netif/re/if_re.c
@@ -33,7 +33,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/dev/re/if_re.c,v 1.25 2004/06/09 14:34:01 naddy Exp $
- * $DragonFly: src/sys/dev/netif/re/if_re.c,v 1.42 2008/05/14 11:59:21 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/re/if_re.c,v 1.43 2008/05/16 13:19:12 sephe Exp $
  */
 
 /*
@@ -1612,11 +1612,11 @@ re_rxeof(struct re_softc *sc)
 		}
 
 		if (rxvlan & RE_RDESC_VLANCTL_TAG) {
-			VLAN_INPUT_TAG(m,
-			   be16toh((rxvlan & RE_RDESC_VLANCTL_DATA)));
-		} else {
-			ifp->if_input(ifp, m);
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag =
+				be16toh((rxvlan & RE_RDESC_VLANCTL_DATA));
 		}
+		ifp->if_input(ifp, m);
 	}
 
 	/* Flush the RX DMA ring */
diff --git a/sys/dev/netif/stge/if_stge.c b/sys/dev/netif/stge/if_stge.c
index e788306a97..bc9509ebff 100644
--- a/sys/dev/netif/stge/if_stge.c
+++ b/sys/dev/netif/stge/if_stge.c
@@ -1,6 +1,6 @@
 /*	$NetBSD: if_stge.c,v 1.32 2005/12/11 12:22:49 christos Exp $	*/
 /*	$FreeBSD: src/sys/dev/stge/if_stge.c,v 1.2 2006/08/12 01:21:36 yongari Exp $	*/
-/*	$DragonFly: src/sys/dev/netif/stge/if_stge.c,v 1.5 2008/05/14 11:59:22 sephe Exp $	*/
+/*	$DragonFly: src/sys/dev/netif/stge/if_stge.c,v 1.6 2008/05/16 13:19:12 sephe Exp $	*/
 
 /*-
  * Copyright (c) 2001 The NetBSD Foundation, Inc.
@@ -1801,11 +1801,11 @@ stge_rxeof(struct stge_softc *sc, int count)
 			/* Check for VLAN tagged packets. */
 			if ((status & RFD_VLANDetected) != 0 &&
 			    (ifp->if_capenable & IFCAP_VLAN_HWTAGGING) != 0) {
-				VLAN_INPUT_TAG(m, RFD_TCI(status64));
-			} else {
-				/* Pass it on. */
-				ifp->if_input(ifp, m);
+				m->m_flags |= M_VLANTAG;
+				m->m_pkthdr.ether_vlantag = RFD_TCI(status64);
 			}
+			/* Pass it on. */
+			ifp->if_input(ifp, m);
 
 			STGE_RXCHAIN_RESET(sc);
 		}
diff --git a/sys/dev/netif/ti/if_ti.c b/sys/dev/netif/ti/if_ti.c
index 5d46cc8bfd..35a3b8b172 100644
--- a/sys/dev/netif/ti/if_ti.c
+++ b/sys/dev/netif/ti/if_ti.c
@@ -30,7 +30,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/pci/if_ti.c,v 1.25.2.14 2002/02/15 04:20:20 silby Exp $
- * $DragonFly: src/sys/dev/netif/ti/if_ti.c,v 1.51 2008/05/14 11:59:22 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/ti/if_ti.c,v 1.52 2008/05/16 13:19:12 sephe Exp $
  */
 
 /*
@@ -1702,14 +1702,11 @@ ti_rxeof(struct ti_softc *sc)
 			m->m_pkthdr.csum_data = cur_rx->ti_tcp_udp_cksum;
 		}
 
-		/*
-		 * If we received a packet with a vlan tag, pass it
-		 * to vlan_input() instead of ether_input().
-		 */
-		if (have_tag)
-			VLAN_INPUT_TAG(m, vlan_tag);
-		else
-			ifp->if_input(ifp, m);
+		if (have_tag) {
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag = vlan_tag;
+		}
+		ifp->if_input(ifp, m);
 	}
 
 	/* Only necessary on the Tigon 1. */
diff --git a/sys/dev/netif/txp/if_txp.c b/sys/dev/netif/txp/if_txp.c
index edb507396d..262931758d 100644
--- a/sys/dev/netif/txp/if_txp.c
+++ b/sys/dev/netif/txp/if_txp.c
@@ -1,6 +1,6 @@
 /*	$OpenBSD: if_txp.c,v 1.48 2001/06/27 06:34:50 kjc Exp $	*/
 /*	$FreeBSD: src/sys/dev/txp/if_txp.c,v 1.4.2.4 2001/12/14 19:50:43 jlemon Exp $ */
-/*	$DragonFly: src/sys/dev/netif/txp/if_txp.c,v 1.48 2008/05/14 11:59:22 sephe Exp $ */
+/*	$DragonFly: src/sys/dev/netif/txp/if_txp.c,v 1.49 2008/05/16 13:19:12 sephe Exp $ */
 
 /*
  * Copyright (c) 2001
@@ -724,10 +724,11 @@ txp_rx_reclaim(struct txp_softc *sc, struct txp_rx_ring *r)
 			m->m_pkthdr.csum_data = 0xffff;
 		}
 
-		if (rxd->rx_stat & RX_STAT_VLAN)
-			VLAN_INPUT_TAG(m, htons(rxd->rx_vlan >> 16));
-		else
-			ifp->if_input(ifp, m);
+		if (rxd->rx_stat & RX_STAT_VLAN) {
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag = htons(rxd->rx_vlan >> 16);
+		}
+		ifp->if_input(ifp, m);
 
 next:
 
diff --git a/sys/dev/netif/vge/if_vge.c b/sys/dev/netif/vge/if_vge.c
index 8662788797..53d21738ed 100644
--- a/sys/dev/netif/vge/if_vge.c
+++ b/sys/dev/netif/vge/if_vge.c
@@ -30,7 +30,7 @@
  * THE POSSIBILITY OF SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/dev/vge/if_vge.c,v 1.24 2006/02/14 12:44:56 glebius Exp $
- * $DragonFly: src/sys/dev/netif/vge/if_vge.c,v 1.8 2008/05/14 11:59:22 sephe Exp $
+ * $DragonFly: src/sys/dev/netif/vge/if_vge.c,v 1.9 2008/05/16 13:19:12 sephe Exp $
  */
 
 /*
@@ -1386,10 +1386,12 @@ vge_rxeof(struct vge_softc *sc, int count)
 			}
 		}
 
-		if (rxstat & VGE_RDSTS_VTAG)
-			VLAN_INPUT_TAG(m, ntohs((rxctl & VGE_RDCTL_VLANID)));
-		else
-			ifp->if_input(ifp, m);
+		if (rxstat & VGE_RDSTS_VTAG) {
+			m->m_flags |= M_VLANTAG;
+			m->m_pkthdr.ether_vlantag =
+				ntohs((rxctl & VGE_RDCTL_VLANID));
+		}
+		ifp->if_input(ifp, m);
 
 		lim++;
 		if (lim == VGE_RX_DESC_CNT)
diff --git a/sys/net/ethernet.h b/sys/net/ethernet.h
index ac5f341245..529cfee424 100644
--- a/sys/net/ethernet.h
+++ b/sys/net/ethernet.h
@@ -2,7 +2,7 @@
  * Fundamental constants relating to ethernet.
  *
  * $FreeBSD: src/sys/net/ethernet.h,v 1.12.2.8 2002/12/01 14:03:09 sobomax Exp $
- * $DragonFly: src/sys/net/ethernet.h,v 1.17 2008/03/19 14:46:03 sephe Exp $
+ * $DragonFly: src/sys/net/ethernet.h,v 1.18 2008/05/16 13:19:12 sephe Exp $
  *
  */
 
@@ -358,6 +358,7 @@ extern const uint8_t	etherbroadcastaddr[ETHER_ADDR_LEN];
 
 struct ifnet;
 struct mbuf;
+struct mbuf_chain;
 
 extern	void (*ng_ether_input_p)(struct ifnet *ifp, struct mbuf **mp);
 extern	void (*ng_ether_input_orphan_p)(struct ifnet *ifp,
@@ -366,19 +367,7 @@ extern	int  (*ng_ether_output_p)(struct ifnet *ifp, struct mbuf **mp);
 extern	void (*ng_ether_attach_p)(struct ifnet *ifp);
 extern	void (*ng_ether_detach_p)(struct ifnet *ifp);
 
-extern	int (*vlan_input_p)(const struct ether_header *eh, struct mbuf *m);
-extern	int (*vlan_input_tag_p)(struct mbuf *m, uint16_t t);
-
-#define VLAN_INPUT_TAG(m, t) do {			\
-	/* XXX: lock */					\
-	if (vlan_input_tag_p != NULL)			\
-		(*vlan_input_tag_p)(m, t);		\
-	else {						\
-		(m)->m_pkthdr.rcvif->if_noproto++;	\
-		m_freem(m);				\
-        }						\
-	/* XXX: unlock */				\
-} while (0)
+extern	int (*vlan_input_p)(struct mbuf *m, struct mbuf_chain *chain);
 
 /*
  * The ETHER_BPF_MTAP macro should be used by drivers which support hardware
diff --git a/sys/net/if_ethersubr.c b/sys/net/if_ethersubr.c
index 0a9dd5a2ac..9443f6fe15 100644
--- a/sys/net/if_ethersubr.c
+++ b/sys/net/if_ethersubr.c
@@ -32,7 +32,7 @@
  *
  *	@(#)if_ethersubr.c	8.1 (Berkeley) 6/10/93
  * $FreeBSD: src/sys/net/if_ethersubr.c,v 1.70.2.33 2003/04/28 15:45:53 archie Exp $
- * $DragonFly: src/sys/net/if_ethersubr.c,v 1.59 2008/05/14 11:59:23 sephe Exp $
+ * $DragonFly: src/sys/net/if_ethersubr.c,v 1.60 2008/05/16 13:19:12 sephe Exp $
  */
 
 #include "opt_atalk.h"
@@ -65,6 +65,7 @@
 #include <net/ifq_var.h>
 #include <net/bpf.h>
 #include <net/ethernet.h>
+#include <net/vlan/if_vlan_ether.h>
 
 #if defined(INET) || defined(INET6)
 #include <netinet/in.h>
@@ -117,8 +118,7 @@ int	(*ng_ether_output_p)(struct ifnet *ifp, struct mbuf **mp);
 void	(*ng_ether_attach_p)(struct ifnet *ifp);
 void	(*ng_ether_detach_p)(struct ifnet *ifp);
 
-int	(*vlan_input_p)(const struct ether_header *eh, struct mbuf *m);
-int	(*vlan_input_tag_p)(struct mbuf *m, uint16_t t);
+int	(*vlan_input_p)(struct mbuf *, struct mbuf_chain *);
 
 static int ether_output(struct ifnet *, struct mbuf *, struct sockaddr *,
 			struct rtentry *);
@@ -555,6 +555,17 @@ ether_input_chain(struct ifnet *ifp, struct mbuf *m, struct mbuf_chain *chain)
 	}
 	eh = mtod(m, struct ether_header *);
 
+	if (ntohs(eh->ether_type) == ETHERTYPE_VLAN &&
+	    (m->m_flags & M_VLANTAG) == 0) {
+		/*
+		 * Extract vlan tag if hardware does not do it for us
+		 */
+		vlan_ether_decap(&m);
+		if (m == NULL)
+			return;
+		eh = mtod(m, struct ether_header *);
+	}
+
 	m->m_pkthdr.rcvif = ifp;
 
 	if (ETHER_IS_MULTICAST(eh->ether_dhost)) {
@@ -566,7 +577,7 @@ ether_input_chain(struct ifnet *ifp, struct mbuf *m, struct mbuf_chain *chain)
 		ifp->if_imcasts++;
 	}
 
-	BPF_MTAP(ifp, m);
+	ETHER_BPF_MTAP(ifp, m);
 
 	ifp->if_ibytes += m->m_pkthdr.len;
 
@@ -700,10 +711,31 @@ post_stats:
 			return;
 		}
 	}
-	eh = NULL; /* catch any further usage */
 
 	ether_type = ntohs(save_eh.ether_type);
 
+	if (m->m_flags & M_VLANTAG) {
+		if (ether_type == ETHERTYPE_VLAN) {
+			/*
+			 * To prevent possible dangerous recursion,
+			 * we don't do vlan-in-vlan
+			 */
+			m->m_pkthdr.rcvif->if_noproto++;
+			m_freem(m);
+		}
+
+		if (vlan_input_p != NULL) {
+			ether_restore_header(&m, eh, &save_eh);
+			if (m != NULL)
+				vlan_input_p(m, chain);
+		} else {
+			m->m_pkthdr.rcvif->if_noproto++;
+			m_freem(m);
+		}
+		return;
+	}
+	KKASSERT(ether_type != ETHERTYPE_VLAN);
+
 	switch (ether_type) {
 #ifdef INET
 	case ETHERTYPE_IP:
@@ -752,15 +784,6 @@ post_stats:
 		break;
 #endif
 
-	case ETHERTYPE_VLAN:
-		if (vlan_input_p != NULL) {
-			(*vlan_input_p)(&save_eh, m);
-		} else {
-			m->m_pkthdr.rcvif->if_noproto++;
-			m_freem(m);
-		}
-		return;
-
 	default:
 #ifdef IPX
 		if (ef_inputp && ef_inputp(ifp, &save_eh, m) == 0)
diff --git a/sys/net/vlan/if_vlan.c b/sys/net/vlan/if_vlan.c
index 8a4d050279..1371d355ba 100644
--- a/sys/net/vlan/if_vlan.c
+++ b/sys/net/vlan/if_vlan.c
@@ -27,7 +27,7 @@
  * SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/net/if_vlan.c,v 1.15.2.13 2003/02/14 22:25:58 fenner Exp $
- * $DragonFly: src/sys/net/vlan/if_vlan.c,v 1.31 2008/03/18 14:12:45 sephe Exp $
+ * $DragonFly: src/sys/net/vlan/if_vlan.c,v 1.32 2008/05/16 13:19:12 sephe Exp $
  */
 
 /*
@@ -40,19 +40,6 @@
  * ether_output() left on our output queue queue when it calls
  * if_start(), rewrite them for use by the real outgoing interface,
  * and ask it to send them.
- *
- *
- * XXX It's incorrect to assume that we must always kludge up
- * headers on the physical device's behalf: some devices support
- * VLAN tag insertion and extraction in firmware. For these cases,
- * one can change the behavior of the vlan interface by setting
- * the LINK0 flag on it (that is setting the vlan interface's LINK0
- * flag, _not_ the parent's LINK0 flag; we try to leave the parent
- * alone). If the interface has the LINK0 flag set, then it will
- * not modify the ethernet header on output, because the parent
- * can do that for itself. On input, the parent can call vlan_input_tag()
- * directly in order to supply us with an incoming mbuf and the vlan
- * tag value that goes with it.
  */
 
 #ifndef NVLAN
@@ -147,8 +134,7 @@ static void	vlan_init(void *);
 static void	vlan_start(struct ifnet *);
 static int	vlan_ioctl(struct ifnet *, u_long, caddr_t, struct ucred *);
 
-static int	vlan_input(const struct ether_header *eh, struct mbuf *m);
-static int	vlan_input_tag(struct mbuf *m, uint16_t t);
+static int	vlan_input(struct mbuf *m, struct mbuf_chain *);
 
 static void	vlan_clrmulti(struct ifvlan *, struct ifnet *);
 static int	vlan_setmulti(struct ifvlan *, struct ifnet *);
@@ -265,7 +251,6 @@ vlan_modevent(module_t mod, int type, void *data)
 	case MOD_LOAD:
 		LIST_INIT(&ifv_list);
 		vlan_input_p = vlan_input;
-		vlan_input_tag_p = vlan_input_tag;
 		vlan_ifdetach_cookie =
 		EVENTHANDLER_REGISTER(ifnet_detach_event,
 				      vlan_ifdetach, NULL,
@@ -276,7 +261,6 @@ vlan_modevent(module_t mod, int type, void *data)
 	case MOD_UNLOAD:
 		if_clone_detach(&vlan_cloner);
 		vlan_input_p = NULL;
-		vlan_input_tag_p = NULL;
 		EVENTHANDLER_DEREGISTER(ifnet_detach_event,
 					vlan_ifdetach_cookie);
 		while (!LIST_EMPTY(&ifv_list))
@@ -457,93 +441,58 @@ vlan_start(struct ifnet *ifp)
 }
 
 static int
-vlan_input_tag(struct mbuf *m, uint16_t t)
+vlan_input(struct mbuf *m, struct mbuf_chain *chain)
 {
-	struct bpf_if *bif;
-	struct ifvlan *ifv;
+	struct ifvlan *ifv = NULL;
 	struct ifnet *rcvif;
+	struct vlan_trunk *vlantrunks;
+	struct vlan_entry *entry;
 
 	rcvif = m->m_pkthdr.rcvif;
-
 	ASSERT_SERIALIZED(rcvif->if_serializer);
+	KKASSERT(m->m_flags & M_VLANTAG);
 
-	/*
-	 * Fake up a header and send the packet to the physical interface's
-	 * bpf tap if active.
-	 */
-	if ((bif = rcvif->if_bpf) != NULL)
-		vlan_ether_ptap(bif, m, t);
-
-	for (ifv = LIST_FIRST(&ifv_list); ifv != NULL;
-	    ifv = LIST_NEXT(ifv, ifv_list)) {
-		if (rcvif == ifv->ifv_p && ifv->ifv_tag == t)
-			break;
-	}
-
-	if (ifv == NULL || (ifv->ifv_if.if_flags & IFF_UP) == 0) {
+	vlantrunks = rcvif->if_vlantrunks;
+	if (vlantrunks == NULL) {
+		rcvif->if_noproto++;
 		m_freem(m);
-		return -1;	/* So the parent can take note */
+		return -1;
 	}
 
-	/*
-	 * Having found a valid vlan interface corresponding to
-	 * the given source interface and vlan tag, run the
-	 * the real packet through ether_input().
-	 */
-	m->m_pkthdr.rcvif = &ifv->ifv_if;
-
-	ifv->ifv_if.if_ipackets++;
-	lwkt_serialize_exit(rcvif->if_serializer);
-	lwkt_serialize_enter(ifv->ifv_if.if_serializer);
-	ether_input(&ifv->ifv_if, m);
-	lwkt_serialize_exit(ifv->ifv_if.if_serializer);
-	lwkt_serialize_enter(rcvif->if_serializer);
-	return 0;
-}
-
-static int
-vlan_input(const struct ether_header *eh, struct mbuf *m)
-{
-	struct ifvlan *ifv;
-	struct ifnet *rcvif;
-	struct ether_header eh_copy;
-
-	rcvif = m->m_pkthdr.rcvif;
-	ASSERT_SERIALIZED(rcvif->if_serializer);
-
-	for (ifv = LIST_FIRST(&ifv_list); ifv != NULL;
-	    ifv = LIST_NEXT(ifv, ifv_list)) {
-		if (rcvif == ifv->ifv_p
-		    && (EVL_VLANOFTAG(ntohs(*mtod(m, u_int16_t *)))
-			== ifv->ifv_tag))
+	crit_enter();
+	LIST_FOREACH(entry, &vlantrunks[mycpuid].vlan_list, ifv_link) {
+		if (entry->ifv->ifv_tag ==
+		    EVL_VLANOFTAG(m->m_pkthdr.ether_vlantag)) {
+			ifv = entry->ifv;
 			break;
+		}
 	}
+	crit_exit();
 
-	if (ifv == NULL || (ifv->ifv_if.if_flags & IFF_UP) == 0) {
+	/*
+	 * Packet is discarded if:
+	 * - no corresponding vlan(4) interface
+	 * - vlan(4) interface has not been completely set up yet,
+	 *   or is being destroyed (ifv->ifv_p != rcvif)
+	 * - vlan(4) interface is not brought up
+	 */
+	if (ifv == NULL || ifv->ifv_p != rcvif ||
+	    (ifv->ifv_if.if_flags & IFF_UP) == 0) {
 		rcvif->if_noproto++;
 		m_freem(m);
 		return -1;	/* so ether_input can take note */
 	}
 
 	/*
-	 * Having found a valid vlan interface corresponding to
-	 * the given source interface and vlan tag, remove the
-	 * remaining encapsulation (ether_vlan_header minus the ether_header
-	 * that had already been removed) and run the real packet
-	 * through ether_input() a second time (it had better be
-	 * reentrant!).
+	 * Clear M_VLANTAG, before the packet is handed to
+	 * vlan(4) interface
 	 */
-	eh_copy = *eh;
-	eh_copy.ether_type = mtod(m, u_int16_t *)[1];	/* evl_proto */
-	m->m_pkthdr.rcvif = &ifv->ifv_if;
-	m_adj(m, EVL_ENCAPLEN);
-	M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT); 
-	*(struct ether_header *)mtod(m, void *) = eh_copy;
+	m->m_flags &= ~M_VLANTAG;
 
 	ifv->ifv_if.if_ipackets++;
 	lwkt_serialize_exit(rcvif->if_serializer);
 	lwkt_serialize_enter(ifv->ifv_if.if_serializer);
-	ether_input(&ifv->ifv_if, m);
+	ether_input_chain(&ifv->ifv_if, m, chain);
 	lwkt_serialize_exit(ifv->ifv_if.if_serializer);
 	lwkt_serialize_enter(rcvif->if_serializer);
 	return 0;
diff --git a/sys/net/vlan/if_vlan_ether.c b/sys/net/vlan/if_vlan_ether.c
index 1ea06b15ca..ba06d8116d 100644
--- a/sys/net/vlan/if_vlan_ether.c
+++ b/sys/net/vlan/if_vlan_ether.c
@@ -27,7 +27,7 @@
  * SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/net/if_vlan.c,v 1.15.2.13 2003/02/14 22:25:58 fenner Exp $
- * $DragonFly: src/sys/net/vlan/if_vlan_ether.c,v 1.2 2008/03/10 11:44:57 sephe Exp $
+ * $DragonFly: src/sys/net/vlan/if_vlan_ether.c,v 1.3 2008/05/16 13:19:12 sephe Exp $
  */
 
 #include <sys/param.h>
@@ -144,3 +144,28 @@ vlan_ether_ptap(struct bpf_if *bp, struct mbuf *m, uint16_t vlantag)
 	/* XXX assumes data was left intact */
 	M_PREPEND(m, ETHER_HDR_LEN, MB_WAIT);
 }
+
+void
+vlan_ether_decap(struct mbuf **m0)
+{
+	struct mbuf *m = *m0;
+	struct ether_vlan_header *evh;
+
+	KKASSERT((m->m_flags & M_VLANTAG) == 0);
+
+	if (m->m_len < sizeof(*evh)) {
+		/* Error in the caller */
+		m_freem(m);
+		*m0 = NULL;
+		return;
+	}
+	evh = mtod(m, struct ether_vlan_header *);
+
+	m->m_pkthdr.ether_vlantag = ntohs(evh->evl_tag);
+	m->m_flags |= M_VLANTAG;
+
+	bcopy((uint8_t *)evh, (uint8_t *)evh + EVL_ENCAPLEN,
+	      2 * ETHER_ADDR_LEN);
+	m_adj(m, EVL_ENCAPLEN);
+	*m0 = m;
+}
diff --git a/sys/net/vlan/if_vlan_ether.h b/sys/net/vlan/if_vlan_ether.h
index c3d0f7fb18..13d186059c 100644
--- a/sys/net/vlan/if_vlan_ether.h
+++ b/sys/net/vlan/if_vlan_ether.h
@@ -27,7 +27,7 @@
  * SUCH DAMAGE.
  *
  * $FreeBSD: src/sys/net/if_vlan_var.h,v 1.5.2.3 2001/12/04 20:01:54 brooks Exp $
- * $DragonFly: src/sys/net/vlan/if_vlan_ether.h,v 1.1 2008/03/10 11:44:57 sephe Exp $
+ * $DragonFly: src/sys/net/vlan/if_vlan_ether.h,v 1.2 2008/05/16 13:19:12 sephe Exp $
  */
 
 #ifndef _NET_IF_VLAN_ETHER_H_
@@ -41,6 +41,7 @@ struct mbuf;
 
 void	vlan_start_dispatch(struct netmsg *);
 void	vlan_ether_ptap(struct bpf_if *, struct mbuf *, uint16_t);
+void	vlan_ether_decap(struct mbuf **);
 
 #endif	/* _KERNEL */
 
-- 
2.41.0