Home | History | Annotate | Line # | Download | only in net
if_mpls.c revision 1.30
      1 /*	$NetBSD: if_mpls.c,v 1.30 2017/10/23 09:32:00 msaitoh Exp $ */
      2 
      3 /*
      4  * Copyright (c) 2010 The NetBSD Foundation, Inc.
      5  * All rights reserved.
      6  *
      7  * This code is derived from software contributed to The NetBSD Foundation
      8  * by Mihai Chelaru <kefren (at) NetBSD.org>
      9  *
     10  * Redistribution and use in source and binary forms, with or without
     11  * modification, are permitted provided that the following conditions
     12  * are met:
     13  * 1. Redistributions of source code must retain the above copyright
     14  *    notice, this list of conditions and the following disclaimer.
     15  * 2. Redistributions in binary form must reproduce the above copyright
     16  *    notice, this list of conditions and the following disclaimer in the
     17  *    documentation and/or other materials provided with the distribution.
     18  *
     19  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
     20  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
     21  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
     22  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
     23  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
     24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
     25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
     26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
     27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
     28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
     29  * POSSIBILITY OF SUCH DAMAGE.
     30  */
     31 
     32 #include <sys/cdefs.h>
     33 __KERNEL_RCSID(0, "$NetBSD: if_mpls.c,v 1.30 2017/10/23 09:32:00 msaitoh Exp $");
     34 
     35 #ifdef _KERNEL_OPT
     36 #include "opt_inet.h"
     37 #include "opt_mpls.h"
     38 #endif
     39 
     40 #include <sys/param.h>
     41 
     42 #include <sys/errno.h>
     43 #include <sys/malloc.h>
     44 #include <sys/mbuf.h>
     45 #include <sys/sysctl.h>
     46 
     47 #include <net/bpf.h>
     48 #include <net/if.h>
     49 #include <net/if_types.h>
     50 #include <net/netisr.h>
     51 #include <net/route.h>
     52 #include <sys/device.h>
     53 #include <sys/module.h>
     54 #include <sys/atomic.h>
     55 
     56 #ifdef INET
     57 #include <netinet/in.h>
     58 #include <netinet/in_systm.h>
     59 #include <netinet/in_var.h>
     60 #include <netinet/ip.h>
     61 #include <netinet/ip_var.h>
     62 #endif
     63 
     64 #ifdef INET6
     65 #include <netinet/ip6.h>
     66 #include <netinet6/in6_var.h>
     67 #include <netinet6/ip6_var.h>
     68 #endif
     69 
     70 #include <netmpls/mpls.h>
     71 #include <netmpls/mpls_var.h>
     72 
     73 #include "if_mpls.h"
     74 
     75 #include "ioconf.h"
     76 
     77 #define TRIM_LABEL do { \
     78 	m_adj(m, sizeof(union mpls_shim)); \
     79 	if (m->m_len < sizeof(union mpls_shim) && \
     80 	    (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) \
     81 		goto done; \
     82 	dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); \
     83 	} while (/* CONSTCOND */ 0)
     84 
     85 
     86 static int mpls_clone_create(struct if_clone *, int);
     87 static int mpls_clone_destroy(struct ifnet *);
     88 
     89 static struct if_clone mpls_if_cloner =
     90 	IF_CLONE_INITIALIZER("mpls", mpls_clone_create, mpls_clone_destroy);
     91 
     92 
     93 static void mpls_input(struct ifnet *, struct mbuf *);
     94 static int mpls_output(struct ifnet *, struct mbuf *, const struct sockaddr *,
     95 	const struct rtentry *);
     96 static int mpls_ioctl(struct ifnet *, u_long, void *);
     97 static int mpls_send_frame(struct mbuf *, struct ifnet *,
     98     const struct rtentry *);
     99 static int mpls_lse(struct mbuf *);
    100 
    101 #ifdef INET
    102 static int mpls_unlabel_inet(struct mbuf *);
    103 static struct mbuf *mpls_label_inet(struct mbuf *, union mpls_shim *, uint);
    104 #endif
    105 
    106 #ifdef INET6
    107 static int mpls_unlabel_inet6(struct mbuf *);
    108 static struct mbuf *mpls_label_inet6(struct mbuf *, union mpls_shim *, uint);
    109 #endif
    110 
    111 static struct mbuf *mpls_prepend_shim(struct mbuf *, union mpls_shim *);
    112 
    113 extern int mpls_defttl, mpls_mapttl_inet, mpls_mapttl_inet6, mpls_icmp_respond,
    114     mpls_forwarding, mpls_frame_accept, mpls_mapprec_inet, mpls_mapclass_inet6,
    115     mpls_rfc4182;
    116 
    117 static u_int mpls_count;
    118 /* ARGSUSED */
    119 void
    120 mplsattach(int count)
    121 {
    122 	/*
    123 	 * Nothing to do here, initialization is handled by the
    124 	 * module initialization code in mplsinit() below).
    125 	 */
    126 }
    127 
    128 static void
    129 mplsinit(void)
    130 {
    131 	if_clone_attach(&mpls_if_cloner);
    132 }
    133 
    134 static int
    135 mplsdetach(void)
    136 {
    137 	int error = 0;
    138 
    139 	if (mpls_count != 0)
    140 		error = EBUSY;
    141 
    142 	if (error == 0)
    143 		if_clone_detach(&mpls_if_cloner);
    144 
    145 	return error;
    146 }
    147 
    148 static int
    149 mpls_clone_create(struct if_clone *ifc, int unit)
    150 {
    151 	struct mpls_softc *sc;
    152 	int rv;
    153 
    154 	atomic_inc_uint(&mpls_count);
    155 	sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO);
    156 
    157 	if_initname(&sc->sc_if, ifc->ifc_name, unit);
    158 	sc->sc_if.if_softc = sc;
    159 	sc->sc_if.if_type = IFT_MPLS;
    160 	sc->sc_if.if_addrlen = 0;
    161 	sc->sc_if.if_hdrlen = sizeof(union mpls_shim);
    162 	sc->sc_if.if_dlt = DLT_NULL;
    163 	sc->sc_if.if_mtu = 1500;
    164 	sc->sc_if.if_flags = 0;
    165 	sc->sc_if._if_input = mpls_input;
    166 	sc->sc_if.if_output = mpls_output;
    167 	sc->sc_if.if_ioctl = mpls_ioctl;
    168 
    169 	rv = if_attach(&sc->sc_if);
    170 	if (rv != 0) {
    171 		free(sc, M_DEVBUF);
    172 		atomic_dec_uint(&mpls_count);
    173 		return rv;
    174 	}
    175 	if_alloc_sadl(&sc->sc_if);
    176 	bpf_attach(&sc->sc_if, DLT_NULL, sizeof(uint32_t));
    177 	return 0;
    178 }
    179 
    180 static int
    181 mpls_clone_destroy(struct ifnet *ifp)
    182 {
    183 	int s;
    184 
    185 	bpf_detach(ifp);
    186 
    187 	s = splnet();
    188 	if_detach(ifp);
    189 	splx(s);
    190 
    191 	free(ifp->if_softc, M_DEVBUF);
    192 	atomic_dec_uint(&mpls_count);
    193 	return 0;
    194 }
    195 
    196 static void
    197 mpls_input(struct ifnet *ifp, struct mbuf *m)
    198 {
    199 #if 0
    200 	/*
    201 	 * TODO - kefren
    202 	 * I'd love to unshim the packet, guess family
    203 	 * and pass it to bpf
    204 	 */
    205 	bpf_mtap_af(ifp, AF_MPLS, m);
    206 #endif
    207 
    208 	mpls_lse(m);
    209 }
    210 
    211 void
    212 mplsintr(void)
    213 {
    214 
    215 	struct mbuf *m;
    216 
    217 	for (;;) {
    218 		IFQ_LOCK(&mplsintrq);
    219 		IF_DEQUEUE(&mplsintrq, m);
    220 		IFQ_UNLOCK(&mplsintrq);
    221 
    222 		if (!m)
    223 			return;
    224 
    225 		if (((m->m_flags & M_PKTHDR) == 0) ||
    226 		    (m->m_pkthdr.rcvif_index == 0))
    227 			panic("mplsintr(): no pkthdr or rcvif");
    228 
    229 #ifdef MBUFTRACE
    230 		m_claimm(m, &mpls_owner);
    231 #endif
    232 		mpls_input(m_get_rcvif_NOMPSAFE(m), m);
    233 	}
    234 }
    235 
    236 /*
    237  * prepend shim and deliver
    238  */
    239 static int
    240 mpls_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,
    241     const struct rtentry *rt)
    242 {
    243 	union mpls_shim mh, *pms;
    244 	struct rtentry *rt1;
    245 	int err;
    246 	uint psize = sizeof(struct sockaddr_mpls);
    247 
    248 	KASSERT(KERNEL_LOCKED_P());
    249 
    250 	if ((ifp->if_flags & (IFF_UP|IFF_RUNNING)) != (IFF_UP|IFF_RUNNING)) {
    251 		m_freem(m);
    252 		return ENETDOWN;
    253 	}
    254 
    255 	if (rt_gettag(rt) == NULL || rt_gettag(rt)->sa_family != AF_MPLS) {
    256 		m_freem(m);
    257 		return EINVAL;
    258 	}
    259 
    260 	bpf_mtap_af(ifp, dst->sa_family, m);
    261 
    262 	memset(&mh, 0, sizeof(mh));
    263 	mh.s_addr = MPLS_GETSADDR(rt);
    264 	mh.shim.bos = 1;
    265 	mh.shim.exp = 0;
    266 	mh.shim.ttl = mpls_defttl;
    267 
    268 	pms = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr;
    269 
    270 	while (psize <= rt_gettag(rt)->sa_len - sizeof(mh)) {
    271 		pms++;
    272 		if (mh.shim.label != MPLS_LABEL_IMPLNULL &&
    273 		    ((m = mpls_prepend_shim(m, &mh)) == NULL))
    274 			return ENOBUFS;
    275 		memset(&mh, 0, sizeof(mh));
    276 		mh.s_addr = ntohl(pms->s_addr);
    277 		mh.shim.bos = mh.shim.exp = 0;
    278 		mh.shim.ttl = mpls_defttl;
    279 		psize += sizeof(mh);
    280 	}
    281 
    282 	switch(dst->sa_family) {
    283 #ifdef INET
    284 	case AF_INET:
    285 		m = mpls_label_inet(m, &mh, psize - sizeof(struct sockaddr_mpls));
    286 		break;
    287 #endif
    288 #ifdef INET6
    289 	case AF_INET6:
    290 		m = mpls_label_inet6(m, &mh, psize - sizeof(struct sockaddr_mpls));
    291 		break;
    292 #endif
    293 	default:
    294 		m = mpls_prepend_shim(m, &mh);
    295 		break;
    296 	}
    297 
    298 	if (m == NULL) {
    299 		IF_DROP(&ifp->if_snd);
    300 		ifp->if_oerrors++;
    301 		return ENOBUFS;
    302 	}
    303 
    304 	ifp->if_opackets++;
    305 	ifp->if_obytes += m->m_pkthdr.len;
    306 
    307 	if ((rt1=rtalloc1(rt->rt_gateway, 1)) == NULL) {
    308 		m_freem(m);
    309 		return EHOSTUNREACH;
    310 	}
    311 
    312 	err = mpls_send_frame(m, rt1->rt_ifp, rt);
    313 	rt_unref(rt1);
    314 	return err;
    315 }
    316 
    317 static int
    318 mpls_ioctl(struct ifnet *ifp, u_long cmd, void *data)
    319 {
    320 	int error = 0, s = splnet();
    321 	struct ifreq *ifr = data;
    322 
    323 	switch(cmd) {
    324 	case SIOCINITIFADDR:
    325 		ifp->if_flags |= IFF_UP | IFF_RUNNING;
    326 		break;
    327 	case SIOCSIFMTU:
    328 		if (ifr != NULL && ifr->ifr_mtu < 576) {
    329 			error = EINVAL;
    330 			break;
    331 		}
    332 		/* FALLTHROUGH */
    333 	case SIOCGIFMTU:
    334 		if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET)
    335 			error = 0;
    336 		break;
    337 	case SIOCSIFFLAGS:
    338 		if ((error = ifioctl_common(ifp, cmd, data)) != 0)
    339 			break;
    340 		if (ifp->if_flags & IFF_UP)
    341 			ifp->if_flags |= IFF_RUNNING;
    342 		break;
    343 	default:
    344 		error = ifioctl_common(ifp, cmd, data);
    345 		break;
    346 	}
    347 	splx(s);
    348 	return error;
    349 }
    350 
    351 /*
    352  * MPLS Label Switch Engine
    353  */
    354 static int
    355 mpls_lse(struct mbuf *m)
    356 {
    357 	struct sockaddr_mpls dst;
    358 	union mpls_shim tshim, *htag;
    359 	struct rtentry *rt = NULL;
    360 	int error = ENOBUFS;
    361 	uint psize = sizeof(struct sockaddr_mpls);
    362 	bool push_back_alert = false;
    363 
    364 	if (m->m_len < sizeof(union mpls_shim) &&
    365 	    (m = m_pullup(m, sizeof(union mpls_shim))) == NULL)
    366 		goto done;
    367 
    368 	dst.smpls_len = sizeof(struct sockaddr_mpls);
    369 	dst.smpls_family = AF_MPLS;
    370 	dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr);
    371 
    372 	/* Check if we're accepting MPLS Frames */
    373 	error = EINVAL;
    374 	if (!mpls_frame_accept)
    375 		goto done;
    376 
    377 	/* TTL decrement */
    378 	if ((m = mpls_ttl_dec(m)) == NULL)
    379 		goto done;
    380 
    381 	/* RFC 4182 */
    382 	if (mpls_rfc4182 != 0)
    383 		while((dst.smpls_addr.shim.label == MPLS_LABEL_IPV4NULL ||
    384 		    dst.smpls_addr.shim.label == MPLS_LABEL_IPV6NULL) &&
    385 		    __predict_false(dst.smpls_addr.shim.bos == 0))
    386 			TRIM_LABEL;
    387 
    388 	/* RFC 3032 Section 2.1 Page 4 */
    389 	if (__predict_false(dst.smpls_addr.shim.label == MPLS_LABEL_RTALERT) &&
    390 	    dst.smpls_addr.shim.bos == 0) {
    391 		TRIM_LABEL;
    392 		push_back_alert = true;
    393 	}
    394 
    395 	if (dst.smpls_addr.shim.label <= MPLS_LABEL_RESMAX) {
    396 		/* Don't swap reserved labels */
    397 		switch (dst.smpls_addr.shim.label) {
    398 #ifdef INET
    399 		case MPLS_LABEL_IPV4NULL:
    400 			/* Pop shim and push mbuf to IP stack */
    401 			if (dst.smpls_addr.shim.bos)
    402 				error = mpls_unlabel_inet(m);
    403 			break;
    404 #endif
    405 #ifdef INET6
    406 		case MPLS_LABEL_IPV6NULL:
    407 			/* Pop shim and push mbuf to IPv6 stack */
    408 			if (dst.smpls_addr.shim.bos)
    409 				error = mpls_unlabel_inet6(m);
    410 			break;
    411 #endif
    412 		case MPLS_LABEL_RTALERT:	/* Yeah, I'm all alerted */
    413 		case MPLS_LABEL_IMPLNULL:	/* This is logical only */
    414 		default:			/* Rest are not allowed */
    415 			break;
    416 		}
    417 		goto done;
    418 	}
    419 
    420 	/* Check if we should do MPLS forwarding */
    421 	error = EHOSTUNREACH;
    422 	if (!mpls_forwarding)
    423 		goto done;
    424 
    425 	/* Get a route to dst */
    426 	dst.smpls_addr.shim.ttl =
    427 	    dst.smpls_addr.shim.bos =
    428 	    dst.smpls_addr.shim.exp = 0;
    429 	dst.smpls_addr.s_addr = htonl(dst.smpls_addr.s_addr);
    430 	if ((rt = rtalloc1((const struct sockaddr*)&dst, 1)) == NULL)
    431 		goto done;
    432 
    433 	/* MPLS packet with no MPLS tagged route ? */
    434 	if ((rt->rt_flags & RTF_GATEWAY) == 0 ||
    435 	     rt_gettag(rt) == NULL ||
    436 	     rt_gettag(rt)->sa_family != AF_MPLS)
    437 		goto done;
    438 
    439 	tshim.s_addr = MPLS_GETSADDR(rt);
    440 
    441 	/* Swap labels */
    442 	if ((m->m_len < sizeof(union mpls_shim)) &&
    443 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0) {
    444 		error = ENOBUFS;
    445 		goto done;
    446 	}
    447 
    448 	/* Replace only the label */
    449 	htag = mtod(m, union mpls_shim *);
    450 	htag->s_addr = ntohl(htag->s_addr);
    451 	htag->shim.label = tshim.shim.label;
    452 	htag->s_addr = htonl(htag->s_addr);
    453 
    454 	/* check if there is anything more to prepend */
    455 	htag = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr;
    456 	while (psize <= rt_gettag(rt)->sa_len - sizeof(tshim)) {
    457 		htag++;
    458 		memset(&tshim, 0, sizeof(tshim));
    459 		tshim.s_addr = ntohl(htag->s_addr);
    460 		tshim.shim.bos = tshim.shim.exp = 0;
    461 		tshim.shim.ttl = mpls_defttl;
    462 		if (tshim.shim.label != MPLS_LABEL_IMPLNULL &&
    463 		    ((m = mpls_prepend_shim(m, &tshim)) == NULL))
    464 			return ENOBUFS;
    465 		psize += sizeof(tshim);
    466 	}
    467 
    468 	if (__predict_false(push_back_alert == true)) {
    469 		/* re-add the router alert label */
    470 		memset(&tshim, 0, sizeof(tshim));
    471 		tshim.s_addr = MPLS_LABEL_RTALERT;
    472 		tshim.shim.bos = tshim.shim.exp = 0;
    473 		tshim.shim.ttl = mpls_defttl;
    474 		if ((m = mpls_prepend_shim(m, &tshim)) == NULL)
    475 			return ENOBUFS;
    476 	}
    477 
    478 	if ((rt->rt_flags & RTF_GATEWAY) == 0) {
    479 		error = EHOSTUNREACH;
    480 		goto done;
    481 	}
    482 
    483 	rt->rt_use++;
    484 	error = mpls_send_frame(m, rt->rt_ifp, rt);
    485 
    486 done:
    487 	if (error != 0 && m != NULL)
    488 		m_freem(m);
    489 	if (rt != NULL)
    490 		rt_unref(rt);
    491 
    492 	return error;
    493 }
    494 
    495 static int
    496 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, const struct rtentry *rt)
    497 {
    498 	union mpls_shim msh;
    499 	int ret;
    500 
    501 	msh.s_addr = MPLS_GETSADDR(rt);
    502 	if (msh.shim.label == MPLS_LABEL_IMPLNULL ||
    503 	    (m->m_flags & (M_MCAST | M_BCAST))) {
    504 		m_adj(m, sizeof(union mpls_shim));
    505 		m->m_pkthdr.csum_flags = 0;
    506 	}
    507 
    508 	switch(ifp->if_type) {
    509 	/* only these are supported for now */
    510 	case IFT_ETHER:
    511 	case IFT_TUNNEL:
    512 	case IFT_LOOP:
    513 #ifdef INET
    514 		ret = ip_if_output(ifp, m, rt->rt_gateway, rt);
    515 #else
    516 		ret = if_output_lock(ifp, ifp, m, rt->rt_gateway, rt);
    517 #endif
    518 		return ret;
    519 		break;
    520 	default:
    521 		return ENETUNREACH;
    522 	}
    523 	return 0;
    524 }
    525 
    526 
    527 
    528 #ifdef INET
    529 static int
    530 mpls_unlabel_inet(struct mbuf *m)
    531 {
    532 	struct ip *iph;
    533 	union mpls_shim *ms;
    534 	int iphlen;
    535 
    536 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    537 
    538 		/* get shim info */
    539 		ms = mtod(m, union mpls_shim *);
    540 		ms->s_addr = ntohl(ms->s_addr);
    541 
    542 		/* and get rid of it */
    543 		m_adj(m, sizeof(union mpls_shim));
    544 
    545 		/* get ip header */
    546 		if (m->m_len < sizeof (struct ip) &&
    547 		    (m = m_pullup(m, sizeof(struct ip))) == NULL)
    548 			return ENOBUFS;
    549 		iph = mtod(m, struct ip *);
    550 		iphlen = iph->ip_hl << 2;
    551 
    552 		/* get it all */
    553 		if (m->m_len < iphlen) {
    554 			if ((m = m_pullup(m, iphlen)) == NULL)
    555 				return ENOBUFS;
    556 			iph = mtod(m, struct ip *);
    557 		}
    558 
    559 		/* check ipsum */
    560 		if (in_cksum(m, iphlen) != 0) {
    561 			m_freem(m);
    562 			return EINVAL;
    563 		}
    564 
    565 		/* set IP ttl from MPLS ttl */
    566 		if (mpls_mapttl_inet)
    567 			iph->ip_ttl = ms->shim.ttl;
    568 
    569 		/* set IP Precedence from MPLS Exp */
    570 		if (mpls_mapprec_inet) {
    571 			iph->ip_tos = (iph->ip_tos << 3) >> 3;
    572 			iph->ip_tos |= ms->shim.exp << 5;
    573 		}
    574 
    575 		/* reset ipsum because we modified TTL and TOS */
    576 		iph->ip_sum = 0;
    577 		iph->ip_sum = in_cksum(m, iphlen);
    578 	} else
    579 		m_adj(m, sizeof(union mpls_shim));
    580 
    581 	/* Put it on IP queue */
    582 	if (__predict_false(!pktq_enqueue(ip_pktq, m, 0))) {
    583 		m_freem(m);
    584 		return ENOBUFS;
    585 	}
    586 	return 0;
    587 }
    588 
    589 /*
    590  * Prepend MPLS label
    591  */
    592 static struct mbuf *
    593 mpls_label_inet(struct mbuf *m, union mpls_shim *ms, uint offset)
    594 {
    595 	struct ip iphdr;
    596 
    597 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    598 		if ((m->m_len < sizeof(struct ip)) &&
    599 		    (m = m_pullup(m, offset + sizeof(struct ip))) == 0)
    600 			return NULL; /* XXX */
    601 		m_copydata(m, offset, sizeof(struct ip), &iphdr);
    602 
    603 		/* Map TTL */
    604 		if (mpls_mapttl_inet)
    605 			ms->shim.ttl = iphdr.ip_ttl;
    606 
    607 		/* Copy IP precedence to EXP */
    608 		if (mpls_mapprec_inet)
    609 			ms->shim.exp = ((u_int8_t)iphdr.ip_tos) >> 5;
    610 	}
    611 
    612 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    613 		return NULL;
    614 
    615 	return m;
    616 }
    617 
    618 #endif	/* INET */
    619 
    620 #ifdef INET6
    621 
    622 static int
    623 mpls_unlabel_inet6(struct mbuf *m)
    624 {
    625 	struct ip6_hdr *ip6hdr;
    626 	union mpls_shim ms;
    627 
    628 	/* TODO: mapclass */
    629 	if (mpls_mapttl_inet6) {
    630 		ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr);
    631 		m_adj(m, sizeof(union mpls_shim));
    632 
    633 		if (m->m_len < sizeof (struct ip6_hdr) &&
    634 		    (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0)
    635 			return ENOBUFS;
    636 		ip6hdr = mtod(m, struct ip6_hdr *);
    637 
    638 		/* Because we just decremented this in mpls_lse */
    639 		ip6hdr->ip6_hlim = ms.shim.ttl + 1;
    640 	} else
    641 		m_adj(m, sizeof(union mpls_shim));
    642 
    643 	/* Put it back on IPv6 queue. */
    644 	if (__predict_false(!pktq_enqueue(ip6_pktq, m, 0))) {
    645 		m_freem(m);
    646 		return ENOBUFS;
    647 	}
    648 	return 0;
    649 }
    650 
    651 static struct mbuf *
    652 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms, uint offset)
    653 {
    654 	struct ip6_hdr ip6h;
    655 
    656 	if (mpls_mapttl_inet6 || mpls_mapclass_inet6) {
    657 		if (m->m_len < sizeof(struct ip6_hdr) &&
    658 		    (m = m_pullup(m, offset + sizeof(struct ip6_hdr))) == 0)
    659 			return NULL;
    660 		m_copydata(m, offset, sizeof(struct ip6_hdr), &ip6h);
    661 
    662 		if (mpls_mapttl_inet6)
    663 			ms->shim.ttl = ip6h.ip6_hlim;
    664 
    665 		if (mpls_mapclass_inet6)
    666 			ms->shim.exp = ip6h.ip6_vfc << 1 >> 5;
    667 	}
    668 
    669 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    670 		return NULL;
    671 
    672 	return m;
    673 }
    674 
    675 #endif	/* INET6 */
    676 
    677 static struct mbuf *
    678 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms)
    679 {
    680 	union mpls_shim *shim;
    681 
    682 	M_PREPEND(m, sizeof(*ms), M_DONTWAIT);
    683 	if (m == NULL)
    684 		return NULL;
    685 
    686 	if (m->m_len < sizeof(union mpls_shim) &&
    687 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0)
    688 		return NULL;
    689 
    690 	shim = mtod(m, union mpls_shim *);
    691 
    692 	memcpy(shim, ms, sizeof(*shim));
    693 	shim->s_addr = htonl(shim->s_addr);
    694 
    695 	return m;
    696 }
    697 
    698 /*
    699  * Module infrastructure
    700  */
    701 #include "if_module.h"
    702 
    703 IF_MODULE(MODULE_CLASS_DRIVER, mpls, "")
    704