Home | History | Annotate | Line # | Download | only in net
if_mpls.c revision 1.28
      1 /*	$NetBSD: if_mpls.c,v 1.28 2016/10/03 11:06:06 ozaki-r 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.28 2016/10/03 11:06:06 ozaki-r 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 
    153 	atomic_inc_uint(&mpls_count);
    154 	sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO);
    155 
    156 	if_initname(&sc->sc_if, ifc->ifc_name, unit);
    157 	sc->sc_if.if_softc = sc;
    158 	sc->sc_if.if_type = IFT_MPLS;
    159 	sc->sc_if.if_addrlen = 0;
    160 	sc->sc_if.if_hdrlen = sizeof(union mpls_shim);
    161 	sc->sc_if.if_dlt = DLT_NULL;
    162 	sc->sc_if.if_mtu = 1500;
    163 	sc->sc_if.if_flags = 0;
    164 	sc->sc_if._if_input = mpls_input;
    165 	sc->sc_if.if_output = mpls_output;
    166 	sc->sc_if.if_ioctl = mpls_ioctl;
    167 
    168 	if_attach(&sc->sc_if);
    169 	if_alloc_sadl(&sc->sc_if);
    170 	bpf_attach(&sc->sc_if, DLT_NULL, sizeof(uint32_t));
    171 	return 0;
    172 }
    173 
    174 static int
    175 mpls_clone_destroy(struct ifnet *ifp)
    176 {
    177 	int s;
    178 
    179 	bpf_detach(ifp);
    180 
    181 	s = splnet();
    182 	if_detach(ifp);
    183 	splx(s);
    184 
    185 	free(ifp->if_softc, M_DEVBUF);
    186 	atomic_dec_uint(&mpls_count);
    187 	return 0;
    188 }
    189 
    190 static void
    191 mpls_input(struct ifnet *ifp, struct mbuf *m)
    192 {
    193 #if 0
    194 	/*
    195 	 * TODO - kefren
    196 	 * I'd love to unshim the packet, guess family
    197 	 * and pass it to bpf
    198 	 */
    199 	bpf_mtap_af(ifp, AF_MPLS, m);
    200 #endif
    201 
    202 	mpls_lse(m);
    203 }
    204 
    205 void
    206 mplsintr(void)
    207 {
    208 
    209 	struct mbuf *m;
    210 
    211 	for (;;) {
    212 		IFQ_LOCK(&mplsintrq);
    213 		IF_DEQUEUE(&mplsintrq, m);
    214 		IFQ_UNLOCK(&mplsintrq);
    215 
    216 		if (!m)
    217 			return;
    218 
    219 		if (((m->m_flags & M_PKTHDR) == 0) ||
    220 		    (m->m_pkthdr.rcvif_index == 0))
    221 			panic("mplsintr(): no pkthdr or rcvif");
    222 
    223 #ifdef MBUFTRACE
    224 		m_claimm(m, &mpls_owner);
    225 #endif
    226 		mpls_input(m_get_rcvif_NOMPSAFE(m), m);
    227 	}
    228 }
    229 
    230 /*
    231  * prepend shim and deliver
    232  */
    233 static int
    234 mpls_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,
    235     const struct rtentry *rt)
    236 {
    237 	union mpls_shim mh, *pms;
    238 	struct rtentry *rt1;
    239 	int err;
    240 	uint psize = sizeof(struct sockaddr_mpls);
    241 
    242 	KASSERT(KERNEL_LOCKED_P());
    243 
    244 	if ((ifp->if_flags & (IFF_UP|IFF_RUNNING)) != (IFF_UP|IFF_RUNNING)) {
    245 		m_freem(m);
    246 		return ENETDOWN;
    247 	}
    248 
    249 	if (rt_gettag(rt) == NULL || rt_gettag(rt)->sa_family != AF_MPLS) {
    250 		m_freem(m);
    251 		return EINVAL;
    252 	}
    253 
    254 	bpf_mtap_af(ifp, dst->sa_family, m);
    255 
    256 	memset(&mh, 0, sizeof(mh));
    257 	mh.s_addr = MPLS_GETSADDR(rt);
    258 	mh.shim.bos = 1;
    259 	mh.shim.exp = 0;
    260 	mh.shim.ttl = mpls_defttl;
    261 
    262 	pms = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr;
    263 
    264 	while (psize <= rt_gettag(rt)->sa_len - sizeof(mh)) {
    265 		pms++;
    266 		if (mh.shim.label != MPLS_LABEL_IMPLNULL &&
    267 		    ((m = mpls_prepend_shim(m, &mh)) == NULL))
    268 			return ENOBUFS;
    269 		memset(&mh, 0, sizeof(mh));
    270 		mh.s_addr = ntohl(pms->s_addr);
    271 		mh.shim.bos = mh.shim.exp = 0;
    272 		mh.shim.ttl = mpls_defttl;
    273 		psize += sizeof(mh);
    274 	}
    275 
    276 	switch(dst->sa_family) {
    277 #ifdef INET
    278 	case AF_INET:
    279 		m = mpls_label_inet(m, &mh, psize - sizeof(struct sockaddr_mpls));
    280 		break;
    281 #endif
    282 #ifdef INET6
    283 	case AF_INET6:
    284 		m = mpls_label_inet6(m, &mh, psize - sizeof(struct sockaddr_mpls));
    285 		break;
    286 #endif
    287 	default:
    288 		m = mpls_prepend_shim(m, &mh);
    289 		break;
    290 	}
    291 
    292 	if (m == NULL) {
    293 		IF_DROP(&ifp->if_snd);
    294 		ifp->if_oerrors++;
    295 		return ENOBUFS;
    296 	}
    297 
    298 	ifp->if_opackets++;
    299 	ifp->if_obytes += m->m_pkthdr.len;
    300 
    301 	if ((rt1=rtalloc1(rt->rt_gateway, 1)) == NULL) {
    302 		m_freem(m);
    303 		return EHOSTUNREACH;
    304 	}
    305 
    306 	err = mpls_send_frame(m, rt1->rt_ifp, rt);
    307 	rtfree(rt1);
    308 	return err;
    309 }
    310 
    311 static int
    312 mpls_ioctl(struct ifnet *ifp, u_long cmd, void *data)
    313 {
    314 	int error = 0, s = splnet();
    315 	struct ifreq *ifr = data;
    316 
    317 	switch(cmd) {
    318 	case SIOCINITIFADDR:
    319 		ifp->if_flags |= IFF_UP | IFF_RUNNING;
    320 		break;
    321 	case SIOCSIFMTU:
    322 		if (ifr != NULL && ifr->ifr_mtu < 576) {
    323 			error = EINVAL;
    324 			break;
    325 		}
    326 		/* FALLTHROUGH */
    327 	case SIOCGIFMTU:
    328 		if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET)
    329 			error = 0;
    330 		break;
    331 	case SIOCSIFFLAGS:
    332 		if ((error = ifioctl_common(ifp, cmd, data)) != 0)
    333 			break;
    334 		if (ifp->if_flags & IFF_UP)
    335 			ifp->if_flags |= IFF_RUNNING;
    336 		break;
    337 	default:
    338 		error = ifioctl_common(ifp, cmd, data);
    339 		break;
    340 	}
    341 	splx(s);
    342 	return error;
    343 }
    344 
    345 /*
    346  * MPLS Label Switch Engine
    347  */
    348 static int
    349 mpls_lse(struct mbuf *m)
    350 {
    351 	struct sockaddr_mpls dst;
    352 	union mpls_shim tshim, *htag;
    353 	struct rtentry *rt = NULL;
    354 	int error = ENOBUFS;
    355 	uint psize = sizeof(struct sockaddr_mpls);
    356 	bool push_back_alert = false;
    357 
    358 	if (m->m_len < sizeof(union mpls_shim) &&
    359 	    (m = m_pullup(m, sizeof(union mpls_shim))) == NULL)
    360 		goto done;
    361 
    362 	dst.smpls_len = sizeof(struct sockaddr_mpls);
    363 	dst.smpls_family = AF_MPLS;
    364 	dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr);
    365 
    366 	/* Check if we're accepting MPLS Frames */
    367 	error = EINVAL;
    368 	if (!mpls_frame_accept)
    369 		goto done;
    370 
    371 	/* TTL decrement */
    372 	if ((m = mpls_ttl_dec(m)) == NULL)
    373 		goto done;
    374 
    375 	/* RFC 4182 */
    376 	if (mpls_rfc4182 != 0)
    377 		while((dst.smpls_addr.shim.label == MPLS_LABEL_IPV4NULL ||
    378 		    dst.smpls_addr.shim.label == MPLS_LABEL_IPV6NULL) &&
    379 		    __predict_false(dst.smpls_addr.shim.bos == 0))
    380 			TRIM_LABEL;
    381 
    382 	/* RFC 3032 Section 2.1 Page 4 */
    383 	if (__predict_false(dst.smpls_addr.shim.label == MPLS_LABEL_RTALERT) &&
    384 	    dst.smpls_addr.shim.bos == 0) {
    385 		TRIM_LABEL;
    386 		push_back_alert = true;
    387 	}
    388 
    389 	if (dst.smpls_addr.shim.label <= MPLS_LABEL_RESMAX) {
    390 		/* Don't swap reserved labels */
    391 		switch (dst.smpls_addr.shim.label) {
    392 #ifdef INET
    393 		case MPLS_LABEL_IPV4NULL:
    394 			/* Pop shim and push mbuf to IP stack */
    395 			if (dst.smpls_addr.shim.bos)
    396 				error = mpls_unlabel_inet(m);
    397 			break;
    398 #endif
    399 #ifdef INET6
    400 		case MPLS_LABEL_IPV6NULL:
    401 			/* Pop shim and push mbuf to IPv6 stack */
    402 			if (dst.smpls_addr.shim.bos)
    403 				error = mpls_unlabel_inet6(m);
    404 			break;
    405 #endif
    406 		case MPLS_LABEL_RTALERT:	/* Yeah, I'm all alerted */
    407 		case MPLS_LABEL_IMPLNULL:	/* This is logical only */
    408 		default:			/* Rest are not allowed */
    409 			break;
    410 		}
    411 		goto done;
    412 	}
    413 
    414 	/* Check if we should do MPLS forwarding */
    415 	error = EHOSTUNREACH;
    416 	if (!mpls_forwarding)
    417 		goto done;
    418 
    419 	/* Get a route to dst */
    420 	dst.smpls_addr.shim.ttl =
    421 	    dst.smpls_addr.shim.bos =
    422 	    dst.smpls_addr.shim.exp = 0;
    423 	dst.smpls_addr.s_addr = htonl(dst.smpls_addr.s_addr);
    424 	if ((rt = rtalloc1((const struct sockaddr*)&dst, 1)) == NULL)
    425 		goto done;
    426 
    427 	/* MPLS packet with no MPLS tagged route ? */
    428 	if ((rt->rt_flags & RTF_GATEWAY) == 0 ||
    429 	     rt_gettag(rt) == NULL ||
    430 	     rt_gettag(rt)->sa_family != AF_MPLS)
    431 		goto done;
    432 
    433 	tshim.s_addr = MPLS_GETSADDR(rt);
    434 
    435 	/* Swap labels */
    436 	if ((m->m_len < sizeof(union mpls_shim)) &&
    437 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0) {
    438 		error = ENOBUFS;
    439 		goto done;
    440 	}
    441 
    442 	/* Replace only the label */
    443 	htag = mtod(m, union mpls_shim *);
    444 	htag->s_addr = ntohl(htag->s_addr);
    445 	htag->shim.label = tshim.shim.label;
    446 	htag->s_addr = htonl(htag->s_addr);
    447 
    448 	/* check if there is anything more to prepend */
    449 	htag = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr;
    450 	while (psize <= rt_gettag(rt)->sa_len - sizeof(tshim)) {
    451 		htag++;
    452 		memset(&tshim, 0, sizeof(tshim));
    453 		tshim.s_addr = ntohl(htag->s_addr);
    454 		tshim.shim.bos = tshim.shim.exp = 0;
    455 		tshim.shim.ttl = mpls_defttl;
    456 		if (tshim.shim.label != MPLS_LABEL_IMPLNULL &&
    457 		    ((m = mpls_prepend_shim(m, &tshim)) == NULL))
    458 			return ENOBUFS;
    459 		psize += sizeof(tshim);
    460 	}
    461 
    462 	if (__predict_false(push_back_alert == true)) {
    463 		/* re-add the router alert label */
    464 		memset(&tshim, 0, sizeof(tshim));
    465 		tshim.s_addr = MPLS_LABEL_RTALERT;
    466 		tshim.shim.bos = tshim.shim.exp = 0;
    467 		tshim.shim.ttl = mpls_defttl;
    468 		if ((m = mpls_prepend_shim(m, &tshim)) == NULL)
    469 			return ENOBUFS;
    470 	}
    471 
    472 	if ((rt->rt_flags & RTF_GATEWAY) == 0) {
    473 		error = EHOSTUNREACH;
    474 		goto done;
    475 	}
    476 
    477 	rt->rt_use++;
    478 	error = mpls_send_frame(m, rt->rt_ifp, rt);
    479 
    480 done:
    481 	if (error != 0 && m != NULL)
    482 		m_freem(m);
    483 	if (rt != NULL)
    484 		rtfree(rt);
    485 
    486 	return error;
    487 }
    488 
    489 static int
    490 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, const struct rtentry *rt)
    491 {
    492 	union mpls_shim msh;
    493 	int ret;
    494 
    495 	msh.s_addr = MPLS_GETSADDR(rt);
    496 	if (msh.shim.label == MPLS_LABEL_IMPLNULL ||
    497 	    (m->m_flags & (M_MCAST | M_BCAST))) {
    498 		m_adj(m, sizeof(union mpls_shim));
    499 		m->m_pkthdr.csum_flags = 0;
    500 	}
    501 
    502 	switch(ifp->if_type) {
    503 	/* only these are supported for now */
    504 	case IFT_ETHER:
    505 	case IFT_TUNNEL:
    506 	case IFT_LOOP:
    507 #ifdef INET
    508 		ret = ip_if_output(ifp, m, rt->rt_gateway, rt);
    509 #else
    510 		ret = if_output_lock(ifp, ifp, m, rt->rt_gateway, rt);
    511 #endif
    512 		return ret;
    513 		break;
    514 	default:
    515 		return ENETUNREACH;
    516 	}
    517 	return 0;
    518 }
    519 
    520 
    521 
    522 #ifdef INET
    523 static int
    524 mpls_unlabel_inet(struct mbuf *m)
    525 {
    526 	struct ip *iph;
    527 	union mpls_shim *ms;
    528 	int iphlen;
    529 
    530 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    531 
    532 		/* get shim info */
    533 		ms = mtod(m, union mpls_shim *);
    534 		ms->s_addr = ntohl(ms->s_addr);
    535 
    536 		/* and get rid of it */
    537 		m_adj(m, sizeof(union mpls_shim));
    538 
    539 		/* get ip header */
    540 		if (m->m_len < sizeof (struct ip) &&
    541 		    (m = m_pullup(m, sizeof(struct ip))) == NULL)
    542 			return ENOBUFS;
    543 		iph = mtod(m, struct ip *);
    544 		iphlen = iph->ip_hl << 2;
    545 
    546 		/* get it all */
    547 		if (m->m_len < iphlen) {
    548 			if ((m = m_pullup(m, iphlen)) == NULL)
    549 				return ENOBUFS;
    550 			iph = mtod(m, struct ip *);
    551 		}
    552 
    553 		/* check ipsum */
    554 		if (in_cksum(m, iphlen) != 0) {
    555 			m_freem(m);
    556 			return EINVAL;
    557 		}
    558 
    559 		/* set IP ttl from MPLS ttl */
    560 		if (mpls_mapttl_inet)
    561 			iph->ip_ttl = ms->shim.ttl;
    562 
    563 		/* set IP Precedence from MPLS Exp */
    564 		if (mpls_mapprec_inet) {
    565 			iph->ip_tos = (iph->ip_tos << 3) >> 3;
    566 			iph->ip_tos |= ms->shim.exp << 5;
    567 		}
    568 
    569 		/* reset ipsum because we modified TTL and TOS */
    570 		iph->ip_sum = 0;
    571 		iph->ip_sum = in_cksum(m, iphlen);
    572 	} else
    573 		m_adj(m, sizeof(union mpls_shim));
    574 
    575 	/* Put it on IP queue */
    576 	if (__predict_false(!pktq_enqueue(ip_pktq, m, 0))) {
    577 		m_freem(m);
    578 		return ENOBUFS;
    579 	}
    580 	return 0;
    581 }
    582 
    583 /*
    584  * Prepend MPLS label
    585  */
    586 static struct mbuf *
    587 mpls_label_inet(struct mbuf *m, union mpls_shim *ms, uint offset)
    588 {
    589 	struct ip iphdr;
    590 
    591 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    592 		if ((m->m_len < sizeof(struct ip)) &&
    593 		    (m = m_pullup(m, offset + sizeof(struct ip))) == 0)
    594 			return NULL; /* XXX */
    595 		m_copydata(m, offset, sizeof(struct ip), &iphdr);
    596 
    597 		/* Map TTL */
    598 		if (mpls_mapttl_inet)
    599 			ms->shim.ttl = iphdr.ip_ttl;
    600 
    601 		/* Copy IP precedence to EXP */
    602 		if (mpls_mapprec_inet)
    603 			ms->shim.exp = ((u_int8_t)iphdr.ip_tos) >> 5;
    604 	}
    605 
    606 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    607 		return NULL;
    608 
    609 	return m;
    610 }
    611 
    612 #endif	/* INET */
    613 
    614 #ifdef INET6
    615 
    616 static int
    617 mpls_unlabel_inet6(struct mbuf *m)
    618 {
    619 	struct ip6_hdr *ip6hdr;
    620 	union mpls_shim ms;
    621 
    622 	/* TODO: mapclass */
    623 	if (mpls_mapttl_inet6) {
    624 		ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr);
    625 		m_adj(m, sizeof(union mpls_shim));
    626 
    627 		if (m->m_len < sizeof (struct ip6_hdr) &&
    628 		    (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0)
    629 			return ENOBUFS;
    630 		ip6hdr = mtod(m, struct ip6_hdr *);
    631 
    632 		/* Because we just decremented this in mpls_lse */
    633 		ip6hdr->ip6_hlim = ms.shim.ttl + 1;
    634 	} else
    635 		m_adj(m, sizeof(union mpls_shim));
    636 
    637 	/* Put it back on IPv6 queue. */
    638 	if (__predict_false(!pktq_enqueue(ip6_pktq, m, 0))) {
    639 		m_freem(m);
    640 		return ENOBUFS;
    641 	}
    642 	return 0;
    643 }
    644 
    645 static struct mbuf *
    646 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms, uint offset)
    647 {
    648 	struct ip6_hdr ip6h;
    649 
    650 	if (mpls_mapttl_inet6 || mpls_mapclass_inet6) {
    651 		if (m->m_len < sizeof(struct ip6_hdr) &&
    652 		    (m = m_pullup(m, offset + sizeof(struct ip6_hdr))) == 0)
    653 			return NULL;
    654 		m_copydata(m, offset, sizeof(struct ip6_hdr), &ip6h);
    655 
    656 		if (mpls_mapttl_inet6)
    657 			ms->shim.ttl = ip6h.ip6_hlim;
    658 
    659 		if (mpls_mapclass_inet6)
    660 			ms->shim.exp = ip6h.ip6_vfc << 1 >> 5;
    661 	}
    662 
    663 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    664 		return NULL;
    665 
    666 	return m;
    667 }
    668 
    669 #endif	/* INET6 */
    670 
    671 static struct mbuf *
    672 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms)
    673 {
    674 	union mpls_shim *shim;
    675 
    676 	M_PREPEND(m, sizeof(*ms), M_DONTWAIT);
    677 	if (m == NULL)
    678 		return NULL;
    679 
    680 	if (m->m_len < sizeof(union mpls_shim) &&
    681 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0)
    682 		return NULL;
    683 
    684 	shim = mtod(m, union mpls_shim *);
    685 
    686 	memcpy(shim, ms, sizeof(*shim));
    687 	shim->s_addr = htonl(shim->s_addr);
    688 
    689 	return m;
    690 }
    691 
    692 /*
    693  * Module infrastructure
    694  */
    695 #include "if_module.h"
    696 
    697 IF_MODULE(MODULE_CLASS_DRIVER, mpls, "")
    698