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