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