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