xref: /netbsd-src/sys/net/if_mpls.c (revision 796c32c94f6e154afc9de0f63da35c91bb739b45)
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@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