1 /* $NetBSD: if_mpls.c,v 1.36 2020/01/29 04:18:34 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.36 2020/01/29 04:18:34 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/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 if_statinc(ifp, if_oerrors); 290 return ENOBUFS; 291 } 292 293 if_statadd2(ifp, if_opackets, 1, if_obytes, m->m_pkthdr.len); 294 295 if ((rt1 = rtalloc1(rt->rt_gateway, 1)) == NULL) { 296 m_freem(m); 297 return EHOSTUNREACH; 298 } 299 300 err = mpls_send_frame(m, rt1->rt_ifp, rt); 301 rt_unref(rt1); 302 return err; 303 } 304 305 static int 306 mpls_ioctl(struct ifnet *ifp, u_long cmd, void *data) 307 { 308 int error = 0, s = splnet(); 309 struct ifreq *ifr = data; 310 311 switch(cmd) { 312 case SIOCINITIFADDR: 313 ifp->if_flags |= IFF_UP | IFF_RUNNING; 314 break; 315 case SIOCSIFMTU: 316 if (ifr != NULL && ifr->ifr_mtu < 576) { 317 error = EINVAL; 318 break; 319 } 320 /* FALLTHROUGH */ 321 case SIOCGIFMTU: 322 if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET) 323 error = 0; 324 break; 325 case SIOCSIFFLAGS: 326 if ((error = ifioctl_common(ifp, cmd, data)) != 0) 327 break; 328 if (ifp->if_flags & IFF_UP) 329 ifp->if_flags |= IFF_RUNNING; 330 break; 331 default: 332 error = ifioctl_common(ifp, cmd, data); 333 break; 334 } 335 splx(s); 336 return error; 337 } 338 339 static inline struct mbuf * 340 mpls_trim_label(struct mbuf *m, union mpls_shim *sh) 341 { 342 m_adj(m, sizeof(union mpls_shim)); 343 344 if (m->m_len < sizeof(union mpls_shim) && 345 (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) 346 return NULL; 347 348 sh->s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 349 350 return m; 351 } 352 353 /* 354 * MPLS Label Switch Engine 355 */ 356 static int 357 mpls_lse(struct mbuf *m) 358 { 359 struct sockaddr_mpls dst; 360 union mpls_shim tshim, *htag; 361 struct rtentry *rt = NULL; 362 int error = ENOBUFS; 363 uint psize = sizeof(struct sockaddr_mpls); 364 bool push_back_alert = false; 365 366 /* If we're not accepting MPLS frames, leave now. */ 367 if (!mpls_frame_accept) { 368 error = EINVAL; 369 goto done; 370 } 371 372 if (m->m_len < sizeof(union mpls_shim) && 373 (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) 374 goto done; 375 376 dst.smpls_len = sizeof(struct sockaddr_mpls); 377 dst.smpls_family = AF_MPLS; 378 dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 379 380 error = EINVAL; 381 382 /* TTL decrement */ 383 if ((m = mpls_ttl_dec(m)) == NULL) 384 goto done; 385 386 /* RFC 4182 */ 387 if (mpls_rfc4182 != 0) { 388 while ((dst.smpls_addr.shim.label == MPLS_LABEL_IPV4NULL || 389 dst.smpls_addr.shim.label == MPLS_LABEL_IPV6NULL) && 390 __predict_false(dst.smpls_addr.shim.bos == 0)) { 391 m = mpls_trim_label(m, &dst.smpls_addr); 392 if (m == NULL) { 393 goto done; 394 } 395 } 396 } 397 398 /* RFC 3032 Section 2.1 Page 4 */ 399 if (__predict_false(dst.smpls_addr.shim.label == MPLS_LABEL_RTALERT) && 400 dst.smpls_addr.shim.bos == 0) { 401 m = mpls_trim_label(m, &dst.smpls_addr); 402 if (m == NULL) { 403 goto done; 404 } 405 push_back_alert = true; 406 } 407 408 if (dst.smpls_addr.shim.label <= MPLS_LABEL_RESMAX) { 409 /* Don't swap reserved labels */ 410 switch (dst.smpls_addr.shim.label) { 411 #ifdef INET 412 case MPLS_LABEL_IPV4NULL: 413 /* Pop shim and push mbuf to IP stack */ 414 if (dst.smpls_addr.shim.bos) { 415 m = mpls_unlabel_inet(m, &error); 416 } 417 break; 418 #endif 419 #ifdef INET6 420 case MPLS_LABEL_IPV6NULL: 421 /* Pop shim and push mbuf to IPv6 stack */ 422 if (dst.smpls_addr.shim.bos) { 423 m = mpls_unlabel_inet6(m, &error); 424 } 425 break; 426 #endif 427 case MPLS_LABEL_RTALERT: /* Yeah, I'm all alerted */ 428 case MPLS_LABEL_IMPLNULL: /* This is logical only */ 429 default: /* Rest are not allowed */ 430 break; 431 } 432 goto done; 433 } 434 435 /* Check if we should do MPLS forwarding */ 436 error = EHOSTUNREACH; 437 if (!mpls_forwarding) 438 goto done; 439 440 /* Get a route to dst */ 441 dst.smpls_addr.shim.ttl = 0; 442 dst.smpls_addr.shim.bos = 0; 443 dst.smpls_addr.shim.exp = 0; 444 dst.smpls_addr.s_addr = htonl(dst.smpls_addr.s_addr); 445 if ((rt = rtalloc1((const struct sockaddr*)&dst, 1)) == NULL) 446 goto done; 447 448 /* MPLS packet with no MPLS tagged route ? */ 449 if ((rt->rt_flags & RTF_GATEWAY) == 0 || 450 rt_gettag(rt) == NULL || 451 rt_gettag(rt)->sa_family != AF_MPLS) 452 goto done; 453 454 tshim.s_addr = MPLS_GETSADDR(rt); 455 456 /* Swap labels */ 457 if ((m->m_len < sizeof(union mpls_shim)) && 458 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) { 459 error = ENOBUFS; 460 goto done; 461 } 462 463 /* Replace only the label */ 464 htag = mtod(m, union mpls_shim *); 465 htag->s_addr = ntohl(htag->s_addr); 466 htag->shim.label = tshim.shim.label; 467 htag->s_addr = htonl(htag->s_addr); 468 469 /* check if there is anything more to prepend */ 470 htag = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr; 471 while (psize <= rt_gettag(rt)->sa_len - sizeof(tshim)) { 472 htag++; 473 memset(&tshim, 0, sizeof(tshim)); 474 tshim.s_addr = ntohl(htag->s_addr); 475 tshim.shim.bos = tshim.shim.exp = 0; 476 tshim.shim.ttl = mpls_defttl; 477 if (tshim.shim.label != MPLS_LABEL_IMPLNULL && 478 ((m = mpls_prepend_shim(m, &tshim)) == NULL)) { 479 error = ENOBUFS; 480 goto done; 481 } 482 psize += sizeof(tshim); 483 } 484 485 if (__predict_false(push_back_alert == true)) { 486 /* re-add the router alert label */ 487 memset(&tshim, 0, sizeof(tshim)); 488 tshim.s_addr = MPLS_LABEL_RTALERT; 489 tshim.shim.bos = tshim.shim.exp = 0; 490 tshim.shim.ttl = mpls_defttl; 491 if ((m = mpls_prepend_shim(m, &tshim)) == NULL) { 492 error = ENOBUFS; 493 goto done; 494 } 495 } 496 497 if ((rt->rt_flags & RTF_GATEWAY) == 0) { 498 error = EHOSTUNREACH; 499 goto done; 500 } 501 502 rt->rt_use++; 503 error = mpls_send_frame(m, rt->rt_ifp, rt); 504 505 done: 506 if (error != 0 && m != NULL) 507 m_freem(m); 508 if (rt != NULL) 509 rt_unref(rt); 510 511 return error; 512 } 513 514 static int 515 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, const struct rtentry *rt) 516 { 517 union mpls_shim msh; 518 int ret; 519 520 msh.s_addr = MPLS_GETSADDR(rt); 521 if (msh.shim.label == MPLS_LABEL_IMPLNULL || 522 (m->m_flags & (M_MCAST | M_BCAST))) { 523 m_adj(m, sizeof(union mpls_shim)); 524 m->m_pkthdr.csum_flags = 0; 525 } 526 527 switch(ifp->if_type) { 528 /* only these are supported for now */ 529 case IFT_ETHER: 530 case IFT_TUNNEL: 531 case IFT_LOOP: 532 #ifdef INET 533 ret = ip_if_output(ifp, m, rt->rt_gateway, rt); 534 #else 535 ret = if_output_lock(ifp, ifp, m, rt->rt_gateway, rt); 536 #endif 537 return ret; 538 break; 539 default: 540 return ENETUNREACH; 541 } 542 return 0; 543 } 544 545 #ifdef INET 546 static struct mbuf * 547 mpls_unlabel_inet(struct mbuf *m, int *error) 548 { 549 struct ip *iph; 550 union mpls_shim ms; 551 int iphlen; 552 553 if (mpls_mapttl_inet || mpls_mapprec_inet) { 554 /* get shim info */ 555 ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 556 557 /* and get rid of it */ 558 m_adj(m, sizeof(union mpls_shim)); 559 560 /* get ip header */ 561 if (m->m_len < sizeof(struct ip) && 562 (m = m_pullup(m, sizeof(struct ip))) == NULL) { 563 *error = ENOBUFS; 564 return NULL; 565 } 566 567 iph = mtod(m, struct ip *); 568 iphlen = iph->ip_hl << 2; 569 570 /* get it all */ 571 if (m->m_len < iphlen) { 572 if ((m = m_pullup(m, iphlen)) == NULL) { 573 *error = ENOBUFS; 574 return NULL; 575 } 576 iph = mtod(m, struct ip *); 577 } 578 579 /* check ipsum */ 580 if (in_cksum(m, iphlen) != 0) { 581 m_freem(m); 582 *error = EINVAL; 583 return NULL; 584 } 585 586 /* set IP ttl from MPLS ttl */ 587 if (mpls_mapttl_inet) 588 iph->ip_ttl = ms.shim.ttl; 589 590 /* set IP Precedence from MPLS Exp */ 591 if (mpls_mapprec_inet) { 592 iph->ip_tos = (iph->ip_tos << 3) >> 3; 593 iph->ip_tos |= ms.shim.exp << 5; 594 } 595 596 /* reset ipsum because we modified TTL and TOS */ 597 iph->ip_sum = 0; 598 iph->ip_sum = in_cksum(m, iphlen); 599 } else { 600 m_adj(m, sizeof(union mpls_shim)); 601 } 602 603 /* Put it on IP queue */ 604 if (__predict_false(!pktq_enqueue(ip_pktq, m, 0))) { 605 m_freem(m); 606 *error = ENOBUFS; 607 return NULL; 608 } 609 610 *error = 0; 611 return m; 612 } 613 614 /* 615 * Prepend MPLS label 616 */ 617 static struct mbuf * 618 mpls_label_inet(struct mbuf *m, union mpls_shim *ms, uint offset) 619 { 620 struct ip iphdr; 621 622 if (mpls_mapttl_inet || mpls_mapprec_inet) { 623 /* XXX Maybe just check m->m_pkthdr.len instead? */ 624 if ((m->m_len < offset + sizeof(struct ip)) && 625 (m = m_pullup(m, offset + sizeof(struct ip))) == 0) 626 return NULL; 627 628 m_copydata(m, offset, sizeof(struct ip), &iphdr); 629 630 /* Map TTL */ 631 if (mpls_mapttl_inet) 632 ms->shim.ttl = iphdr.ip_ttl; 633 634 /* Copy IP precedence to EXP */ 635 if (mpls_mapprec_inet) 636 ms->shim.exp = ((u_int8_t)iphdr.ip_tos) >> 5; 637 } 638 639 if ((m = mpls_prepend_shim(m, ms)) == NULL) 640 return NULL; 641 642 return m; 643 } 644 #endif /* INET */ 645 646 #ifdef INET6 647 static struct mbuf * 648 mpls_unlabel_inet6(struct mbuf *m, int *error) 649 { 650 struct ip6_hdr *ip6hdr; 651 union mpls_shim ms; 652 653 /* TODO: mapclass */ 654 if (mpls_mapttl_inet6) { 655 ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 656 m_adj(m, sizeof(union mpls_shim)); 657 658 if (m->m_len < sizeof(struct ip6_hdr) && 659 (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0) { 660 *error = ENOBUFS; 661 return NULL; 662 } 663 ip6hdr = mtod(m, struct ip6_hdr *); 664 665 /* Because we just decremented this in mpls_lse */ 666 ip6hdr->ip6_hlim = ms.shim.ttl + 1; 667 } else { 668 m_adj(m, sizeof(union mpls_shim)); 669 } 670 671 /* Put it back on IPv6 queue. */ 672 if (__predict_false(!pktq_enqueue(ip6_pktq, m, 0))) { 673 m_freem(m); 674 *error = ENOBUFS; 675 return NULL; 676 } 677 678 *error = 0; 679 return m; 680 } 681 682 static struct mbuf * 683 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms, uint offset) 684 { 685 struct ip6_hdr ip6h; 686 687 if (mpls_mapttl_inet6 || mpls_mapclass_inet6) { 688 /* XXX Maybe just check m->m_pkthdr.len instead? */ 689 if ((m->m_len < offset + sizeof(struct ip6_hdr)) && 690 (m = m_pullup(m, offset + sizeof(struct ip6_hdr))) == 0) 691 return NULL; 692 693 m_copydata(m, offset, sizeof(struct ip6_hdr), &ip6h); 694 695 if (mpls_mapttl_inet6) 696 ms->shim.ttl = ip6h.ip6_hlim; 697 698 if (mpls_mapclass_inet6) 699 ms->shim.exp = ip6h.ip6_vfc << 1 >> 5; 700 } 701 702 if ((m = mpls_prepend_shim(m, ms)) == NULL) 703 return NULL; 704 705 return m; 706 } 707 #endif /* INET6 */ 708 709 static struct mbuf * 710 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms) 711 { 712 union mpls_shim *shim; 713 714 M_PREPEND(m, sizeof(*ms), M_DONTWAIT); 715 if (m == NULL) 716 return NULL; 717 718 if (m->m_len < sizeof(union mpls_shim) && 719 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) 720 return NULL; 721 722 shim = mtod(m, union mpls_shim *); 723 724 memcpy(shim, ms, sizeof(*shim)); 725 shim->s_addr = htonl(shim->s_addr); 726 727 return m; 728 } 729 730 /* 731 * Module infrastructure 732 */ 733 #include "if_module.h" 734 735 IF_MODULE(MODULE_CLASS_DRIVER, mpls, NULL) 736