Implemented MPLS pseudowire (mpw(4)) to be used with VPLS and VPWS.

ok mpi@, claudio@.

8 files changed, 639 insertions, 9 deletions

diff --git a/sys/conf/GENERIC b/sys/conf/GENERIC
index 07699d96ffe..84044cd67d8 100644
--- a/sys/conf/GENERIC
+++ b/sys/conf/GENERIC
@@ -1,4 +1,4 @@
-#	$OpenBSD: GENERIC,v 1.217 2015/07/17 22:52:29 tedu Exp $
+#	$OpenBSD: GENERIC,v 1.218 2015/07/20 22:16:41 rzalamena Exp $
 #
 #	Machine-independent option; used by all architectures for their
 #	GENERIC kernel
@@ -95,6 +95,7 @@ pseudo-device	gif		# IPv[46] over IPv[46] tunnel (RFC1933)
 pseudo-device	gre		# GRE encapsulation interface
 pseudo-device	loop		# network loopback
 pseudo-device	mpe		# MPLS PE interface
+pseudo-device	mpw		# MPLS pseudowire support
 pseudo-device	ppp		# PPP
 pseudo-device	pppoe		# PPP over Ethernet (RFC 2516)
 pseudo-device	pppx		# PPP multiplexer
diff --git a/sys/conf/files b/sys/conf/files
index 1a2d343b102..b1ae3a5a8df 100644
--- a/sys/conf/files
+++ b/sys/conf/files
@@ -1,4 +1,4 @@
-#	$OpenBSD: files,v 1.597 2015/07/19 02:35:35 deraadt Exp $
+#	$OpenBSD: files,v 1.598 2015/07/20 22:16:41 rzalamena Exp $
 #	$NetBSD: files,v 1.87 1996/05/19 17:17:50 jonathan Exp $
 
 #	@(#)files.newconf	7.5 (Berkeley) 5/10/93
@@ -543,6 +543,7 @@ pseudo-device gre: ifnet
 pseudo-device crypto: ifnet
 pseudo-device trunk: ifnet, ether, ifmedia
 pseudo-device mpe: ifnet, ether
+pseudo-device mpw: ifnet, ether
 pseudo-device vether: ifnet, ether
 pseudo-device pppx: ifnet
 pseudo-device vxlan: ifnet, ether, ifmedia
@@ -786,6 +787,7 @@ file net/if_gre.c			gre			needs-count
 file net/if_trunk.c			trunk			needs-count
 file net/trunklacp.c			trunk
 file net/if_mpe.c			mpe			needs-count
+file net/if_mpw.c			mpw & bridge		needs-count
 file net/if_vether.c			vether			needs-count
 file net/if_pppx.c			pppx			needs-count
 file net/if_vxlan.c			vxlan			needs-count
diff --git a/sys/net/if_bridge.c b/sys/net/if_bridge.c
index 1feead383c7..094aadb609a 100644
--- a/sys/net/if_bridge.c
+++ b/sys/net/if_bridge.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_bridge.c,v 1.255 2015/07/17 18:05:59 mpi Exp $	*/
+/*	$OpenBSD: if_bridge.c,v 1.256 2015/07/20 22:16:41 rzalamena Exp $	*/
 
 /*
  * Copyright (c) 1999, 2000 Jason L. Wright (jason@thought.net)
@@ -36,6 +36,7 @@
 #include "pf.h"
 #include "carp.h"
 #include "vlan.h"
+#include "mpw.h"
 
 #include <sys/param.h>
 #include <sys/systm.h>
@@ -386,6 +387,11 @@ bridge_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 			/* Nothing needed */
 		}
 #endif /* NGIF */
+#if NMPW > 0
+		else if (ifs->if_type == IFT_MPLSTUNNEL) {
+			/* Nothing needed */
+		}
+#endif /* NMPW */
 		else {
 			error = EINVAL;
 			break;
@@ -1032,7 +1038,15 @@ bridge_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
 			    (p->bif_flags & IFBIF_STP) &&
 			    (p->bif_state == BSTP_IFSTATE_DISCARDING))
 				continue;
-
+#if NMPW > 0
+			/*
+			 * Split horizon: avoid broadcasting messages from
+			 * wire to another wire.
+			 */
+			if (ifp->if_type == IFT_MPLSTUNNEL &&
+			    dst_if->if_type == IFT_MPLSTUNNEL)
+				continue;
+#endif /* NMPW */
 			if ((p->bif_flags & IFBIF_DISCOVER) == 0 &&
 			    (m->m_flags & (M_BCAST | M_MCAST)) == 0)
 				continue;
@@ -1480,7 +1494,15 @@ bridge_broadcast(struct bridge_softc *sc, struct ifnet *ifp,
 		 */
 		if (dst_if->if_index == ifp->if_index)
 			continue;
-
+#if NMPW > 0
+		/*
+		 * Split horizon: avoid broadcasting messages from wire to
+		 * another wire.
+		 */
+		if (ifp->if_type == IFT_MPLSTUNNEL &&
+		    dst_if->if_type == IFT_MPLSTUNNEL)
+			continue;
+#endif /* NMPW */
 		if (IF_QFULL(&dst_if->if_snd)) {
 			IF_DROP(&dst_if->if_snd);
 			sc->sc_if.if_oerrors++;
diff --git a/sys/net/if_mpw.c b/sys/net/if_mpw.c
new file mode 100644
index 00000000000..a6fb4721baa
--- /dev/null
+++ b/sys/net/if_mpw.c
@@ -0,0 +1,568 @@
+/*
+ * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "bpfilter.h"
+#include "vlan.h"
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/mbuf.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <sys/errno.h>
+
+#include <net/if.h>
+#include <net/if_types.h>
+#include <net/route.h>
+
+#include <netinet/in.h>
+
+#include <netinet/if_ether.h>
+#include <netmpls/mpls.h>
+
+#if NBPFILTER > 0
+#include <net/bpf.h>
+#endif /* NBPFILTER */
+
+#if NVLAN > 0
+#include <net/if_vlan_var.h>
+#endif
+
+struct mpw_softc {
+	struct		ifnet sc_if;
+
+	struct		ifaddr sc_ifa;
+	struct		sockaddr_mpls sc_smpls; /* Local label */
+
+	uint32_t	sc_flags;
+	uint32_t	sc_type;
+	struct		shim_hdr sc_rshim;
+	struct		sockaddr_storage sc_nexthop;
+};
+
+void	mpwattach(int);
+int	mpw_clone_create(struct if_clone *, int);
+int	mpw_clone_destroy(struct ifnet *);
+int	mpw_ioctl(struct ifnet *, u_long, caddr_t);
+int	mpw_output(struct ifnet *, struct mbuf *, struct sockaddr *,
+    struct rtentry *);
+void	mpw_start(struct ifnet *);
+int	mpw_input(struct ifnet *, struct mbuf *);
+#if NVLAN > 0
+struct	mbuf *mpw_vlan_handle(struct mbuf *, struct mpw_softc *);
+#endif /* NVLAN */
+
+struct if_clone mpw_cloner =
+    IF_CLONE_INITIALIZER("mpw", mpw_clone_create, mpw_clone_destroy);
+
+/* ARGSUSED */
+void
+mpwattach(int n)
+{
+	if_clone_attach(&mpw_cloner);
+}
+
+int
+mpw_clone_create(struct if_clone *ifc, int unit)
+{
+	struct mpw_softc *sc;
+	struct ifnet *ifp;
+	struct ifih *ifih;
+
+	sc = malloc(sizeof(*sc), M_DEVBUF, M_NOWAIT | M_ZERO);
+	if (sc == NULL)
+		return (ENOMEM);
+
+	ifih = malloc(sizeof(*ifih), M_DEVBUF, M_NOWAIT | M_ZERO);
+	if (ifih == NULL) {
+		free(sc, M_DEVBUF, sizeof(*sc));
+		return (ENOMEM);
+	}
+
+	ifp = &sc->sc_if;
+	snprintf(ifp->if_xname, sizeof(ifp->if_xname), "mpw%d", unit);
+	ifp->if_softc = sc;
+	ifp->if_mtu = ETHERMTU;
+	ifp->if_flags = IFF_POINTOPOINT;
+	ifp->if_ioctl = mpw_ioctl;
+	ifp->if_output = mpw_output;
+	ifp->if_start = mpw_start;
+	ifp->if_type = IFT_MPLSTUNNEL;
+	ifp->if_hdrlen = ETHER_HDR_LEN;
+	IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN);
+	IFQ_SET_READY(&ifp->if_snd);
+
+	if_attach(ifp);
+	if_alloc_sadl(ifp);
+
+	sc->sc_ifa.ifa_ifp = ifp;
+	sc->sc_ifa.ifa_rtrequest = link_rtrequest;
+	sc->sc_ifa.ifa_addr = (struct sockaddr *) ifp->if_sadl;
+	sc->sc_smpls.smpls_len = sizeof(sc->sc_smpls);
+	sc->sc_smpls.smpls_family = AF_MPLS;
+
+	ifih->ifih_input = mpw_input;
+	SLIST_INSERT_HEAD(&ifp->if_inputs, ifih, ifih_next);
+
+#if NBPFILTER > 0
+	bpfattach(&ifp->if_bpf, ifp, DLT_EN10MB, ETHER_HDR_LEN);
+#endif /* NBFILTER */
+
+	return (0);
+}
+
+int
+mpw_clone_destroy(struct ifnet *ifp)
+{
+	struct mpw_softc *sc = ifp->if_softc;
+	struct ifih *ifih = SLIST_FIRST(&ifp->if_inputs);
+	int s;
+
+	ifp->if_flags &= ~IFF_RUNNING;
+
+	if (sc->sc_smpls.smpls_label) {
+		s = splsoftnet();
+		rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_UP,
+		    smplstosa(&sc->sc_smpls));
+		splx(s);
+	}
+
+	SLIST_REMOVE(&ifp->if_inputs, ifih, ifih, ifih_next);
+	free(ifih, M_DEVBUF, sizeof(*ifih));
+
+	if_detach(ifp);
+	free(sc, M_DEVBUF, sizeof(*sc));
+
+	return (0);
+}
+
+int
+mpw_input(struct ifnet *ifp, struct mbuf *m)
+{
+	/* Don't have local broadcast. */
+	m_freem(m);
+	return (1);
+}
+
+int
+mpw_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
+{
+	struct ifreq *ifr = (struct ifreq *) data;
+	struct mpw_softc *sc = ifp->if_softc;
+	struct sockaddr_in *sin;
+	struct sockaddr_in *sin_nexthop;
+	int error = 0;
+	int s;
+	struct ifmpwreq imr;
+
+	switch (cmd) {
+	case SIOCSIFMTU:
+		if (ifr->ifr_mtu < MPE_MTU_MIN ||
+		    ifr->ifr_mtu > MPE_MTU_MAX)
+			error = EINVAL;
+		else
+			ifp->if_mtu = ifr->ifr_mtu;
+		break;
+
+	case SIOCSIFFLAGS:
+		if ((ifp->if_flags & IFF_UP))
+			ifp->if_flags |= IFF_RUNNING;
+		else
+			ifp->if_flags &= ~IFF_RUNNING;
+		break;
+
+	case SIOCSETMPWCFG:
+		error = suser(curproc, 0);
+		if (error != 0)
+			break;
+
+		error = copyin(ifr->ifr_data, &imr, sizeof(imr));
+		if (error != 0)
+			break;
+
+		/* Teardown all configuration if got no nexthop */
+		sin = (struct sockaddr_in *) &imr.imr_nexthop;
+		if (sin->sin_addr.s_addr == 0) {
+			s = splsoftnet();
+			if (rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_UP,
+			    smplstosa(&sc->sc_smpls)) == 0)
+				sc->sc_smpls.smpls_label = 0;
+			splx(s);
+
+			memset(&sc->sc_rshim, 0, sizeof(sc->sc_rshim));
+			memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop));
+			sc->sc_flags = 0;
+			sc->sc_type = 0;
+			break;
+		}
+
+		/* Validate input */
+		if (sin->sin_family != AF_INET ||
+		    imr.imr_lshim.shim_label > MPLS_LABEL_MAX ||
+		    imr.imr_lshim.shim_label <= MPLS_LABEL_RESERVED_MAX ||
+		    imr.imr_rshim.shim_label > MPLS_LABEL_MAX ||
+		    imr.imr_rshim.shim_label <= MPLS_LABEL_RESERVED_MAX) {
+			error = EINVAL;
+			break;
+		}
+
+		/* Setup labels and create inbound route */
+		imr.imr_lshim.shim_label =
+		    htonl(imr.imr_lshim.shim_label << MPLS_LABEL_OFFSET);
+		imr.imr_rshim.shim_label =
+		    htonl(imr.imr_rshim.shim_label << MPLS_LABEL_OFFSET);
+
+		if (sc->sc_smpls.smpls_label != imr.imr_lshim.shim_label) {
+			s = splsoftnet();
+			if (sc->sc_smpls.smpls_label)
+				rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_UP,
+				    smplstosa(&sc->sc_smpls));
+
+			sc->sc_smpls.smpls_label = imr.imr_lshim.shim_label;
+			error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS | RTF_UP,
+			    smplstosa(&sc->sc_smpls));
+			splx(s);
+			if (error != 0) {
+				sc->sc_smpls.smpls_label = 0;
+				break;
+			}
+		}
+
+		/* Apply configuration */
+		sc->sc_flags = imr.imr_flags;
+		sc->sc_type = imr.imr_type;
+		sc->sc_rshim.shim_label = imr.imr_rshim.shim_label;
+		sc->sc_rshim.shim_label |= MPLS_BOS_MASK;
+
+		memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop));
+		sin_nexthop = (struct sockaddr_in *) &sc->sc_nexthop;
+		sin_nexthop->sin_family = sin->sin_family;
+		sin_nexthop->sin_len = sizeof(struct sockaddr_in);
+		sin_nexthop->sin_addr.s_addr = sin->sin_addr.s_addr;
+		break;
+
+	case SIOCGETMPWCFG:
+		imr.imr_flags = sc->sc_flags;
+		imr.imr_type = sc->sc_type;
+		imr.imr_lshim.shim_label =
+		    ((ntohl(sc->sc_smpls.smpls_label & MPLS_LABEL_MASK)) >>
+			MPLS_LABEL_OFFSET);
+		imr.imr_rshim.shim_label =
+		    ((ntohl(sc->sc_rshim.shim_label & MPLS_LABEL_MASK)) >>
+			MPLS_LABEL_OFFSET);
+		memcpy(&imr.imr_nexthop, &sc->sc_nexthop,
+		    sizeof(imr.imr_nexthop));
+
+		error = copyout(&imr, ifr->ifr_data, sizeof(imr));
+		break;
+
+	default:
+		error = ENOTTY;
+		break;
+	}
+
+	return (error);
+}
+
+int
+mpw_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
+    struct rtentry *rt)
+{
+	struct mpw_softc *sc = ifp->if_softc;
+	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
+	struct ether_header *eh, ehc;
+	struct shim_hdr *shim;
+	int s;
+
+	if (sc->sc_type == IMR_TYPE_NONE) {
+		m_freem(m);
+		return (EHOSTUNREACH);
+	}
+
+	if (sc->sc_flags & IMR_FLAG_CONTROLWORD) {
+		shim = mtod(m, struct shim_hdr *);
+		m_adj(m, MPLS_HDRLEN);
+
+		/*
+		 * The first 4 bits identifies that this packet is a
+		 * control word. If the control word is configured and
+		 * we received an IP datagram we shall drop it.
+		 */
+		if (shim->shim_label & CW_ZERO_MASK) {
+			ifp->if_ierrors++;
+			m_freem(m);
+			return (EINVAL);
+		}
+
+		/* We don't support fragmentation just yet. */
+		if (shim->shim_label & CW_FRAG_MASK) {
+			ifp->if_ierrors++;
+			m_freem(m);
+			return (EINVAL);
+		}
+	}
+
+	if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) {
+		m_copydata(m, 0, sizeof(ehc), (caddr_t) &ehc);
+		m_adj(m, ETHER_HDR_LEN);
+
+		/* Ethernet tagged expects at least 2 VLANs */
+		if (ntohs(ehc.ether_type) != ETHERTYPE_QINQ) {
+			ifp->if_ierrors++;
+			m_freem(m);
+			return (EINVAL);
+		}
+
+		/* Remove dummy VLAN and update ethertype */
+		if (EVL_VLANOFTAG(*mtod(m, uint16_t *)) == 0) {
+			m_adj(m, EVL_ENCAPLEN);
+			ehc.ether_type = htons(ETHERTYPE_VLAN);
+		}
+
+		M_PREPEND(m, sizeof(*eh), M_NOWAIT);
+		if (m == NULL)
+			return (ENOMEM);
+
+		eh = mtod(m, struct ether_header *);
+		memcpy(eh, &ehc, sizeof(*eh));
+	}
+
+	ml_enqueue(&ml, m);
+
+	s = splnet();
+	if_input(ifp, &ml);
+	splx(s);
+
+	return (0);
+}
+
+#if NVLAN > 0
+extern void vlan_start(struct ifnet *ifp);
+
+/*
+ * This routine handles VLAN tag reinsertion in packets flowing through
+ * the pseudowire. Also it does the necessary modifications to the VLANs
+ * to respect the RFC.
+ */
+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 ether_header eh;
+	struct ifvlan fifv;
+	struct vlan_shim {
+		uint16_t	vs_tpid;
+		uint16_t	vs_tci;
+	} vs;
+
+	ifp0 = ifp = 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)
+		return (m);
+
+	/*
+	 * Case ethernet-tagged:
+	 *  0 VLAN: Drop packet
+	 *  1 VLAN: Tag packet with dummy VLAN
+	 *  >1 VLAN: Nothing
+	 */
+	if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED && ifv == NULL) {
+		m_freem(m);
+		return (NULL);
+	}
+
+	/* 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));
+	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);
+
+	/* 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)
+		return (NULL);
+
+	/* 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);
+
+		moff -= sizeof(vs);
+		m_copyback(m, moff, sizeof(vs), &vs, M_NOWAIT);
+
+		ifp = ifv->ifv_p;
+		ifv = ifp->if_softc;
+	}
+
+	/* 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);
+	}
+
+	return (m);
+}
+#endif /* NVLAN */
+
+void
+mpw_start(struct ifnet *ifp)
+{
+	struct mpw_softc *sc = ifp->if_softc;
+	struct mbuf *m;
+	struct rtentry *rt;
+	struct shim_hdr *shim;
+	struct sockaddr_storage ss;
+
+	rt = rtalloc((struct sockaddr *) &sc->sc_nexthop,
+	    RT_REPORT | RT_RESOLVE, 0);
+	if (rt == NULL)
+		return;
+
+	/*
+	 * XXX: lie about being MPLS, so mpls_output() get the TTL from
+	 * the right place.
+	 */
+	memcpy(&ss, &sc->sc_nexthop, sizeof(sc->sc_nexthop));
+	((struct sockaddr *) &ss)->sa_family = AF_MPLS;
+
+	for (;;) {
+		IF_DEQUEUE(&ifp->if_snd, m);
+		if (m == NULL)
+			break;
+
+		if ((ifp->if_flags & IFF_RUNNING) == 0 ||
+		    sc->sc_rshim.shim_label == 0 ||
+		    sc->sc_type == IMR_TYPE_NONE) {
+			m_freem(m);
+			continue;
+		}
+
+#if NVLAN > 0
+		m = mpw_vlan_handle(m, sc);
+		if (m == NULL)
+			continue;
+#else
+		/* Ethernet tagged doesn't work without VLANs'*/
+		if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) {
+			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);
+			if (m == NULL)
+				continue;
+
+			shim = mtod(m, struct shim_hdr *);
+			memset(shim, 0, sizeof(*shim));
+		}
+
+		M_PREPEND(m, sizeof(*shim), M_NOWAIT);
+		if (m == NULL)
+			continue;
+
+		shim = mtod(m, struct shim_hdr *);
+		shim->shim_label = htonl(mpls_defttl) & MPLS_TTL_MASK;
+		shim->shim_label |= sc->sc_rshim.shim_label;
+
+		/* XXX: MPLS only uses domain 0 */
+		m->m_pkthdr.ph_rtableid = 0;
+
+		mpls_output(rt->rt_ifp, m, (struct sockaddr *) &ss, rt);
+	}
+
+	rtfree(rt);
+}
diff --git a/sys/net/if_vlan.c b/sys/net/if_vlan.c
index 1146cc7e2b2..633c5d9ea07 100644
--- a/sys/net/if_vlan.c
+++ b/sys/net/if_vlan.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_vlan.c,v 1.134 2015/07/02 09:40:02 mpi Exp $	*/
+/*	$OpenBSD: if_vlan.c,v 1.135 2015/07/20 22:16:41 rzalamena Exp $	*/
 
 /*
  * Copyright 1998 Massachusetts Institute of Technology
@@ -47,6 +47,8 @@
  * will not modify the ethernet header.
  */
 
+#include "mpw.h"
+
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/malloc.h>
@@ -204,6 +206,16 @@ vlan_start(struct ifnet *ifp)
 		if (prio <= 1)
 			prio = !prio;
 
+#if NMPW > 0
+		struct ifnet *ifpn = if_get(m->m_pkthdr.ph_ifidx);
+		/*
+		 * If this packet came from a pseudowire it means it already
+		 * has all tags it needs, so just output it.
+		 */
+		if (ifpn && ifpn->if_type == IFT_MPLSTUNNEL) {
+			/* NOTHING */
+		} else
+#endif /* NMPW */
 		/*
 		 * If the underlying interface cannot do VLAN tag insertion
 		 * itself, create an encapsulation header.
diff --git a/sys/netmpls/mpls.h b/sys/netmpls/mpls.h
index a49e3ada76e..07231e0df47 100644
--- a/sys/netmpls/mpls.h
+++ b/sys/netmpls/mpls.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: mpls.h,v 1.32 2015/07/20 21:16:39 rzalamena Exp $	*/
+/*	$OpenBSD: mpls.h,v 1.33 2015/07/20 22:16:41 rzalamena Exp $	*/
 
 /*
  * Copyright (C) 1999, 2000 and 2001 AYAME Project, WIDE Project.
@@ -64,6 +64,9 @@ struct shim_hdr {
 #define MPLS_BOS_OFFSET		8
 #define MPLS_TTL_MASK		__MADDR(0x000000ffU)
 
+#define CW_ZERO_MASK		__MADDR(0xf0000000U)
+#define CW_FRAG_MASK		__MADDR(0x00300000U)
+
 #define MPLS_BOS_ISSET(l)	(((l) & MPLS_BOS_MASK) == MPLS_BOS_MASK)
 
 /* Reserved lavel values (RFC3032) */
@@ -132,6 +135,20 @@ struct rt_mpls {
 	&mpls_mapttl_ip6 \
 }
 
+#define IMR_TYPE_NONE			0
+#define IMR_TYPE_ETHERNET		1
+#define IMR_TYPE_ETHERNET_TAGGED	2
+
+#define IMR_FLAG_CONTROLWORD		0x1
+
+struct ifmpwreq {
+	uint32_t	imr_flags;
+	uint32_t	imr_type; /* pseudowire type */
+	struct		shim_hdr imr_lshim; /* local label */
+	struct		shim_hdr imr_rshim; /* remote label */
+	struct		sockaddr_storage imr_nexthop;
+};
+
 #endif
 
 #ifdef _KERNEL
diff --git a/sys/netmpls/mpls_input.c b/sys/netmpls/mpls_input.c
index ff87587f8e6..d1aa4ca2922 100644
--- a/sys/netmpls/mpls_input.c
+++ b/sys/netmpls/mpls_input.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: mpls_input.c,v 1.45 2015/07/20 21:16:39 rzalamena Exp $	*/
+/*	$OpenBSD: mpls_input.c,v 1.46 2015/07/20 22:16:41 rzalamena Exp $	*/
 
 /*
  * Copyright (c) 2008 Claudio Jeker <claudio@openbsd.org>
@@ -278,6 +278,11 @@ do_v6:
 				goto done;
 			}
 #endif
+			if (ifp->if_type == IFT_MPLSTUNNEL) {
+				ifp->if_output(ifp, m, rt_key(rt), rt);
+				goto done;
+			}
+
 			if (!rt->rt_gateway) {
 				m_freem(m);
 				goto done;
diff --git a/sys/sys/sockio.h b/sys/sys/sockio.h
index 437082b2623..11ef87427f3 100644
--- a/sys/sys/sockio.h
+++ b/sys/sys/sockio.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: sockio.h,v 1.57 2015/01/21 02:23:14 guenther Exp $	*/
+/*	$OpenBSD: sockio.h,v 1.58 2015/07/20 22:16:41 rzalamena Exp $	*/
 /*	$NetBSD: sockio.h,v 1.5 1995/08/23 00:40:47 thorpej Exp $	*/
 
 /*-
@@ -186,6 +186,9 @@
 #define SIOCSLIFPHYTTL	_IOW('i', 168, struct ifreq)	/* set tunnel ttl */
 #define SIOCGLIFPHYTTL	_IOWR('i', 169, struct ifreq)	/* get tunnel ttl */
 
+#define SIOCSETMPWCFG	_IOW('i', 170, struct ifreq) /* set mpw config */
+#define SIOCGETMPWCFG	_IOWR('i', 171, struct ifreq) /* get mpw config */
+
 #define	SIOCSVH		_IOWR('i', 245, struct ifreq)	/* set carp param */
 #define	SIOCGVH		_IOWR('i', 246, struct ifreq)	/* get carp param */