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