1 /* $OpenBSD: if_mpip.c,v 1.15 2021/03/26 19:00:21 kn 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), sc->sc_rdomain); 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), sc->sc_rdomain); 169 170 sc->sc_smpls.smpls_label = shim; 171 sc->sc_rdomain = rdomain; 172 173 /* only install with a label or mpip_clone_destroy() will ignore it */ 174 if (sc->sc_smpls.smpls_label == MPLS_LABEL2SHIM(0)) 175 return 0; 176 177 error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 178 smplstosa(&sc->sc_smpls), sc->sc_rdomain); 179 if (error) { 180 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 181 return (error); 182 } 183 184 return (0); 185 } 186 187 static int 188 mpip_set_label(struct mpip_softc *sc, struct ifreq *ifr) 189 { 190 struct shim_hdr label; 191 uint32_t shim; 192 int error; 193 194 error = copyin(ifr->ifr_data, &label, sizeof(label)); 195 if (error != 0) 196 return (error); 197 198 if (label.shim_label > MPLS_LABEL_MAX || 199 label.shim_label <= MPLS_LABEL_RESERVED_MAX) 200 return (EINVAL); 201 202 shim = MPLS_LABEL2SHIM(label.shim_label); 203 204 if (sc->sc_smpls.smpls_label == shim) 205 return (0); 206 207 return (mpip_set_route(sc, shim, sc->sc_rdomain)); 208 } 209 210 static int 211 mpip_get_label(struct mpip_softc *sc, struct ifreq *ifr) 212 { 213 struct shim_hdr label; 214 215 label.shim_label = MPLS_SHIM2LABEL(sc->sc_smpls.smpls_label); 216 217 if (label.shim_label == 0) 218 return (EADDRNOTAVAIL); 219 220 return (copyout(&label, ifr->ifr_data, sizeof(label))); 221 } 222 223 static int 224 mpip_del_label(struct mpip_softc *sc) 225 { 226 if (sc->sc_smpls.smpls_label != MPLS_LABEL2SHIM(0)) { 227 rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 228 smplstosa(&sc->sc_smpls), sc->sc_rdomain); 229 } 230 231 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 232 233 return (0); 234 } 235 236 static int 237 mpip_set_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 238 { 239 struct mpip_neighbor *n, *o; 240 struct sockaddr *sa = (struct sockaddr *)&req->addr; 241 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 242 uint32_t label; 243 244 if (smpls->smpls_family != AF_MPLS) 245 return (EINVAL); 246 label = smpls->smpls_label; 247 if (label > MPLS_LABEL_MAX || label <= MPLS_LABEL_RESERVED_MAX) 248 return (EINVAL); 249 250 switch (sa->sa_family) { 251 case AF_INET: { 252 struct sockaddr_in *sin = (struct sockaddr_in *)sa; 253 254 if (in_nullhost(sin->sin_addr) || 255 IN_MULTICAST(sin->sin_addr.s_addr)) 256 return (EINVAL); 257 258 break; 259 } 260 #ifdef INET6 261 case AF_INET6: { 262 struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sa; 263 264 if (IN6_IS_ADDR_UNSPECIFIED(&sin6->sin6_addr) || 265 IN6_IS_ADDR_MULTICAST(&sin6->sin6_addr)) 266 return (EINVAL); 267 268 /* check scope */ 269 270 break; 271 } 272 #endif 273 default: 274 return (EAFNOSUPPORT); 275 } 276 277 if (sc->sc_dead) 278 return (ENXIO); 279 280 n = malloc(sizeof(*n), M_DEVBUF, M_WAITOK|M_CANFAIL|M_ZERO); 281 if (n == NULL) 282 return (ENOMEM); 283 284 n->n_rshim.shim_label = MPLS_LABEL2SHIM(label); 285 n->n_nexthop = req->addr; 286 287 o = sc->sc_neighbor; 288 sc->sc_neighbor = n; 289 290 NET_UNLOCK(); 291 ifq_barrier(&sc->sc_if.if_snd); 292 NET_LOCK(); 293 294 free(o, M_DEVBUF, sizeof(*o)); 295 296 return (0); 297 } 298 299 static int 300 mpip_get_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 301 { 302 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 303 struct mpip_neighbor *n = sc->sc_neighbor; 304 305 if (n == NULL) 306 return (EADDRNOTAVAIL); 307 308 smpls->smpls_len = sizeof(*smpls); 309 smpls->smpls_family = AF_MPLS; 310 smpls->smpls_label = MPLS_SHIM2LABEL(n->n_rshim.shim_label); 311 req->addr = n->n_nexthop; 312 313 return (0); 314 } 315 316 static int 317 mpip_del_neighbor(struct mpip_softc *sc, struct ifreq *req) 318 { 319 struct mpip_neighbor *o; 320 321 if (sc->sc_dead) 322 return (ENXIO); 323 324 o = sc->sc_neighbor; 325 sc->sc_neighbor = NULL; 326 327 NET_UNLOCK(); 328 ifq_barrier(&sc->sc_if.if_snd); 329 NET_LOCK(); 330 331 free(o, M_DEVBUF, sizeof(*o)); 332 333 return (0); 334 } 335 336 int 337 mpip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 338 { 339 struct mpip_softc *sc = ifp->if_softc; 340 struct ifreq *ifr = (struct ifreq *)data; 341 int error = 0; 342 343 switch (cmd) { 344 case SIOCSIFADDR: 345 break; 346 case SIOCSIFFLAGS: 347 if ((ifp->if_flags & IFF_UP)) 348 ifp->if_flags |= IFF_RUNNING; 349 else 350 ifp->if_flags &= ~IFF_RUNNING; 351 break; 352 case SIOCSIFMTU: 353 if (ifr->ifr_mtu < 60 || /* XXX */ 354 ifr->ifr_mtu > 65536) /* XXX */ 355 error = EINVAL; 356 else 357 ifp->if_mtu = ifr->ifr_mtu; 358 break; 359 360 case SIOCGPWE3: 361 ifr->ifr_pwe3 = IF_PWE3_IP; 362 break; 363 case SIOCSPWE3CTRLWORD: 364 sc->sc_cword = ifr->ifr_pwe3 ? 1 : 0; 365 break; 366 case SIOCGPWE3CTRLWORD: 367 ifr->ifr_pwe3 = sc->sc_cword; 368 break; 369 case SIOCSPWE3FAT: 370 sc->sc_fword = ifr->ifr_pwe3 ? 1 : 0; 371 break; 372 case SIOCGPWE3FAT: 373 ifr->ifr_pwe3 = sc->sc_fword; 374 break; 375 376 case SIOCSETLABEL: 377 error = mpip_set_label(sc, ifr); 378 break; 379 case SIOCGETLABEL: 380 error = mpip_get_label(sc, ifr); 381 break; 382 case SIOCDELLABEL: 383 error = mpip_del_label(sc); 384 break; 385 386 case SIOCSPWE3NEIGHBOR: 387 error = mpip_set_neighbor(sc, (struct if_laddrreq *)data); 388 break; 389 case SIOCGPWE3NEIGHBOR: 390 error = mpip_get_neighbor(sc, (struct if_laddrreq *)data); 391 break; 392 case SIOCDPWE3NEIGHBOR: 393 error = mpip_del_neighbor(sc, ifr); 394 break; 395 396 case SIOCSLIFPHYRTABLE: 397 if (ifr->ifr_rdomainid < 0 || 398 ifr->ifr_rdomainid > RT_TABLEID_MAX || 399 !rtable_exists(ifr->ifr_rdomainid) || 400 ifr->ifr_rdomainid != rtable_l2(ifr->ifr_rdomainid)) { 401 error = EINVAL; 402 break; 403 } 404 if (sc->sc_rdomain != ifr->ifr_rdomainid) { 405 error = mpip_set_route(sc, sc->sc_smpls.smpls_label, 406 ifr->ifr_rdomainid); 407 } 408 break; 409 case SIOCGLIFPHYRTABLE: 410 ifr->ifr_rdomainid = sc->sc_rdomain; 411 break; 412 413 case SIOCSLIFPHYTTL: 414 if (ifr->ifr_ttl != -1 && 415 (ifr->ifr_ttl < 1 || ifr->ifr_ttl > 0xff)) { 416 error = EINVAL; 417 break; 418 } 419 420 /* commit */ 421 sc->sc_ttl = ifr->ifr_ttl; 422 break; 423 case SIOCGLIFPHYTTL: 424 ifr->ifr_ttl = sc->sc_ttl; 425 break; 426 427 case SIOCSTXHPRIO: 428 error = if_txhprio_l3_check(ifr->ifr_hdrprio); 429 if (error != 0) 430 break; 431 432 sc->sc_txhprio = ifr->ifr_hdrprio; 433 break; 434 case SIOCGTXHPRIO: 435 ifr->ifr_hdrprio = sc->sc_txhprio; 436 break; 437 438 case SIOCSRXHPRIO: 439 error = if_rxhprio_l3_check(ifr->ifr_hdrprio); 440 if (error != 0) 441 break; 442 443 sc->sc_rxhprio = ifr->ifr_hdrprio; 444 break; 445 case SIOCGRXHPRIO: 446 ifr->ifr_hdrprio = sc->sc_rxhprio; 447 break; 448 449 case SIOCADDMULTI: 450 case SIOCDELMULTI: 451 break; 452 453 default: 454 error = ENOTTY; 455 break; 456 } 457 458 return (error); 459 } 460 461 static void 462 mpip_input(struct mpip_softc *sc, struct mbuf *m) 463 { 464 struct ifnet *ifp = &sc->sc_if; 465 int rxprio = sc->sc_rxhprio; 466 uint32_t shim, exp; 467 struct mbuf *n; 468 uint8_t ttl, tos; 469 470 if (!ISSET(ifp->if_flags, IFF_RUNNING)) 471 goto drop; 472 473 shim = *mtod(m, uint32_t *); 474 m_adj(m, sizeof(shim)); 475 476 ttl = ntohl(shim & MPLS_TTL_MASK); 477 exp = ntohl(shim & MPLS_EXP_MASK) >> MPLS_EXP_OFFSET; 478 479 if (sc->sc_fword) { 480 uint32_t label; 481 482 if (MPLS_BOS_ISSET(shim)) 483 goto drop; 484 485 if (m->m_len < sizeof(shim)) { 486 m = m_pullup(m, sizeof(shim)); 487 if (m == NULL) 488 return; 489 } 490 491 shim = *mtod(m, uint32_t *); 492 if (!MPLS_BOS_ISSET(shim)) 493 goto drop; 494 495 label = MPLS_SHIM2LABEL(shim); 496 if (label <= MPLS_LABEL_RESERVED_MAX) { 497 counters_inc(ifp->if_counters, ifc_noproto); /* ? */ 498 goto drop; 499 } 500 501 label -= MPLS_LABEL_RESERVED_MAX + 1; 502 label ^= sc->sc_flow; 503 SET(m->m_pkthdr.csum_flags, M_FLOWID); 504 m->m_pkthdr.ph_flowid = label; 505 506 m_adj(m, sizeof(shim)); 507 } else if (!MPLS_BOS_ISSET(shim)) 508 goto drop; 509 510 if (sc->sc_cword) { 511 if (m->m_len < sizeof(shim)) { 512 m = m_pullup(m, sizeof(shim)); 513 if (m == NULL) 514 return; 515 } 516 shim = *mtod(m, uint32_t *); 517 518 /* 519 * The first 4 bits identifies that this packet is a 520 * control word. If the control word is configured and 521 * we received an IP datagram we shall drop it. 522 */ 523 if (shim & CW_ZERO_MASK) { 524 counters_inc(ifp->if_counters, ifc_ierrors); 525 goto drop; 526 } 527 528 /* We don't support fragmentation just yet. */ 529 if (shim & CW_FRAG_MASK) { 530 counters_inc(ifp->if_counters, ifc_ierrors); 531 goto drop; 532 } 533 534 m_adj(m, sizeof(shim)); 535 } 536 537 n = m; 538 while (n->m_len == 0) { 539 n = n->m_next; 540 if (n == NULL) 541 goto drop; 542 } 543 544 switch (*mtod(n, uint8_t *) >> 4) { 545 case 4: { 546 struct ip *ip; 547 if (m->m_len < sizeof(*ip)) { 548 m = m_pullup(m, sizeof(*ip)); 549 if (m == NULL) 550 return; 551 } 552 ip = mtod(m, struct ip *); 553 tos = ip->ip_tos; 554 555 if (sc->sc_ttl == -1) { 556 m = mpls_ip_adjttl(m, ttl); 557 if (m == NULL) 558 return; 559 } 560 561 m->m_pkthdr.ph_family = AF_INET; 562 break; 563 } 564 #ifdef INET6 565 case 6: { 566 struct ip6_hdr *ip6; 567 uint32_t flow; 568 if (m->m_len < sizeof(*ip6)) { 569 m = m_pullup(m, sizeof(*ip6)); 570 if (m == NULL) 571 return; 572 } 573 ip6 = mtod(m, struct ip6_hdr *); 574 flow = bemtoh32(&ip6->ip6_flow); 575 tos = flow >> 20; 576 577 if (sc->sc_ttl == -1) { 578 m = mpls_ip6_adjttl(m, ttl); 579 if (m == NULL) 580 return; 581 } 582 583 m->m_pkthdr.ph_family = AF_INET6; 584 break; 585 } 586 #endif /* INET6 */ 587 default: 588 counters_inc(ifp->if_counters, ifc_noproto); 589 goto drop; 590 } 591 592 switch (rxprio) { 593 case IF_HDRPRIO_PACKET: 594 /* nop */ 595 break; 596 case IF_HDRPRIO_OUTER: 597 m->m_pkthdr.pf.prio = exp; 598 break; 599 case IF_HDRPRIO_PAYLOAD: 600 m->m_pkthdr.pf.prio = IFQ_TOS2PRIO(tos); 601 break; 602 default: 603 m->m_pkthdr.pf.prio = rxprio; 604 break; 605 } 606 607 if_vinput(ifp, m); 608 return; 609 drop: 610 m_freem(m); 611 } 612 613 int 614 mpip_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst, 615 struct rtentry *rt) 616 { 617 struct mpip_softc *sc = ifp->if_softc; 618 int error; 619 620 if (dst->sa_family == AF_LINK && 621 rt != NULL && ISSET(rt->rt_flags, RTF_LOCAL)) { 622 mpip_input(sc, m); 623 return (0); 624 } 625 626 if (!ISSET(ifp->if_flags, IFF_RUNNING)) { 627 error = ENETDOWN; 628 goto drop; 629 } 630 631 switch (dst->sa_family) { 632 case AF_INET: 633 #ifdef INET6 634 case AF_INET6: 635 #endif 636 break; 637 default: 638 error = EAFNOSUPPORT; 639 goto drop; 640 } 641 642 m->m_pkthdr.ph_family = dst->sa_family; 643 644 error = if_enqueue(ifp, m); 645 if (error) 646 counters_inc(ifp->if_counters, ifc_oerrors); 647 return (error); 648 649 drop: 650 m_freem(m); 651 return (error); 652 } 653 654 void 655 mpip_start(struct ifnet *ifp) 656 { 657 struct mpip_softc *sc = ifp->if_softc; 658 struct mpip_neighbor *n = sc->sc_neighbor; 659 struct rtentry *rt; 660 struct ifnet *ifp0; 661 struct mbuf *m; 662 uint32_t shim; 663 struct sockaddr_mpls smpls = { 664 .smpls_len = sizeof(smpls), 665 .smpls_family = AF_MPLS, 666 }; 667 int txprio = sc->sc_txhprio; 668 uint32_t exp, bos; 669 uint8_t tos, prio, ttl; 670 671 if (!ISSET(ifp->if_flags, IFF_RUNNING) || n == NULL) { 672 ifq_purge(&ifp->if_snd); 673 return; 674 } 675 676 rt = rtalloc(sstosa(&n->n_nexthop), RT_RESOLVE, sc->sc_rdomain); 677 if (!rtisvalid(rt)) { 678 ifq_purge(&ifp->if_snd); 679 goto rtfree; 680 } 681 682 ifp0 = if_get(rt->rt_ifidx); 683 if (ifp0 == NULL) { 684 ifq_purge(&ifp->if_snd); 685 goto rtfree; 686 } 687 688 while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) { 689 #if NBPFILTER > 0 690 caddr_t if_bpf = sc->sc_if.if_bpf; 691 if (if_bpf) { 692 bpf_mtap_af(if_bpf, m->m_pkthdr.ph_family, 693 m, BPF_DIRECTION_OUT); 694 } 695 #endif /* NBPFILTER */ 696 697 if (sc->sc_ttl == -1) { 698 switch (m->m_pkthdr.ph_family) { 699 case AF_INET: { 700 struct ip *ip; 701 ip = mtod(m, struct ip *); 702 ttl = ip->ip_ttl; 703 break; 704 } 705 #ifdef INET6 706 case AF_INET6: { 707 struct ip6_hdr *ip6; 708 ip6 = mtod(m, struct ip6_hdr *); 709 ttl = ip6->ip6_hlim; 710 break; 711 } 712 #endif 713 default: 714 unhandled_af(m->m_pkthdr.ph_family); 715 } 716 } else 717 ttl = mpls_defttl; 718 719 switch (txprio) { 720 case IF_HDRPRIO_PACKET: 721 prio = m->m_pkthdr.pf.prio; 722 break; 723 case IF_HDRPRIO_PAYLOAD: 724 switch (m->m_pkthdr.ph_family) { 725 case AF_INET: { 726 struct ip *ip; 727 ip = mtod(m, struct ip *); 728 tos = ip->ip_tos; 729 break; 730 } 731 #ifdef INET6 732 case AF_INET6: { 733 struct ip6_hdr *ip6; 734 uint32_t flow; 735 ip6 = mtod(m, struct ip6_hdr *); 736 flow = bemtoh32(&ip6->ip6_flow); 737 tos = flow >> 20; 738 break; 739 } 740 #endif 741 default: 742 unhandled_af(m->m_pkthdr.ph_family); 743 } 744 745 prio = IFQ_TOS2PRIO(tos); 746 break; 747 default: 748 prio = txprio; 749 break; 750 } 751 exp = htonl(prio << MPLS_EXP_OFFSET); 752 753 if (sc->sc_cword) { 754 m = m_prepend(m, sizeof(shim), M_NOWAIT); 755 if (m == NULL) 756 continue; 757 758 *mtod(m, uint32_t *) = 0; 759 } 760 761 bos = MPLS_BOS_MASK; 762 763 if (sc->sc_fword) { 764 uint32_t flow = 0; 765 m = m_prepend(m, sizeof(shim), M_NOWAIT); 766 if (m == NULL) 767 continue; 768 769 if (ISSET(m->m_pkthdr.csum_flags, M_FLOWID)) 770 flow = m->m_pkthdr.ph_flowid; 771 flow ^= sc->sc_flow; 772 flow += MPLS_LABEL_RESERVED_MAX + 1; 773 774 shim = htonl(1) & MPLS_TTL_MASK; 775 shim |= htonl(flow << MPLS_LABEL_OFFSET) & 776 MPLS_LABEL_MASK; 777 shim |= exp | bos; 778 *mtod(m, uint32_t *) = shim; 779 780 bos = 0; 781 } 782 783 m = m_prepend(m, sizeof(shim), M_NOWAIT); 784 if (m == NULL) 785 continue; 786 787 shim = htonl(ttl) & MPLS_TTL_MASK; 788 shim |= n->n_rshim.shim_label; 789 shim |= exp | bos; 790 *mtod(m, uint32_t *) = shim; 791 792 m->m_pkthdr.ph_rtableid = sc->sc_rdomain; 793 CLR(m->m_flags, M_BCAST|M_MCAST); 794 795 mpls_output(ifp0, m, (struct sockaddr *)&smpls, rt); 796 } 797 798 if_put(ifp0); 799 rtfree: 800 rtfree(rt); 801 } 802