xref: /netbsd-src/sys/net/if_mpls.c (revision e89934bbf778a6d6d6894877c4da59d0c7835b0f)
1 /*	$NetBSD: if_mpls.c,v 1.29 2016/12/12 03:55:57 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@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.29 2016/12/12 03:55:57 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 	rt_unref(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 		rt_unref(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