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