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