1 /* $OpenBSD: if_mpip.c,v 1.13 2021/02/20 05:03:37 dlg Exp $ */ 2 3 /* 4 * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org> 5 * Copyright (c) 2019 David Gwynne <dlg@openbsd.org> 6 * 7 * Permission to use, copy, modify, and distribute this software for any 8 * purpose with or without fee is hereby granted, provided that the above 9 * copyright notice and this permission notice appear in all copies. 10 * 11 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 12 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 13 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 14 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 15 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 */ 19 20 #include "bpfilter.h" 21 22 #include <sys/param.h> 23 #include <sys/systm.h> 24 #include <sys/mbuf.h> 25 #include <sys/socket.h> 26 #include <sys/ioctl.h> 27 #include <sys/errno.h> 28 29 #include <net/if.h> 30 #include <net/if_var.h> 31 #include <net/if_dl.h> 32 #include <net/if_types.h> 33 #include <net/route.h> 34 35 #include <netinet/in.h> 36 #include <netinet/ip.h> 37 38 #ifdef INET6 39 #include <netinet/ip6.h> 40 #endif 41 42 #include <netmpls/mpls.h> 43 44 #if NBPFILTER > 0 45 #include <net/bpf.h> 46 #endif /* NBPFILTER */ 47 48 struct mpip_neighbor { 49 struct shim_hdr n_rshim; 50 struct sockaddr_storage n_nexthop; 51 }; 52 53 struct mpip_softc { 54 struct ifnet sc_if; 55 unsigned int sc_dead; 56 uint32_t sc_flow; /* xor for mbuf flowid */ 57 58 int sc_txhprio; 59 int sc_rxhprio; 60 struct ifaddr sc_ifa; 61 struct sockaddr_mpls sc_smpls; /* Local label */ 62 unsigned int sc_rdomain; 63 struct mpip_neighbor *sc_neighbor; 64 65 unsigned int sc_cword; /* control word */ 66 unsigned int sc_fword; /* flow-aware transport */ 67 int sc_ttl; 68 }; 69 70 void mpipattach(int); 71 int mpip_clone_create(struct if_clone *, int); 72 int mpip_clone_destroy(struct ifnet *); 73 int mpip_ioctl(struct ifnet *, u_long, caddr_t); 74 int mpip_output(struct ifnet *, struct mbuf *, struct sockaddr *, 75 struct rtentry *); 76 void mpip_start(struct ifnet *); 77 78 struct if_clone mpip_cloner = 79 IF_CLONE_INITIALIZER("mpip", mpip_clone_create, mpip_clone_destroy); 80 81 void 82 mpipattach(int n) 83 { 84 if_clone_attach(&mpip_cloner); 85 } 86 87 int 88 mpip_clone_create(struct if_clone *ifc, int unit) 89 { 90 struct mpip_softc *sc; 91 struct ifnet *ifp; 92 93 sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK|M_CANFAIL|M_ZERO); 94 if (sc == NULL) 95 return (ENOMEM); 96 97 sc->sc_txhprio = 0; 98 sc->sc_rxhprio = IF_HDRPRIO_PACKET; 99 sc->sc_neighbor = 0; 100 sc->sc_cword = 0; /* default to no control word */ 101 sc->sc_fword = 0; /* both sides have to agree on FAT first */ 102 sc->sc_flow = arc4random() & 0xfffff; 103 sc->sc_smpls.smpls_len = sizeof(sc->sc_smpls); 104 sc->sc_smpls.smpls_family = AF_MPLS; 105 sc->sc_ttl = -1; 106 107 ifp = &sc->sc_if; 108 snprintf(ifp->if_xname, sizeof(ifp->if_xname), "%s%d", 109 ifc->ifc_name, unit); 110 ifp->if_softc = sc; 111 ifp->if_type = IFT_TUNNEL; 112 ifp->if_flags = IFF_POINTOPOINT | IFF_MULTICAST; 113 ifp->if_xflags = IFXF_CLONED; 114 ifp->if_ioctl = mpip_ioctl; 115 ifp->if_bpf_mtap = p2p_bpf_mtap; 116 ifp->if_input = p2p_input; 117 ifp->if_output = mpip_output; 118 ifp->if_start = mpip_start; 119 ifp->if_rtrequest = p2p_rtrequest; 120 ifp->if_mtu = 1500; 121 ifp->if_hardmtu = 65535; 122 123 if_attach(ifp); 124 if_counters_alloc(ifp); 125 if_alloc_sadl(ifp); 126 127 #if NBPFILTER > 0 128 bpfattach(&ifp->if_bpf, ifp, DLT_LOOP, sizeof(uint32_t)); 129 #endif 130 131 sc->sc_ifa.ifa_ifp = ifp; 132 sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl); 133 134 return (0); 135 } 136 137 int 138 mpip_clone_destroy(struct ifnet *ifp) 139 { 140 struct mpip_softc *sc = ifp->if_softc; 141 142 NET_LOCK(); 143 ifp->if_flags &= ~IFF_RUNNING; 144 sc->sc_dead = 1; 145 146 if (sc->sc_smpls.smpls_label) { 147 rt_ifa_del(&sc->sc_ifa, RTF_LOCAL | RTF_MPLS, 148 smplstosa(&sc->sc_smpls), 0); 149 } 150 NET_UNLOCK(); 151 152 ifq_barrier(&ifp->if_snd); 153 154 if_detach(ifp); 155 156 free(sc->sc_neighbor, M_DEVBUF, sizeof(*sc->sc_neighbor)); 157 free(sc, M_DEVBUF, sizeof(*sc)); 158 159 return (0); 160 } 161 162 static int 163 mpip_set_route(struct mpip_softc *sc, uint32_t shim, unsigned int rdomain) 164 { 165 int error; 166 167 rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 168 smplstosa(&sc->sc_smpls), 0); 169 170 sc->sc_smpls.smpls_label = shim; 171 sc->sc_rdomain = rdomain; 172 173 error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 174 smplstosa(&sc->sc_smpls), 0); 175 if (error) { 176 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 177 return (error); 178 } 179 180 return (0); 181 } 182 183 static int 184 mpip_set_label(struct mpip_softc *sc, struct ifreq *ifr) 185 { 186 struct shim_hdr label; 187 uint32_t shim; 188 int error; 189 190 error = copyin(ifr->ifr_data, &label, sizeof(label)); 191 if (error != 0) 192 return (error); 193 194 if (label.shim_label > MPLS_LABEL_MAX || 195 label.shim_label <= MPLS_LABEL_RESERVED_MAX) 196 return (EINVAL); 197 198 shim = MPLS_LABEL2SHIM(label.shim_label); 199 200 if (sc->sc_smpls.smpls_label == shim) 201 return (0); 202 203 return (mpip_set_route(sc, shim, sc->sc_rdomain)); 204 } 205 206 static int 207 mpip_get_label(struct mpip_softc *sc, struct ifreq *ifr) 208 { 209 struct shim_hdr label; 210 211 label.shim_label = MPLS_SHIM2LABEL(sc->sc_smpls.smpls_label); 212 213 if (label.shim_label == 0) 214 return (EADDRNOTAVAIL); 215 216 return (copyout(&label, ifr->ifr_data, sizeof(label))); 217 } 218 219 static int 220 mpip_del_label(struct mpip_softc *sc) 221 { 222 if (sc->sc_smpls.smpls_label != MPLS_LABEL2SHIM(0)) { 223 rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 224 smplstosa(&sc->sc_smpls), 0); 225 } 226 227 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 228 229 return (0); 230 } 231 232 static int 233 mpip_set_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 234 { 235 struct mpip_neighbor *n, *o; 236 struct sockaddr *sa = (struct sockaddr *)&req->addr; 237 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 238 uint32_t label; 239 240 if (smpls->smpls_family != AF_MPLS) 241 return (EINVAL); 242 label = smpls->smpls_label; 243 if (label > MPLS_LABEL_MAX || label <= MPLS_LABEL_RESERVED_MAX) 244 return (EINVAL); 245 246 switch (sa->sa_family) { 247 case AF_INET: { 248 struct sockaddr_in *sin = (struct sockaddr_in *)sa; 249 250 if (in_nullhost(sin->sin_addr) || 251 IN_MULTICAST(sin->sin_addr.s_addr)) 252 return (EINVAL); 253 254 break; 255 } 256 #ifdef INET6 257 case AF_INET6: { 258 struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sa; 259 260 if (IN6_IS_ADDR_UNSPECIFIED(&sin6->sin6_addr) || 261 IN6_IS_ADDR_MULTICAST(&sin6->sin6_addr)) 262 return (EINVAL); 263 264 /* check scope */ 265 266 break; 267 } 268 #endif 269 default: 270 return (EAFNOSUPPORT); 271 } 272 273 if (sc->sc_dead) 274 return (ENXIO); 275 276 n = malloc(sizeof(*n), M_DEVBUF, M_WAITOK|M_CANFAIL|M_ZERO); 277 if (n == NULL) 278 return (ENOMEM); 279 280 n->n_rshim.shim_label = MPLS_LABEL2SHIM(label); 281 n->n_nexthop = req->addr; 282 283 o = sc->sc_neighbor; 284 sc->sc_neighbor = n; 285 286 NET_UNLOCK(); 287 ifq_barrier(&sc->sc_if.if_snd); 288 NET_LOCK(); 289 290 free(o, M_DEVBUF, sizeof(*o)); 291 292 return (0); 293 } 294 295 static int 296 mpip_get_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 297 { 298 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 299 struct mpip_neighbor *n = sc->sc_neighbor; 300 301 if (n == NULL) 302 return (EADDRNOTAVAIL); 303 304 smpls->smpls_len = sizeof(*smpls); 305 smpls->smpls_family = AF_MPLS; 306 smpls->smpls_label = MPLS_SHIM2LABEL(n->n_rshim.shim_label); 307 req->addr = n->n_nexthop; 308 309 return (0); 310 } 311 312 static int 313 mpip_del_neighbor(struct mpip_softc *sc, struct ifreq *req) 314 { 315 struct mpip_neighbor *o; 316 317 if (sc->sc_dead) 318 return (ENXIO); 319 320 o = sc->sc_neighbor; 321 sc->sc_neighbor = NULL; 322 323 NET_UNLOCK(); 324 ifq_barrier(&sc->sc_if.if_snd); 325 NET_LOCK(); 326 327 free(o, M_DEVBUF, sizeof(*o)); 328 329 return (0); 330 } 331 332 int 333 mpip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 334 { 335 struct mpip_softc *sc = ifp->if_softc; 336 struct ifreq *ifr = (struct ifreq *)data; 337 int error = 0; 338 339 switch (cmd) { 340 case SIOCSIFADDR: 341 break; 342 case SIOCSIFFLAGS: 343 if ((ifp->if_flags & IFF_UP)) 344 ifp->if_flags |= IFF_RUNNING; 345 else 346 ifp->if_flags &= ~IFF_RUNNING; 347 break; 348 case SIOCSIFMTU: 349 if (ifr->ifr_mtu < 60 || /* XXX */ 350 ifr->ifr_mtu > 65536) /* XXX */ 351 error = EINVAL; 352 else 353 ifp->if_mtu = ifr->ifr_mtu; 354 break; 355 356 case SIOCGPWE3: 357 ifr->ifr_pwe3 = IF_PWE3_IP; 358 break; 359 case SIOCSPWE3CTRLWORD: 360 sc->sc_cword = ifr->ifr_pwe3 ? 1 : 0; 361 break; 362 case SIOCGPWE3CTRLWORD: 363 ifr->ifr_pwe3 = sc->sc_cword; 364 break; 365 case SIOCSPWE3FAT: 366 sc->sc_fword = ifr->ifr_pwe3 ? 1 : 0; 367 break; 368 case SIOCGPWE3FAT: 369 ifr->ifr_pwe3 = sc->sc_fword; 370 break; 371 372 case SIOCSETLABEL: 373 error = mpip_set_label(sc, ifr); 374 break; 375 case SIOCGETLABEL: 376 error = mpip_get_label(sc, ifr); 377 break; 378 case SIOCDELLABEL: 379 error = mpip_del_label(sc); 380 break; 381 382 case SIOCSPWE3NEIGHBOR: 383 error = mpip_set_neighbor(sc, (struct if_laddrreq *)data); 384 break; 385 case SIOCGPWE3NEIGHBOR: 386 error = mpip_get_neighbor(sc, (struct if_laddrreq *)data); 387 break; 388 case SIOCDPWE3NEIGHBOR: 389 error = mpip_del_neighbor(sc, ifr); 390 break; 391 392 case SIOCSLIFPHYRTABLE: 393 if (ifr->ifr_rdomainid < 0 || 394 ifr->ifr_rdomainid > RT_TABLEID_MAX || 395 !rtable_exists(ifr->ifr_rdomainid) || 396 ifr->ifr_rdomainid != rtable_l2(ifr->ifr_rdomainid)) { 397 error = EINVAL; 398 break; 399 } 400 if (sc->sc_rdomain != ifr->ifr_rdomainid) { 401 error = mpip_set_route(sc, sc->sc_smpls.smpls_label, 402 ifr->ifr_rdomainid); 403 } 404 break; 405 case SIOCGLIFPHYRTABLE: 406 ifr->ifr_rdomainid = sc->sc_rdomain; 407 break; 408 409 case SIOCSLIFPHYTTL: 410 if (ifr->ifr_ttl != -1 && 411 (ifr->ifr_ttl < 1 || ifr->ifr_ttl > 0xff)) { 412 error = EINVAL; 413 break; 414 } 415 416 /* commit */ 417 sc->sc_ttl = ifr->ifr_ttl; 418 break; 419 case SIOCGLIFPHYTTL: 420 ifr->ifr_ttl = sc->sc_ttl; 421 break; 422 423 case SIOCSTXHPRIO: 424 error = if_txhprio_l3_check(ifr->ifr_hdrprio); 425 if (error != 0) 426 break; 427 428 sc->sc_txhprio = ifr->ifr_hdrprio; 429 break; 430 case SIOCGTXHPRIO: 431 ifr->ifr_hdrprio = sc->sc_txhprio; 432 break; 433 434 case SIOCSRXHPRIO: 435 error = if_rxhprio_l3_check(ifr->ifr_hdrprio); 436 if (error != 0) 437 break; 438 439 sc->sc_rxhprio = ifr->ifr_hdrprio; 440 break; 441 case SIOCGRXHPRIO: 442 ifr->ifr_hdrprio = sc->sc_rxhprio; 443 break; 444 445 case SIOCADDMULTI: 446 case SIOCDELMULTI: 447 break; 448 449 default: 450 error = ENOTTY; 451 break; 452 } 453 454 return (error); 455 } 456 457 static void 458 mpip_input(struct mpip_softc *sc, struct mbuf *m) 459 { 460 struct ifnet *ifp = &sc->sc_if; 461 int rxprio = sc->sc_rxhprio; 462 uint32_t shim, exp; 463 struct mbuf *n; 464 uint8_t ttl, tos; 465 466 if (!ISSET(ifp->if_flags, IFF_RUNNING)) 467 goto drop; 468 469 shim = *mtod(m, uint32_t *); 470 m_adj(m, sizeof(shim)); 471 472 ttl = ntohl(shim & MPLS_TTL_MASK); 473 exp = ntohl(shim & MPLS_EXP_MASK) >> MPLS_EXP_OFFSET; 474 475 if (sc->sc_fword) { 476 uint32_t label; 477 478 if (MPLS_BOS_ISSET(shim)) 479 goto drop; 480 481 if (m->m_len < sizeof(shim)) { 482 m = m_pullup(m, sizeof(shim)); 483 if (m == NULL) 484 return; 485 } 486 487 shim = *mtod(m, uint32_t *); 488 if (!MPLS_BOS_ISSET(shim)) 489 goto drop; 490 491 label = MPLS_SHIM2LABEL(shim); 492 if (label <= MPLS_LABEL_RESERVED_MAX) { 493 counters_inc(ifp->if_counters, ifc_noproto); /* ? */ 494 goto drop; 495 } 496 497 label -= MPLS_LABEL_RESERVED_MAX + 1; 498 label ^= sc->sc_flow; 499 SET(m->m_pkthdr.csum_flags, M_FLOWID); 500 m->m_pkthdr.ph_flowid = label; 501 502 m_adj(m, sizeof(shim)); 503 } else if (!MPLS_BOS_ISSET(shim)) 504 goto drop; 505 506 if (sc->sc_cword) { 507 if (m->m_len < sizeof(shim)) { 508 m = m_pullup(m, sizeof(shim)); 509 if (m == NULL) 510 return; 511 } 512 shim = *mtod(m, uint32_t *); 513 514 /* 515 * The first 4 bits identifies that this packet is a 516 * control word. If the control word is configured and 517 * we received an IP datagram we shall drop it. 518 */ 519 if (shim & CW_ZERO_MASK) { 520 counters_inc(ifp->if_counters, ifc_ierrors); 521 goto drop; 522 } 523 524 /* We don't support fragmentation just yet. */ 525 if (shim & CW_FRAG_MASK) { 526 counters_inc(ifp->if_counters, ifc_ierrors); 527 goto drop; 528 } 529 530 m_adj(m, sizeof(shim)); 531 } 532 533 n = m; 534 while (n->m_len == 0) { 535 n = n->m_next; 536 if (n == NULL) 537 goto drop; 538 } 539 540 switch (*mtod(n, uint8_t *) >> 4) { 541 case 4: { 542 struct ip *ip; 543 if (m->m_len < sizeof(*ip)) { 544 m = m_pullup(m, sizeof(*ip)); 545 if (m == NULL) 546 return; 547 } 548 ip = mtod(m, struct ip *); 549 tos = ip->ip_tos; 550 551 if (sc->sc_ttl == -1) { 552 m = mpls_ip_adjttl(m, ttl); 553 if (m == NULL) 554 return; 555 } 556 557 m->m_pkthdr.ph_family = AF_INET; 558 break; 559 } 560 #ifdef INET6 561 case 6: { 562 struct ip6_hdr *ip6; 563 uint32_t flow; 564 if (m->m_len < sizeof(*ip6)) { 565 m = m_pullup(m, sizeof(*ip6)); 566 if (m == NULL) 567 return; 568 } 569 ip6 = mtod(m, struct ip6_hdr *); 570 flow = bemtoh32(&ip6->ip6_flow); 571 tos = flow >> 20; 572 573 if (sc->sc_ttl == -1) { 574 m = mpls_ip6_adjttl(m, ttl); 575 if (m == NULL) 576 return; 577 } 578 579 m->m_pkthdr.ph_family = AF_INET6; 580 break; 581 } 582 #endif /* INET6 */ 583 default: 584 counters_inc(ifp->if_counters, ifc_noproto); 585 goto drop; 586 } 587 588 switch (rxprio) { 589 case IF_HDRPRIO_PACKET: 590 /* nop */ 591 break; 592 case IF_HDRPRIO_OUTER: 593 m->m_pkthdr.pf.prio = exp; 594 break; 595 case IF_HDRPRIO_PAYLOAD: 596 m->m_pkthdr.pf.prio = IFQ_TOS2PRIO(tos); 597 break; 598 default: 599 m->m_pkthdr.pf.prio = rxprio; 600 break; 601 } 602 603 if_vinput(ifp, m); 604 return; 605 drop: 606 m_freem(m); 607 } 608 609 int 610 mpip_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst, 611 struct rtentry *rt) 612 { 613 struct mpip_softc *sc = ifp->if_softc; 614 int error; 615 616 if (dst->sa_family == AF_LINK && 617 rt != NULL && ISSET(rt->rt_flags, RTF_LOCAL)) { 618 mpip_input(sc, m); 619 return (0); 620 } 621 622 if (!ISSET(ifp->if_flags, IFF_RUNNING)) { 623 error = ENETDOWN; 624 goto drop; 625 } 626 627 switch (dst->sa_family) { 628 case AF_INET: 629 #ifdef INET6 630 case AF_INET6: 631 #endif 632 break; 633 default: 634 error = EAFNOSUPPORT; 635 goto drop; 636 } 637 638 m->m_pkthdr.ph_family = dst->sa_family; 639 640 error = if_enqueue(ifp, m); 641 if (error) 642 counters_inc(ifp->if_counters, ifc_oerrors); 643 return (error); 644 645 drop: 646 m_freem(m); 647 return (error); 648 } 649 650 void 651 mpip_start(struct ifnet *ifp) 652 { 653 struct mpip_softc *sc = ifp->if_softc; 654 struct mpip_neighbor *n = sc->sc_neighbor; 655 struct rtentry *rt; 656 struct ifnet *ifp0; 657 struct mbuf *m; 658 uint32_t shim; 659 struct sockaddr_mpls smpls = { 660 .smpls_len = sizeof(smpls), 661 .smpls_family = AF_MPLS, 662 }; 663 int txprio = sc->sc_txhprio; 664 uint32_t exp, bos; 665 uint8_t tos, prio, ttl; 666 667 if (!ISSET(ifp->if_flags, IFF_RUNNING) || n == NULL) { 668 ifq_purge(&ifp->if_snd); 669 return; 670 } 671 672 rt = rtalloc(sstosa(&n->n_nexthop), RT_RESOLVE, sc->sc_rdomain); 673 if (!rtisvalid(rt)) { 674 ifq_purge(&ifp->if_snd); 675 goto rtfree; 676 } 677 678 ifp0 = if_get(rt->rt_ifidx); 679 if (ifp0 == NULL) { 680 ifq_purge(&ifp->if_snd); 681 goto rtfree; 682 } 683 684 while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) { 685 #if NBPFILTER > 0 686 caddr_t if_bpf = sc->sc_if.if_bpf; 687 if (if_bpf) { 688 bpf_mtap_af(if_bpf, m->m_pkthdr.ph_family, 689 m, BPF_DIRECTION_OUT); 690 } 691 #endif /* NBPFILTER */ 692 693 if (sc->sc_ttl == -1) { 694 switch (m->m_pkthdr.ph_family) { 695 case AF_INET: { 696 struct ip *ip; 697 ip = mtod(m, struct ip *); 698 ttl = ip->ip_ttl; 699 break; 700 } 701 #ifdef INET6 702 case AF_INET6: { 703 struct ip6_hdr *ip6; 704 ip6 = mtod(m, struct ip6_hdr *); 705 ttl = ip6->ip6_hlim; 706 break; 707 } 708 #endif 709 default: 710 unhandled_af(m->m_pkthdr.ph_family); 711 } 712 } else 713 ttl = mpls_defttl; 714 715 switch (txprio) { 716 case IF_HDRPRIO_PACKET: 717 prio = m->m_pkthdr.pf.prio; 718 break; 719 case IF_HDRPRIO_PAYLOAD: 720 switch (m->m_pkthdr.ph_family) { 721 case AF_INET: { 722 struct ip *ip; 723 ip = mtod(m, struct ip *); 724 tos = ip->ip_tos; 725 break; 726 } 727 #ifdef INET6 728 case AF_INET6: { 729 struct ip6_hdr *ip6; 730 uint32_t flow; 731 ip6 = mtod(m, struct ip6_hdr *); 732 flow = bemtoh32(&ip6->ip6_flow); 733 tos = flow >> 20; 734 break; 735 } 736 #endif 737 default: 738 unhandled_af(m->m_pkthdr.ph_family); 739 } 740 741 prio = IFQ_TOS2PRIO(tos); 742 break; 743 default: 744 prio = txprio; 745 break; 746 } 747 exp = htonl(prio << MPLS_EXP_OFFSET); 748 749 if (sc->sc_cword) { 750 m = m_prepend(m, sizeof(shim), M_NOWAIT); 751 if (m == NULL) 752 continue; 753 754 *mtod(m, uint32_t *) = 0; 755 } 756 757 bos = MPLS_BOS_MASK; 758 759 if (sc->sc_fword) { 760 uint32_t flow = 0; 761 m = m_prepend(m, sizeof(shim), M_NOWAIT); 762 if (m == NULL) 763 continue; 764 765 if (ISSET(m->m_pkthdr.csum_flags, M_FLOWID)) 766 flow = m->m_pkthdr.ph_flowid; 767 flow ^= sc->sc_flow; 768 flow += MPLS_LABEL_RESERVED_MAX + 1; 769 770 shim = htonl(1) & MPLS_TTL_MASK; 771 shim |= htonl(flow << MPLS_LABEL_OFFSET) & 772 MPLS_LABEL_MASK; 773 shim |= exp | bos; 774 *mtod(m, uint32_t *) = shim; 775 776 bos = 0; 777 } 778 779 m = m_prepend(m, sizeof(shim), M_NOWAIT); 780 if (m == NULL) 781 continue; 782 783 shim = htonl(ttl) & MPLS_TTL_MASK; 784 shim |= n->n_rshim.shim_label; 785 shim |= exp | bos; 786 *mtod(m, uint32_t *) = shim; 787 788 m->m_pkthdr.ph_rtableid = sc->sc_rdomain; 789 CLR(m->m_flags, M_BCAST|M_MCAST); 790 791 mpls_output(ifp0, m, (struct sockaddr *)&smpls, rt); 792 } 793 794 if_put(ifp0); 795 rtfree: 796 rtfree(rt); 797 } 798