Index: if_vlan.c
===================================================================
RCS file: /cvs/src/sys/net/if_vlan.c,v
retrieving revision 1.150
diff -u -p -r1.150 if_vlan.c
--- if_vlan.c	8 Dec 2015 11:35:42 -0000	1.150
+++ if_vlan.c	4 Jan 2016 05:26:02 -0000
@@ -78,23 +78,33 @@
 #define TAG_HASH_MASK		(TAG_HASH_SIZE - 1)
 #define TAG_HASH(tag)		(tag & TAG_HASH_MASK)
 SRPL_HEAD(, ifvlan) *vlan_tagh, *svlan_tagh;
-struct rwlock vlan_tagh_lk = RWLOCK_INITIALIZER("vlantag");
+struct rwlock vlan_cfg_lk = RWLOCK_INITIALIZER("vlantag");
 
 int	vlan_input(struct ifnet *, struct mbuf *, void *);
 void	vlan_start(struct ifnet *ifp);
 int	vlan_ioctl(struct ifnet *ifp, u_long cmd, caddr_t addr);
-int	vlan_unconfig(struct ifnet *ifp, struct ifnet *newp);
-int	vlan_config(struct ifvlan *, struct ifnet *, u_int16_t);
-void	vlan_vlandev_state(void *);
+int	vlan_up(struct ifvlan *);
+int	vlan_down(struct ifvlan *);
+int	vlan_p_unconfig(struct ifnet *, struct ifnet *);
+int	vlan_p_config(struct ifvlan *, struct ifnet *);
+void	vlan_link_hook(void *);
+void	vlan_link_state(struct ifvlan *, u_char, uint64_t);
 void	vlanattach(int count);
 int	vlan_set_promisc(struct ifnet *ifp);
-int	vlan_ether_addmulti(struct ifvlan *, struct ifreq *);
-int	vlan_ether_delmulti(struct ifvlan *, struct ifreq *);
-void	vlan_ether_purgemulti(struct ifvlan *);
-void	vlan_ether_resetmulti(struct ifvlan *, struct ifnet *);
+
+int	vlan_set_compat(struct ifnet *, struct ifreq *);
+int	vlan_get_compat(struct ifnet *, struct ifreq *);
+
+int	vlan_multi_add(struct ifvlan *, struct ifreq *);
+int	vlan_multi_del(struct ifvlan *, struct ifreq *);
+void	vlan_multi_apply(struct ifvlan *, struct ifnet *, u_long);
+void	vlan_multi_free(struct ifvlan *);
+
+int	vlan_inuse(uint16_t, unsigned int, uint16_t);
+int	vlan_inuse_locked(uint16_t, unsigned int, uint16_t);
+
 int	vlan_clone_create(struct if_clone *, int);
 int	vlan_clone_destroy(struct ifnet *);
-void	vlan_ifdetach(void *);
 
 struct if_clone vlan_cloner =
     IF_CLONE_INITIALIZER("vlan", vlan_clone_create, vlan_clone_destroy);
@@ -145,10 +155,9 @@ vlan_clone_create(struct if_clone *ifc, 
 	LIST_INIT(&ifv->vlan_mc_listhead);
 	ifp = &ifv->ifv_if;
 	ifp->if_softc = ifv;
-	snprintf(ifp->if_xname, sizeof ifp->if_xname, "%s%d", ifc->ifc_name,
-	    unit);
+	snprintf(ifp->if_xname, sizeof(ifp->if_xname), "%s%d",
+	    ifc->ifc_name, unit);
 	/* NB: flags are not set here */
-	/* NB: mtu is not set here */
 
 	/* Special handling for the IEEE 802.1ad QinQ variant */
 	if (strcmp("svlan", ifc->ifc_name) == 0)
@@ -157,10 +166,11 @@ vlan_clone_create(struct if_clone *ifc, 
 		ifv->ifv_type = ETHERTYPE_VLAN;
 
 	refcnt_init(&ifv->ifv_refcnt);
-
+	ifp->if_xflags = IFXF_MPSAFE;
 	ifp->if_start = vlan_start;
 	ifp->if_ioctl = vlan_ioctl;
-	IFQ_SET_MAXLEN(&ifp->if_snd, 1);
+	ifp->if_hardmtu = 0xffff;
+	ifp->if_link_state = LINK_STATE_DOWN;
 	IFQ_SET_READY(&ifp->if_snd);
 	if_attach(ifp);
 	ether_ifattach(ifp);
@@ -190,22 +200,18 @@ vlan_clone_destroy(struct ifnet *ifp)
 {
 	struct ifvlan	*ifv = ifp->if_softc;
 
-	vlan_unconfig(ifp, NULL);
+	if (ISSET(ifp->if_flags, IFF_RUNNING))
+		vlan_down(ifv);
+
 	ether_ifdetach(ifp);
 	if_detach(ifp);
 	refcnt_finalize(&ifv->ifv_refcnt, "vlanrefs");
+	vlan_multi_free(ifv);
 	free(ifv, M_DEVBUF, sizeof(*ifv));
 
 	return (0);
 }
 
-void
-vlan_ifdetach(void *ptr)
-{
-	struct ifvlan	*ifv = ptr;
-	vlan_clone_destroy(&ifv->ifv_if);
-}
-
 static inline int
 vlan_mplstunnel(int ifidx)
 {
@@ -227,31 +233,23 @@ vlan_mplstunnel(int ifidx)
 void
 vlan_start(struct ifnet *ifp)
 {
-	struct ifvlan   *ifv;
+	struct ifvlan   *ifv = ifp->if_softc;
 	struct ifnet	*p;
 	struct mbuf	*m;
 	uint8_t		 prio;
 
-	ifv = ifp->if_softc;
-	p = ifv->ifv_p;
-
-	for (;;) {
-		IFQ_DEQUEUE(&ifp->if_snd, m);
-		if (m == NULL)
-			break;
+	p = if_get(ifv->ifv_p);
+	if (p == NULL) {
+		IFQ_PURGE(&ifp->if_snd);
+		return;
+	}
 
+	while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) {
 #if NBPFILTER > 0
 		if (ifp->if_bpf)
 			bpf_mtap_ether(ifp->if_bpf, m, BPF_DIRECTION_OUT);
 #endif /* NBPFILTER > 0 */
 
-		if ((p->if_flags & (IFF_UP|IFF_RUNNING)) !=
-		    (IFF_UP|IFF_RUNNING)) {
-			ifp->if_oerrors++;
-			m_freem(m);
-			continue;
-		}
-
 		/* IEEE 802.1p has prio 0 and 1 swapped */
 		prio = m->m_pkthdr.pf.prio;
 		if (prio <= 1)
@@ -274,36 +272,47 @@ vlan_start(struct ifnet *ifp)
 			    (prio << EVL_PRIO_BITS);
 			m->m_flags |= M_VLANTAG;
 		} else {
-			struct ether_vlan_header evh;
-
-			m_copydata(m, 0, ETHER_HDR_LEN, (caddr_t)&evh);
-			evh.evl_proto = evh.evl_encap_proto;
-			evh.evl_encap_proto = htons(ifv->ifv_type);
-			evh.evl_tag = htons(ifv->ifv_tag +
-			    (prio << EVL_PRIO_BITS));
-			m_adj(m, ETHER_HDR_LEN);
-			M_PREPEND(m, sizeof(evh), M_DONTWAIT);
-			if (m == NULL) {
+			if (vlan_inject(m, ifv->ifv_type,
+			    ifv->ifv_tag + (prio << EVL_PRIO_BITS)) != 0) {
 				ifp->if_oerrors++;
 				continue;
 			}
-			m_copyback(m, 0, sizeof(evh), &evh, M_NOWAIT);
-			m->m_flags &= ~M_VLANTAG;
 		}
 
-		if (if_enqueue(p, m)) {
-			ifp->if_oerrors++;
+		if (if_enqueue(p, m) != 0)
 			continue;
-		}
+
 		ifp->if_opackets++;
 	}
+
+	if_put(p);
+}
+
+int
+vlan_inject(struct mbuf *m, uint16_t type, uint16_t tag)
+{
+	struct ether_vlan_header evh;
+
+	m_copydata(m, 0, ETHER_HDR_LEN, (caddr_t)&evh);
+	evh.evl_proto = evh.evl_encap_proto;
+	evh.evl_encap_proto = htons(type);
+	evh.evl_tag = htons(tag);
+	m_adj(m, ETHER_HDR_LEN);
+	M_PREPEND(m, sizeof(evh), M_DONTWAIT);
+	if (m == NULL)
+		return (ENOMEM);
+
+	m_copyback(m, 0, sizeof(evh), &evh, M_NOWAIT);
+	m->m_flags &= ~M_VLANTAG;
+
+	return (0);
 }
 
 /*
  * vlan_input() returns 1 if it has consumed the packet, 0 otherwise.
  */
 int
-vlan_input(struct ifnet *ifp, struct mbuf *m, void *cookie)
+vlan_input(struct ifnet *p, struct mbuf *m, void *cookie)
 {
 	struct ifvlan			*ifv;
 	struct ether_vlan_header	*evl;
@@ -323,7 +332,7 @@ vlan_input(struct ifnet *ifp, struct mbu
 	} else if ((etype == ETHERTYPE_VLAN) || (etype == ETHERTYPE_QINQ)) {
 		if (m->m_len < EVL_ENCAPLEN &&
 		    (m = m_pullup(m, EVL_ENCAPLEN)) == NULL) {
-			ifp->if_ierrors++;
+			p->if_ierrors++;
 			return (1);
 		}
 
@@ -345,13 +354,14 @@ vlan_input(struct ifnet *ifp, struct mbu
 
 	list = &tagh[TAG_HASH(tag)];
 	SRPL_FOREACH(ifv, list, &i, ifv_list) {
-		if (ifp == ifv->ifv_p && tag == ifv->ifv_tag &&
+		if (p->if_index == ifv->ifv_p &&
+		    tag == ifv->ifv_tag &&
 		    etype == ifv->ifv_type)
 			break;
 	}
 
 	if (ifv == NULL) {
-		ifp->if_noproto++;
+		p->if_noproto++;
 		goto drop;
 	}
 
@@ -364,12 +374,8 @@ vlan_input(struct ifnet *ifp, struct mbu
 	 * promiscuous mode.
 	 */
 	if (!ETHER_IS_MULTICAST(eh->ether_dhost) &&
-	    (ifp->if_flags & IFF_PROMISC) &&
-	    (ifv->ifv_if.if_flags & IFF_PROMISC) == 0) {
-		if (bcmp(&ifv->ifv_ac.ac_enaddr, eh->ether_dhost,
-		    ETHER_ADDR_LEN))
-			goto drop;
-	}
+	    bcmp(&ifv->ifv_ac.ac_enaddr, eh->ether_dhost, ETHER_ADDR_LEN) != 0)
+		goto drop;
 
 	/*
 	 * Having found a valid vlan interface corresponding to
@@ -395,41 +401,146 @@ drop:
 	return (1);
 }
 
+void
+vlan_p_detach(void *v)
+{
+	struct ifvlan *ifv = v;
+	struct ifnet *ifp = &ifv->ifv_if;
+
+	if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+		(void)vlan_down(ifv);
+		CLR(ifp->if_flags, IFF_UP);
+	}
+
+	ifv->ifv_p = 0;
+}
+
+void
+vlan_link_hook(void *v)
+{
+	struct ifvlan *ifv = v;
+	struct ifnet *p;
+
+	u_char link = LINK_STATE_DOWN;
+	uint64_t baud = 0;
+
+	p = if_get(ifv->ifv_p);
+	if (p != NULL) {
+		link = p->if_link_state;
+		baud = p->if_baudrate;
+	}
+	if_put(p);
+
+	vlan_link_state(ifv, link, baud);
+}
+
+void
+vlan_link_state(struct ifvlan *ifv, u_char link, uint64_t baud)
+{
+	if (ifv->ifv_if.if_link_state == link)
+		return;
+
+	ifv->ifv_if.if_link_state = link;
+	ifv->ifv_if.if_baudrate = baud;
+
+	if_link_state_change(&ifv->ifv_if);
+}
+
 int
-vlan_config(struct ifvlan *ifv, struct ifnet *p, u_int16_t tag)
+vlan_promisc(struct ifvlan *ifv, int promisc)
 {
-	struct sockaddr_dl	*sdl1, *sdl2;
-	SRPL_HEAD(, ifvlan)	*tagh, *list;
-	u_int			 flags;
-
-	if (p->if_type != IFT_ETHER)
-		return EPROTONOSUPPORT;
-	if (ifv->ifv_p == p && ifv->ifv_tag == tag) /* noop */
+	struct ifnet *p;
+	int error = 0;
+
+	if (ifv->ifv_promisc == promisc)
 		return (0);
 
-	/* Remember existing interface flags and reset the interface */
-	flags = ifv->ifv_flags;
-	vlan_unconfig(&ifv->ifv_if, p);
-	ifv->ifv_p = p;
-	ifv->ifv_if.if_baudrate = p->if_baudrate;
-
-	if (p->if_capabilities & IFCAP_VLAN_MTU) {
-		ifv->ifv_if.if_mtu = p->if_mtu;
-		ifv->ifv_if.if_hardmtu = p->if_hardmtu;
-	} else {
-		ifv->ifv_if.if_mtu = p->if_mtu - EVL_ENCAPLEN;
-		ifv->ifv_if.if_hardmtu = p->if_hardmtu - EVL_ENCAPLEN;
+	p = if_get(ifv->ifv_p);
+	if (p != NULL) {
+		error = ifpromisc(p, promisc);
 	}
+	if_put(p);
 
-	ifv->ifv_if.if_flags = p->if_flags &
-	    (IFF_UP | IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST);
+	if (error == 0)
+		ifv->ifv_promisc = promisc;
 
-	/* Reset promisc mode on the interface and its parent */
-	if (flags & IFVF_PROMISC) {
-		ifv->ifv_if.if_flags |= IFF_PROMISC;
-		vlan_set_promisc(&ifv->ifv_if);
+	return (error);
+}
+
+int
+vlan_parent_config(struct ifvlan *ifv, struct ifnet *p)
+{
+	int error;
+
+	vlan_multi_apply(ifv, p, SIOCADDMULTI);
+
+	if (ifv->ifv_promisc) {
+		error = ifpromisc(p, 1);
+		if (error != 0)
+			goto delmulti;
 	}
 
+	return (0);
+
+delmulti:
+	vlan_multi_apply(ifv, p, SIOCDELMULTI);
+	return (error);
+}
+
+int
+vlan_up(struct ifvlan *ifv)
+{
+	struct ifnet *ifp = &ifv->ifv_if;
+	struct ifnet *p;
+	SRPL_HEAD(, ifvlan) *tagh, *list;
+	int error = 0;
+	u_int hardmtu;
+
+	KASSERT(!ISSET(ifp->if_flags, IFF_RUNNING));
+
+	tagh = ifv->ifv_type == ETHERTYPE_QINQ ? svlan_tagh : vlan_tagh;
+	list = &tagh[TAG_HASH(ifv->ifv_tag)];
+
+	if (ifv->ifv_tag == EVL_VLID_NONE)
+		return (EADDRNOTAVAIL);
+
+	p = if_get(ifv->ifv_p);
+	if (p == NULL)
+		return (ENXIO);
+
+	if (p->if_type != IFT_ETHER) {
+		error = EPROTONOSUPPORT;
+		goto put;
+	}
+
+	hardmtu = p->if_hardmtu;
+	if (!ISSET(p->if_capabilities, IFCAP_VLAN_MTU))
+		hardmtu -= EVL_ENCAPLEN;
+
+	if (ifp->if_mtu > hardmtu) {
+		error = ENOBUFS;
+		goto put;
+	}
+
+	error = rw_enter(&vlan_cfg_lk, RW_WRITE | RW_INTR);
+	if (error != 0)
+		goto put;
+
+	error = vlan_inuse_locked(ifv->ifv_type, ifv->ifv_p, ifv->ifv_tag);
+	if (error != 0)
+		goto leave;
+
+	error = vlan_parent_config(ifv, p);
+	if (error != 0)
+		goto leave;
+
+	/* commit */
+	ifp->if_hardmtu = hardmtu;
+	ifp->if_flags = p->if_flags &
+	    (IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST);
+
+	if_setlladdr(ifp, LLADDR(p->if_sadl));
+
 	/*
 	 * If the parent interface can do hardware-assisted
 	 * VLAN encapsulation, then propagate its hardware-
@@ -439,257 +550,328 @@ vlan_config(struct ifvlan *ifv, struct i
 	 * possibly compute the correct checksums for tagged packets.
 	 */
 	if (p->if_capabilities & IFCAP_VLAN_HWTAGGING)
-		ifv->ifv_if.if_capabilities = p->if_capabilities &
-		    IFCAP_CSUM_MASK;
+		ifp->if_capabilities = p->if_capabilities & IFCAP_CSUM_MASK;
 
 	/*
 	 * Hardware VLAN tagging only works with the default VLAN
 	 * ethernet type (0x8100).
 	 */
 	if (ifv->ifv_type != ETHERTYPE_VLAN)
-		ifv->ifv_if.if_capabilities &= ~IFCAP_VLAN_HWTAGGING;
-
-	/*
-	 * Set up our ``Ethernet address'' to reflect the underlying
-	 * physical interface's.
-	 */
-	sdl1 = ifv->ifv_if.if_sadl;
-	sdl2 = p->if_sadl;
-	sdl1->sdl_type = IFT_ETHER;
-	sdl1->sdl_alen = ETHER_ADDR_LEN;
-	bcopy(LLADDR(sdl2), LLADDR(sdl1), ETHER_ADDR_LEN);
-	bcopy(LLADDR(sdl2), ifv->ifv_ac.ac_enaddr, ETHER_ADDR_LEN);
-
-	ifv->ifv_tag = tag;
+		ifp->if_capabilities &= ~IFCAP_VLAN_HWTAGGING;
 
 	/* Register callback for physical link state changes */
 	ifv->lh_cookie = hook_establish(p->if_linkstatehooks, 1,
-	    vlan_vlandev_state, ifv);
+	    vlan_link_hook, ifv);
 
 	/* Register callback if parent wants to unregister */
 	ifv->dh_cookie = hook_establish(p->if_detachhooks, 0,
-	    vlan_ifdetach, ifv);
-
-	vlan_vlandev_state(ifv);
+	    vlan_p_detach, ifv);
 
-	/* Change input handler of the physical interface. */
+	SRPL_INSERT_HEAD_LOCKED(&vlan_tagh_rc, list, ifv, ifv_list);
 	if_ih_insert(p, vlan_input, NULL);
 
-	tagh = ifv->ifv_type == ETHERTYPE_QINQ ? svlan_tagh : vlan_tagh;
-	list = &tagh[TAG_HASH(tag)];
+	vlan_link_state(ifv, p->if_link_state, p->if_baudrate);
 
-	rw_enter_write(&vlan_tagh_lk);
-	SRPL_INSERT_HEAD_LOCKED(&vlan_tagh_rc, list, ifv, ifv_list);
-	rw_exit_write(&vlan_tagh_lk);
+	SET(ifp->if_flags, IFF_RUNNING);
 
-	return (0);
+leave:
+	rw_exit(&vlan_cfg_lk);
+
+put:
+	if_put(p);
+	return (error);
 }
 
 int
-vlan_unconfig(struct ifnet *ifp, struct ifnet *newp)
+vlan_down(struct ifvlan *ifv)
 {
-	struct sockaddr_dl	*sdl;
-	struct ifvlan		*ifv;
-	SRPL_HEAD(, ifvlan)	*tagh, *list;
-	struct ifnet		*p;
+	SRPL_HEAD(, ifvlan) *tagh, *list;
+	struct ifnet *ifp = &ifv->ifv_if;
+	struct ifnet *p;
+	int error;
 
-	ifv = ifp->if_softc;
-	if ((p = ifv->ifv_p) == NULL)
-		return 0;
-
-	/* Unset promisc mode on the interface and its parent */
-	if (ifv->ifv_flags & IFVF_PROMISC) {
-		ifp->if_flags &= ~IFF_PROMISC;
-		vlan_set_promisc(ifp);
-	}
+	KASSERT(ISSET(ifp->if_flags, IFF_RUNNING));
 
 	tagh = ifv->ifv_type == ETHERTYPE_QINQ ? svlan_tagh : vlan_tagh;
 	list = &tagh[TAG_HASH(ifv->ifv_tag)];
 
-	rw_enter_write(&vlan_tagh_lk);
-	SRPL_REMOVE_LOCKED(&vlan_tagh_rc, list, ifv, ifvlan, ifv_list);
-	rw_exit_write(&vlan_tagh_lk);
+	error = rw_enter(&vlan_cfg_lk, RW_WRITE | RW_INTR);
+	if (error != 0)
+		return (error);
 
-	/* Restore previous input handler. */
-	if_ih_remove(p, vlan_input, NULL);
+	CLR(ifp->if_flags, IFF_RUNNING);
 
-	hook_disestablish(p->if_linkstatehooks, ifv->lh_cookie);
-	hook_disestablish(p->if_detachhooks, ifv->dh_cookie);
-	/* Reset link state */
-	if (newp != NULL) {
-		ifp->if_link_state = LINK_STATE_INVALID;
-		if_link_state_change(ifp);
-	}
+	ifq_barrier(&ifp->if_snd);
 
-	/*
- 	 * Since the interface is being unconfigured, we need to
-	 * empty the list of multicast groups that we may have joined
-	 * while we were alive and remove them from the parent's list
-	 * as well.
-	 */
-	vlan_ether_resetmulti(ifv, newp);
+	vlan_link_state(ifv, LINK_STATE_DOWN, 0);
 
-	/* Disconnect from parent. */
-	ifv->ifv_p = NULL;
-	ifv->ifv_if.if_mtu = ETHERMTU;
-	ifv->ifv_if.if_hardmtu = ETHERMTU;
-	ifv->ifv_flags = 0;
-
-	/* Clear our MAC address. */
-	sdl = ifv->ifv_if.if_sadl;
-	sdl->sdl_type = IFT_ETHER;
-	sdl->sdl_alen = ETHER_ADDR_LEN;
-	bzero(LLADDR(sdl), ETHER_ADDR_LEN);
-	bzero(ifv->ifv_ac.ac_enaddr, ETHER_ADDR_LEN);
+	if_ih_remove(p, vlan_input, NULL);
+	SRPL_REMOVE_LOCKED(&vlan_tagh_rc, list, ifv, ifvlan, ifv_list);
 
-	return (0);
-}
+	hook_disestablish(p->if_detachhooks, ifv->dh_cookie);
+	hook_disestablish(p->if_linkstatehooks, ifv->lh_cookie);
 
-void
-vlan_vlandev_state(void *v)
-{
-	struct ifvlan	*ifv = v;
+	ifp->if_capabilities = 0;
 
-	if (ifv->ifv_if.if_link_state == ifv->ifv_p->if_link_state)
-		return;
+	if_setlladdr(ifp, etheranyaddr);
 
-	ifv->ifv_if.if_link_state = ifv->ifv_p->if_link_state;
-	ifv->ifv_if.if_baudrate = ifv->ifv_p->if_baudrate;
-	if_link_state_change(&ifv->ifv_if);
-}
+	CLR(ifp->if_flags, IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST);
 
-int
-vlan_set_promisc(struct ifnet *ifp)
-{
-	struct ifvlan	*ifv = ifp->if_softc;
-	int		 error = 0;
+	ifp->if_hardmtu = 0xffff;
+
+	rw_exit(&vlan_cfg_lk);
 
-	if ((ifp->if_flags & IFF_PROMISC) != 0) {
-		if ((ifv->ifv_flags & IFVF_PROMISC) == 0)
-			if ((error = ifpromisc(ifv->ifv_p, 1)) == 0)
-				ifv->ifv_flags |= IFVF_PROMISC;
-	} else {
-		if ((ifv->ifv_flags & IFVF_PROMISC) != 0)
-			if ((error = ifpromisc(ifv->ifv_p, 0)) == 0)
-				ifv->ifv_flags &= ~IFVF_PROMISC;
-	}
 	return (0);
 }
 
 int
 vlan_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 {
-	struct proc	*p = curproc;	/* XXX */
-	struct ifaddr	*ifa;
-	struct ifnet	*pr;
-	struct ifreq	*ifr;
-	struct ifvlan	*ifv;
-	struct vlanreq	 vlr;
-	int		 error = 0, s;
-
-	ifr = (struct ifreq *)data;
-	ifa = (struct ifaddr *)data;
-	ifv = ifp->if_softc;
+	struct ifvlan *ifv = ifp->if_softc;
+	struct ifreq *ifr = (struct ifreq *)data;
+	struct if_parent *parent = (struct if_parent *)data;
+	struct ifnet *p;
+	uint16_t tag;
+	int error = 0;
 
 	switch (cmd) {
 	case SIOCSIFADDR:
-		if (ifv->ifv_p != NULL)
-			ifp->if_flags |= IFF_UP;
-		else
-			error = EINVAL;
+		ifp->if_flags |= IFF_UP;
+		/* FALLTHROUGH */
+
+	case SIOCSIFFLAGS:
+		error = vlan_promisc(ifv,
+		    ISSET(ifp->if_flags, IFF_PROMISC) ? 1 : 0);
+		if (error != 0)
+			break;
+
+		if (ISSET(ifp->if_flags, IFF_UP)) {
+			if (!ISSET(ifp->if_flags, IFF_RUNNING))
+				error = vlan_up(ifv);
+		} else {
+			if (ISSET(ifp->if_flags, IFF_RUNNING))
+				error = vlan_down(ifv);
+		}
 		break;
 
-	case SIOCSIFMTU:
-		if (ifv->ifv_p != NULL) {
-			if (ifr->ifr_mtu < ETHERMIN ||
-			    ifr->ifr_mtu > ifv->ifv_if.if_hardmtu)
-				error = EINVAL;
-			else
-				ifp->if_mtu = ifr->ifr_mtu;
-		} else
+	case SIOCSVNETID:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
+			break;
+		}
+
+		tag = ifr->ifr_vnetid;
+		if (tag == ifv->ifv_tag)
+			break;
+
+		if (tag < EVL_VLID_MIN || tag > EVL_VLID_MAX) {
 			error = EINVAL;
+			break;
+		}
+
+		error = vlan_inuse(ifv->ifv_type, ifv->ifv_p, tag);
+		if (error != 0)
+			break;
 
+		ifv->ifv_tag = tag;
 		break;
 
-	case SIOCSETVLAN:
-		if ((error = suser(p, 0)) != 0)
-			break;
-		if ((error = copyin(ifr->ifr_data, &vlr, sizeof vlr)))
+	case SIOCGVNETID:
+		if (ifr->ifr_vnetid == EVL_VLID_NONE)
+			error = EADDRNOTAVAIL;
+		else
+			ifr->ifr_vnetid = (uint32_t)ifv->ifv_tag;
+		break;
+
+	case SIOCDVNETID:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
 			break;
-		if (vlr.vlr_parent[0] == '\0') {
-			s = splnet();
-			vlan_unconfig(ifp, NULL);
-			if (ifp->if_flags & IFF_UP)
-				if_down(ifp);
-			ifp->if_flags &= ~IFF_RUNNING;
-			splx(s);
-			break;
-		}
-		pr = ifunit(vlr.vlr_parent);
-		if (pr == NULL) {
-			error = ENOENT;
+		}
+
+		ifv->ifv_tag = 0;
+		break;
+
+	case SIOCSIFPARENT:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
 			break;
 		}
-		/*
-		 * Don't let the caller set up a VLAN tag with
-		 * anything except VLID bits.
-		 */
-		if (vlr.vlr_tag & ~EVL_VLID_MASK) {
+
+		p = ifunit(parent->ifp_parent);
+		if (p == NULL) {
 			error = EINVAL;
 			break;
 		}
-		error = vlan_config(ifv, pr, vlr.vlr_tag);
-		if (error)
+
+		if (ifv->ifv_p == p->if_index) {
+			/* nop */
 			break;
-		ifp->if_flags |= IFF_RUNNING;
+		}
 
-		/* Update promiscuous mode, if necessary. */
-		vlan_set_promisc(ifp);
-		break;
-		
-	case SIOCGETVLAN:
-		bzero(&vlr, sizeof vlr);
-		if (ifv->ifv_p) {
-			snprintf(vlr.vlr_parent, sizeof(vlr.vlr_parent),
-			    "%s", ifv->ifv_p->if_xname);
-			vlr.vlr_tag = ifv->ifv_tag;
+		if (p->if_type != IFT_ETHER) {
+			error = EPROTONOSUPPORT;
+			break;
 		}
-		error = copyout(&vlr, ifr->ifr_data, sizeof vlr);
+
+		error = vlan_inuse(ifv->ifv_type, p->if_index, ifv->ifv_tag);
+		if (error != 0)
+			break;
+
+		ifv->ifv_p = p->if_index;
 		break;
-	case SIOCSIFFLAGS:
-		/*
-		 * For promiscuous mode, we enable promiscuous mode on
-		 * the parent if we need promiscuous on the VLAN interface.
-		 */
-		if (ifv->ifv_p != NULL)
-			error = vlan_set_promisc(ifp);
+
+	case SIOCGIFPARENT:
+		p = if_get(ifv->ifv_p);
+		if (p == NULL)
+			error = EADDRNOTAVAIL;
+		else {
+			memcpy(parent->ifp_parent, p->if_xname,
+			    sizeof(parent->ifp_parent));
+		}
+		if_put(p);
 		break;
 
+	case SIOCDIFPARENT:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
+			break;
+		}
+
+		ifv->ifv_p = 0;
+                break;
+
 	case SIOCADDMULTI:
-		error = (ifv->ifv_p != NULL) ?
-		    vlan_ether_addmulti(ifv, ifr) : EINVAL;
+		error = vlan_multi_add(ifv, ifr);
 		break;
-
 	case SIOCDELMULTI:
-		error = (ifv->ifv_p != NULL) ?
-		    vlan_ether_delmulti(ifv, ifr) : EINVAL;
+		error = vlan_multi_del(ifv, ifr);
+		break;
+
+	case SIOCSETVLAN:
+		error = vlan_set_compat(ifp, ifr);
 		break;
+	case SIOCGETVLAN:
+		error = vlan_get_compat(ifp, ifr);
+		break;
+
 	default:
-		error = ENOTTY;
+		error = ether_ioctl(ifp, &ifv->ifv_ac, cmd, data);
+		break;
 	}
-	return error;
+
+	return (error);
 }
 
+int
+vlan_set_compat(struct ifnet *ifp, struct ifreq *ifr)
+{
+	struct vlanreq vlr;
+	struct ifreq req;
+	struct if_parent parent;
+
+	int error;
+
+	error = suser(curproc, 0);
+	if (error != 0)
+		return (error);
+
+	error = copyin(ifr->ifr_data, &vlr, sizeof(vlr));
+	if (error != 0)
+		return (error);
+
+	if (vlr.vlr_parent[0] == '\0')
+		return (vlan_ioctl(ifp, SIOCDIFPARENT, (caddr_t)ifr));
+
+	memset(&req, 0, sizeof(req));
+	memcpy(req.ifr_name, ifp->if_xname, sizeof(req.ifr_name));
+	req.ifr_vnetid = vlr.vlr_tag;
+
+	error = vlan_ioctl(ifp, SIOCSVNETID, (caddr_t)&req);
+	if (error != 0)
+		return (error);
+
+	memset(&parent, 0, sizeof(parent));
+	memcpy(parent.ifp_name, ifp->if_xname, sizeof(parent.ifp_name));
+	memcpy(parent.ifp_parent, vlr.vlr_parent, sizeof(parent.ifp_parent));
+	error = vlan_ioctl(ifp, SIOCSIFPARENT, (caddr_t)&parent);
+	if (error != 0)
+		return (error);
+
+	memset(&req, 0, sizeof(req));
+	memcpy(req.ifr_name, ifp->if_xname, sizeof(req.ifr_name));
+	SET(ifp->if_flags, IFF_UP);
+	return (vlan_ioctl(ifp, SIOCSIFFLAGS, (caddr_t)&req));
+}
 
 int
-vlan_ether_addmulti(struct ifvlan *ifv, struct ifreq *ifr)
+vlan_get_compat(struct ifnet *ifp, struct ifreq *ifr)
 {
-	struct ifnet		*ifp = ifv->ifv_p;
+	struct ifvlan *ifv = ifp->if_softc;
+	struct vlanreq vlr;
+	struct ifnet *p;
+
+	memset(&vlr, 0, sizeof(vlr));
+	p = if_get(ifv->ifv_p);
+	if (p != NULL)
+		memcpy(vlr.vlr_parent, p->if_xname, sizeof(vlr.vlr_parent));
+	if_put(p);
+
+	vlr.vlr_tag = ifv->ifv_tag;
+
+	return (copyout(&vlr, ifr->ifr_data, sizeof(vlr)));
+}
+
+/*
+ * do a quick check of up and running vlans for existing configurations.
+ *
+ * NOTE: this does allow the same config on down vlans, but vlan_up()
+ * will catch them.
+ */
+int
+vlan_inuse(uint16_t type, unsigned int ifidx, uint16_t tag)
+{
+	int error = 0;
+
+	error = rw_enter(&vlan_cfg_lk, RW_READ | RW_INTR);
+	if (error != 0)
+		return (error);
+
+	error = vlan_inuse_locked(type, ifidx, tag);
+
+	rw_exit(&vlan_cfg_lk);
+
+	return (error);
+}
+
+int
+vlan_inuse_locked(uint16_t type, unsigned int ifidx, uint16_t tag)
+{
+	SRPL_HEAD(, ifvlan) *tagh, *list;
+	struct ifvlan *ifv;
+
+	tagh = type == ETHERTYPE_QINQ ? svlan_tagh : vlan_tagh;
+	list = &tagh[TAG_HASH(tag)];
+
+	SRPL_FOREACH_LOCKED(ifv, list, ifv_list) {
+		if (ifv->ifv_tag == tag &&
+		    ifv->ifv_type == type && /* wat */
+		    ifv->ifv_p == ifidx)
+			return (EADDRINUSE);
+	}
+
+	return (0);
+}
+
+int
+vlan_multi_add(struct ifvlan *ifv, struct ifreq *ifr)
+{
+	struct ifnet		*p;
 	struct vlan_mc_entry	*mc;
 	u_int8_t		 addrlo[ETHER_ADDR_LEN], addrhi[ETHER_ADDR_LEN];
 	int			 error;
 
-	error = ether_addmulti(ifr, (struct arpcom *)&ifv->ifv_ac);
+	error = ether_addmulti(ifr, &ifv->ifv_ac);
 	if (error != ENETRESET)
 		return (error);
 
@@ -712,24 +894,28 @@ vlan_ether_addmulti(struct ifvlan *ifv, 
 	memcpy(&mc->mc_addr, &ifr->ifr_addr, ifr->ifr_addr.sa_len);
 	LIST_INSERT_HEAD(&ifv->vlan_mc_listhead, mc, mc_entries);
 
-	if ((error = (*ifp->if_ioctl)(ifp, SIOCADDMULTI, (caddr_t)ifr)) != 0)
+	p = if_get(ifv->ifv_p);
+	error = p == NULL ? 0 : (*p->if_ioctl)(p, SIOCADDMULTI, (caddr_t)ifr);
+	if_put(p);
+
+	if (error != 0)
 		goto ioctl_failed;
 
 	return (error);
 
  ioctl_failed:
 	LIST_REMOVE(mc, mc_entries);
-	free(mc, M_DEVBUF, sizeof *mc);
+	free(mc, M_DEVBUF, sizeof(*mc));
  alloc_failed:
-	(void)ether_delmulti(ifr, (struct arpcom *)&ifv->ifv_ac);
+	(void)ether_delmulti(ifr, &ifv->ifv_ac);
 
 	return (error);
 }
 
 int
-vlan_ether_delmulti(struct ifvlan *ifv, struct ifreq *ifr)
+vlan_multi_del(struct ifvlan *ifv, struct ifreq *ifr)
 {
-	struct ifnet		*ifp = ifv->ifv_p;
+	struct ifnet		*p;
 	struct ether_multi	*enm;
 	struct vlan_mc_entry	*mc;
 	u_int8_t		 addrlo[ETHER_ADDR_LEN], addrhi[ETHER_ADDR_LEN];
@@ -745,35 +931,42 @@ vlan_ether_delmulti(struct ifvlan *ifv, 
 	if (enm == NULL)
 		return (EINVAL);
 
-	LIST_FOREACH(mc, &ifv->vlan_mc_listhead, mc_entries)
+	LIST_FOREACH(mc, &ifv->vlan_mc_listhead, mc_entries) {
 		if (mc->mc_enm == enm)
 			break;
+	}
 
 	/* We won't delete entries we didn't add */
 	if (mc == NULL)
 		return (EINVAL);
 
-	if ((error = ether_delmulti(ifr, (struct arpcom *)&ifv->ifv_ac)) != 0)
+	error = ether_delmulti(ifr, &ifv->ifv_ac);
+	if (error != ENETRESET)
 		return (error);
 
-	/* We no longer use this multicast address.  Tell parent so. */
-	if ((error = (*ifp->if_ioctl)(ifp, SIOCDELMULTI, (caddr_t)ifr)) != 0) {
-		/* And forget about this address. */
-		LIST_REMOVE(mc, mc_entries);
-		free(mc, M_DEVBUF, sizeof *mc);
-	} else
-		(void)ether_addmulti(ifr, (struct arpcom *)&ifv->ifv_ac);
-	return (error);
+	if (!ISSET(ifv->ifv_if.if_flags, IFF_RUNNING))
+		goto forget;
+
+	p = if_get(ifv->ifv_p);
+	error = p == NULL ? 0 : (*p->if_ioctl)(p, SIOCDELMULTI, (caddr_t)ifr);
+	if_put(p);
+
+	if (error != 0) {
+		(void)ether_addmulti(ifr, &ifv->ifv_ac);
+		return (error);
+	}
+
+forget:
+	/* forget about this address */
+	LIST_REMOVE(mc, mc_entries);
+	free(mc, M_DEVBUF, sizeof(*mc));
+
+	return (0);
 }
 
-/*
- * Delete any multicast address we have asked to add from parent
- * interface.  Called when the vlan is being unconfigured.
- */
 void
-vlan_ether_purgemulti(struct ifvlan *ifv)
+vlan_multi_apply(struct ifvlan *ifv, struct ifnet *p, u_long cmd)
 {
-	struct ifnet		*ifp = ifv->ifv_p;
 	struct vlan_mc_entry	*mc;
 	union {
 		struct ifreq ifreq;
@@ -784,44 +977,21 @@ vlan_ether_purgemulti(struct ifvlan *ifv
 	} ifreq;
 	struct ifreq	*ifr = &ifreq.ifreq;
 
-	memcpy(ifr->ifr_name, ifp->if_xname, IFNAMSIZ);
-	while ((mc = LIST_FIRST(&ifv->vlan_mc_listhead)) != NULL) {
+	memcpy(ifr->ifr_name, p->if_xname, IFNAMSIZ);
+	LIST_FOREACH(mc, &ifv->vlan_mc_listhead, mc_entries) {
 		memcpy(&ifr->ifr_addr, &mc->mc_addr, mc->mc_addr.ss_len);
-		(void)(*ifp->if_ioctl)(ifp, SIOCDELMULTI, (caddr_t)ifr);
-		LIST_REMOVE(mc, mc_entries);
-		free(mc, M_DEVBUF, sizeof *mc);
+
+		(void)(*p->if_ioctl)(p, cmd, (caddr_t)ifr);
 	}
 }
 
 void
-vlan_ether_resetmulti(struct ifvlan *ifv, struct ifnet *p)
+vlan_multi_free(struct ifvlan *ifv)
 {
-	struct ifnet		*ifp = ifv->ifv_p;
 	struct vlan_mc_entry	*mc;
-	union {
-		struct ifreq ifreq;
-		struct {
-			char			ifr_name[IFNAMSIZ];
-			struct sockaddr_storage	ifr_ss;
-		} ifreq_storage;
-	} ifreq;
-	struct ifreq	*ifr = &ifreq.ifreq;
 
-	if (p == NULL) {
-		vlan_ether_purgemulti(ifv);
-		return;
-	} else if (ifp == p)
-		return;
-
-	LIST_FOREACH(mc, &ifv->vlan_mc_listhead, mc_entries) {
-		memcpy(&ifr->ifr_addr, &mc->mc_addr, mc->mc_addr.ss_len);
-	
-		/* Remove from the old parent */
-		memcpy(ifr->ifr_name, ifp->if_xname, IFNAMSIZ);
-		(void)(*ifp->if_ioctl)(ifp, SIOCDELMULTI, (caddr_t)ifr);
-
-		/* Try to add to the new parent */
-		memcpy(ifr->ifr_name, p->if_xname, IFNAMSIZ);
-		(void)(*p->if_ioctl)(p, SIOCADDMULTI, (caddr_t)ifr);
+	while ((mc = LIST_FIRST(&ifv->vlan_mc_listhead)) != NULL) {
+		LIST_REMOVE(mc, mc_entries);
+		free(mc, M_DEVBUF, sizeof(*mc));
 	}
 }
Index: if_vlan_var.h
===================================================================
RCS file: /cvs/src/sys/net/if_vlan_var.h,v
retrieving revision 1.31
diff -u -p -r1.31 if_vlan_var.h
--- if_vlan_var.h	3 Dec 2015 16:27:32 -0000	1.31
+++ if_vlan_var.h	4 Jan 2016 05:26:02 -0000
@@ -42,7 +42,11 @@ struct	ether_vlan_header {
 	u_int16_t evl_proto;
 };
 
-#define	EVL_VLID_MASK	0x0FFF
+#define	EVL_VLID_MASK	0xFFF
+#define	EVL_VLID_NONE	0x000
+/* 0x000 and 0xFFF are reserved */
+#define	EVL_VLID_MIN	0x001
+#define	EVL_VLID_MAX	0xFFE
 #define	EVL_VLANOFTAG(tag) ((tag) & EVL_VLID_MASK)
 #define	EVL_PRIOFTAG(tag) (((tag) >> EVL_PRIO_BITS) & 7)
 #define	EVL_ENCAPLEN	4	/* length in octets of encapsulation */
@@ -76,28 +80,26 @@ struct vlan_mc_entry {
 
 struct	ifvlan {
 	struct	arpcom ifv_ac;	/* make this an interface */
-	struct	ifnet *ifv_p;	/* parent interface of this vlan */
+	unsigned int ifv_p;	/* parent interface */
 	struct	ifv_linkmib {
-		int	ifvm_parent;
-		u_int16_t ifvm_proto; /* encapsulation ethertype */
 		u_int16_t ifvm_tag; /* tag to apply on packets leaving if */
 		u_int16_t ifvm_prio; /* prio to apply on packet leaving if */
 		u_int16_t ifvm_type; /* non-standard ethertype or 0x8100 */
 	}	ifv_mib;
 	LIST_HEAD(__vlan_mchead, vlan_mc_entry)	vlan_mc_listhead;
 	SRPL_ENTRY(ifvlan) ifv_list;
-	int ifv_flags;
+	int ifv_promisc;
 	struct refcnt ifv_refcnt;
 	void *lh_cookie;
 	void *dh_cookie;
-	struct ifih *ifv_ifih;
 };
 
 #define	ifv_if		ifv_ac.ac_if
 #define	ifv_tag		ifv_mib.ifvm_tag
 #define	ifv_prio	ifv_mib.ifvm_prio
 #define	ifv_type	ifv_mib.ifvm_type
-#define	IFVF_PROMISC	0x01
+
+int	vlan_inject(struct mbuf *, uint16_t, uint16_t);
 #endif /* _KERNEL */
 
 #endif /* _NET_IF_VLAN_VAR_H_ */
Index: if_mpw.c
===================================================================
RCS file: /cvs/src/sys/net/if_mpw.c,v
retrieving revision 1.12
diff -u -p -r1.12 if_mpw.c
--- if_mpw.c	5 Dec 2015 10:07:55 -0000	1.12
+++ if_mpw.c	4 Jan 2016 05:26:02 -0000
@@ -351,164 +351,83 @@ extern void vlan_start(struct ifnet *ifp
 struct mbuf *
 mpw_vlan_handle(struct mbuf *m, struct mpw_softc *sc)
 {
-	int needsdummy = 0;
-	int fakeifv = 0;
-	struct ifvlan *ifv = NULL;
-	struct ether_vlan_header *evh;
-	struct ifnet *ifp, *ifp0;
-	int nvlan, moff;
+	struct ifnet *p;
+	struct ifvlan *ifv;
 	struct ether_header eh;
-	struct ifvlan fifv;
-	struct vlan_shim {
-		uint16_t	vs_tpid;
-		uint16_t	vs_tci;
-	} vs;
-
-	ifp = ifp0 = if_get(m->m_pkthdr.ph_ifidx);
-	KASSERT(ifp != NULL);
-	if (ifp->if_start == vlan_start)
-		ifv = ifp->if_softc;
-
-	/* If we were relying on VLAN HW support, fake an ifv */
-	if (ifv == NULL && (m->m_flags & M_VLANTAG) == M_VLANTAG) {
-		memset(&fifv, 0, sizeof(fifv));
-		fifv.ifv_tag = EVL_VLANOFTAG(m->m_pkthdr.ether_vtag);
-		fifv.ifv_prio = EVL_PRIOFTAG(m->m_pkthdr.ether_vtag);
-		ifv = &fifv;
-		fakeifv = 1;
-	}
 
-	/*
-	 * Always remove VLAN flag as we are inserting them here. Also we
-	 * might get a tagged packet with no VLAN interface, in this case
-	 * we can't do anything.
-	 */
-	m->m_flags &= ~M_VLANTAG;
-
-	/*
-	 * Do VLAN managing.
-	 *
-	 * Case ethernet (raw):
-	 *  No VLAN: just pass it.
-	 *  One or more VLANs: insert VLAN tag back.
-	 *
-	 * NOTE: In case of raw access mode, the if_vlan will do the job
-	 * of dropping non tagged packets for us.
-	 */
-	if (sc->sc_type == IMR_TYPE_ETHERNET && ifv == NULL) {
-		if_put(ifp0);
-		return (m);
-	}
+	KASSERT(!ISSET(m->m_flags, M_VLANTAG));
 
 	/*
-	 * Case ethernet-tagged:
-	 *  0 VLAN: Drop packet
-	 *  1 VLAN: Tag packet with dummy VLAN
-	 *  >1 VLAN: Nothing
+	 * packet must have come from a local vlan interface so it can be
+	 * tagged across the tunnel.
 	 */
-	if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED && ifv == NULL) {
-		m_freem(m);
-		if_put(ifp0);
-		return (NULL);
-	}
+	p = if_get(m->m_pkthdr.ph_ifidx);
+	if (p == NULL)
+		goto drop;
 
-	/* Copy and remove ethernet header */
-	m_copydata(m, 0, sizeof(eh), (caddr_t) &eh);
-	if (ntohs(eh.ether_type) == ETHERTYPE_VLAN ||
-	    ntohs(eh.ether_type) == ETHERTYPE_QINQ)
-		m_adj(m, sizeof(*evh));
+	if (p->if_start == vlan_start && ISSET(p->if_flags, IFF_RUNNING))
+		ifv = p->if_softc;
 	else
-		m_adj(m, sizeof(eh));
-
-	/* Count VLAN stack size */
-	nvlan = 0;
-	while ((ifp = ifv->ifv_p) != NULL && ifp->if_start == vlan_start) {
-		ifv = ifp->if_softc;
-		nvlan++;
-	}
-	moff = sizeof(*evh) + (nvlan * EVL_ENCAPLEN);
+		goto put;
 
-	/* The mode ethernet tagged always need at least 2 VLANs */
-	if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED && nvlan == 0) {
-		needsdummy = 1;
-		moff += EVL_ENCAPLEN;
-	}
-
-	/* Add VLAN to the beginning of the packet */
-	M_PREPEND(m, moff, M_NOWAIT);
-	if (m == NULL) {
-		if_put(ifp0);
-		return (NULL);
-	}
+	/* Copy ethernet header */
+	m_copydata(m, 0, sizeof(eh), (caddr_t)&eh);
 
-	/* Copy original ethernet type */
-	moff -= sizeof(eh.ether_type);
-	m_copyback(m, moff, sizeof(eh.ether_type), &eh.ether_type, M_NOWAIT);
-
-	/* Fill inner VLAN values */
-	ifv = ifp0->if_softc;
-	while (nvlan-- > 0) {
-		vs.vs_tci = htons((ifv->ifv_prio << EVL_PRIO_BITS) +
-		    ifv->ifv_tag);
-		vs.vs_tpid = htons(ifv->ifv_type);
+	if (vlan_inject(m, ifv->ifv_type, ifv->ifv_tag) != 0)
+		goto put;
 
-		moff -= sizeof(vs);
-		m_copyback(m, moff, sizeof(vs), &vs, M_NOWAIT);
-
-		ifp = ifv->ifv_p;
-		ifv = ifp->if_softc;
-	}
+	switch (ntohs(eh.ether_type)) {
+	case ETHERTYPE_VLAN:
+	case ETHERTYPE_QINQ:
+		/* 2nd tag is already being tunnelled */
+		break;
 
-	/* Copy ethernet header back */
-	evh = mtod(m, struct ether_vlan_header *);
-	memcpy(evh->evl_dhost, eh.ether_dhost, sizeof(evh->evl_dhost));
-	memcpy(evh->evl_shost, eh.ether_shost, sizeof(evh->evl_shost));
-
-	if (fakeifv)
-		ifv = &fifv;
-
-	/* Insert the last VLAN and optionally a dummy VLAN */
-	if (needsdummy) {
-		evh->evl_encap_proto = ntohs(ETHERTYPE_QINQ);
-		evh->evl_tag = 0;
-
-		vs.vs_tci = ntohs((m->m_pkthdr.pf.prio << EVL_PRIO_BITS) +
-		    ifv->ifv_tag);
-		vs.vs_tpid = ntohs(ETHERTYPE_VLAN);
-		m_copyback(m, moff, sizeof(vs), &vs, M_NOWAIT);
-	} else {
-		evh->evl_encap_proto = (nvlan > 0) ?
-		    ntohs(ETHERTYPE_QINQ) : ntohs(ETHERTYPE_VLAN);
-		evh->evl_tag = ntohs((m->m_pkthdr.pf.prio << EVL_PRIO_BITS) +
-		    ifv->ifv_tag);
+	default:
+		/* inject dummy tag */
+		if (vlan_inject(m, ETHERTYPE_QINQ, 0) != 0)
+			goto put;
+		break;
 	}
 
-	if_put(ifp0);
+	if_put(p);
 
 	return (m);
+
+put:
+	if_put(p);
+drop:
+	m_freem(m);
+	return (NULL);
 }
 #endif /* NVLAN */
 
 void
-mpw_start(struct ifnet *ifp0)
+mpw_start(struct ifnet *ifp)
 {
-	struct mpw_softc *sc = ifp0->if_softc;
-	struct mbuf *m;
+	struct mpw_softc *sc = ifp->if_softc;
 	struct rtentry *rt;
-	struct ifnet *ifp;
+	struct ifnet *p;
+	struct mbuf *m;
 	struct shim_hdr *shim;
 	struct sockaddr_storage ss;
 
-	rt = rtalloc((struct sockaddr *) &sc->sc_nexthop, RT_RESOLVE, 0);
-	if (!rtisvalid(rt)) {
-		rtfree(rt);
+	if (!ISSET(ifp->if_flags, IFF_RUNNING) ||
+	    sc->sc_rshim.shim_label == 0 ||
+	    sc->sc_type == IMR_TYPE_NONE) {
+		IFQ_PURGE(&ifp->if_snd);
 		return;
 	}
 
-	ifp = if_get(rt->rt_ifidx);
-	if (ifp == NULL) {
-		rtfree(rt);
-		return;
+	rt = rtalloc((struct sockaddr *)&sc->sc_nexthop, RT_RESOLVE, 0);
+	if (!rtisvalid(rt)) {
+		IFQ_PURGE(&ifp->if_snd);
+		goto rtfree;
+	}
+
+	p = if_get(rt->rt_ifidx);
+	if (p == NULL) {
+		IFQ_PURGE(&ifp->if_snd);
+		goto rtfree;
 	}
 
 	/*
@@ -516,36 +435,27 @@ mpw_start(struct ifnet *ifp0)
 	 * the right place.
 	 */
 	memcpy(&ss, &sc->sc_nexthop, sizeof(sc->sc_nexthop));
-	((struct sockaddr *) &ss)->sa_family = AF_MPLS;
-
-	for (;;) {
-		IFQ_DEQUEUE(&ifp0->if_snd, m);
-		if (m == NULL)
-			break;
+	((struct sockaddr *)&ss)->sa_family = AF_MPLS;
 
-		if ((ifp0->if_flags & IFF_RUNNING) == 0 ||
-		    sc->sc_rshim.shim_label == 0 ||
-		    sc->sc_type == IMR_TYPE_NONE) {
-			m_freem(m);
-			continue;
-		}
+	while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) {
+#if NBPFILTER > 0
+		if (ifp->if_bpf)
+			bpf_mtap(ifp->if_bpf, m, BPF_DIRECTION_OUT);
+#endif /* NBPFILTER */
 
+		if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) {
 #if NVLAN > 0
-		m = mpw_vlan_handle(m, sc);
-		if (m == NULL)
-			continue;
+			m = mpw_vlan_handle(m, sc);
+			if (m == NULL) {
+				ifp->if_oerrors++;
+				continue;
+			}
 #else
-		/* Ethernet tagged doesn't work without VLANs'*/
-		if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) {
+			/* Ethernet tagged doesn't work without VLANs'*/
 			m_freem(m);
 			continue;
-		}
 #endif /* NVLAN */
-
-#if NBPFILTER > 0
-		if (sc->sc_if.if_bpf)
-			bpf_mtap(sc->sc_if.if_bpf, m, BPF_DIRECTION_OUT);
-#endif /* NBPFILTER */
+		}
 
 		if (sc->sc_flags & IMR_FLAG_CONTROLWORD) {
 			M_PREPEND(m, sizeof(*shim), M_NOWAIT);
@@ -567,9 +477,10 @@ mpw_start(struct ifnet *ifp0)
 		/* XXX: MPLS only uses domain 0 */
 		m->m_pkthdr.ph_rtableid = 0;
 
-		mpls_output(ifp, m, (struct sockaddr *) &ss, rt);
+		mpls_output(p, m, (struct sockaddr *)&ss, rt);
 	}
 
-	if_put(ifp);
+	if_put(p);
+rtfree:
 	rtfree(rt);
 }