1 /* $OpenBSD: if_mpip.c,v 1.7 2019/04/19 07:39: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_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 IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN); 121 122 if_attach(ifp); 123 if_counters_alloc(ifp); 124 if_alloc_sadl(ifp); 125 126 #if NBPFILTER > 0 127 bpfattach(&ifp->if_bpf, ifp, DLT_LOOP, sizeof(uint32_t)); 128 #endif 129 130 sc->sc_ifa.ifa_ifp = ifp; 131 sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl); 132 133 return (0); 134 } 135 136 int 137 mpip_clone_destroy(struct ifnet *ifp) 138 { 139 struct mpip_softc *sc = ifp->if_softc; 140 141 NET_LOCK(); 142 ifp->if_flags &= ~IFF_RUNNING; 143 sc->sc_dead = 1; 144 145 if (sc->sc_smpls.smpls_label) { 146 rt_ifa_del(&sc->sc_ifa, RTF_LOCAL | RTF_MPLS, 147 smplstosa(&sc->sc_smpls), 0); 148 } 149 NET_UNLOCK(); 150 151 ifq_barrier(&ifp->if_snd); 152 153 if_detach(ifp); 154 155 free(sc->sc_neighbor, M_DEVBUF, sizeof(*sc->sc_neighbor)); 156 free(sc, M_DEVBUF, sizeof(*sc)); 157 158 return (0); 159 } 160 161 static int 162 mpip_set_route(struct mpip_softc *sc, uint32_t shim, unsigned int rdomain) 163 { 164 int error; 165 166 rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 167 smplstosa(&sc->sc_smpls), 0); 168 169 sc->sc_smpls.smpls_label = shim; 170 sc->sc_rdomain = rdomain; 171 172 error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 173 smplstosa(&sc->sc_smpls), 0); 174 if (error) { 175 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 176 return (error); 177 } 178 179 return (0); 180 } 181 182 static int 183 mpip_set_label(struct mpip_softc *sc, struct ifreq *ifr) 184 { 185 struct shim_hdr label; 186 uint32_t shim; 187 int error; 188 189 error = copyin(ifr->ifr_data, &label, sizeof(label)); 190 if (error != 0) 191 return (error); 192 193 if (label.shim_label > MPLS_LABEL_MAX || 194 label.shim_label <= MPLS_LABEL_RESERVED_MAX) 195 return (EINVAL); 196 197 shim = MPLS_LABEL2SHIM(label.shim_label); 198 199 if (sc->sc_smpls.smpls_label == shim) 200 return (0); 201 202 return (mpip_set_route(sc, shim, sc->sc_rdomain)); 203 } 204 205 static int 206 mpip_get_label(struct mpip_softc *sc, struct ifreq *ifr) 207 { 208 struct shim_hdr label; 209 210 label.shim_label = MPLS_SHIM2LABEL(sc->sc_smpls.smpls_label); 211 212 if (label.shim_label == 0) 213 return (EADDRNOTAVAIL); 214 215 return (copyout(&label, ifr->ifr_data, sizeof(label))); 216 } 217 218 static int 219 mpip_del_label(struct mpip_softc *sc) 220 { 221 if (sc->sc_smpls.smpls_label != MPLS_LABEL2SHIM(0)) { 222 rt_ifa_del(&sc->sc_ifa, RTF_MPLS | RTF_LOCAL, 223 smplstosa(&sc->sc_smpls), 0); 224 } 225 226 sc->sc_smpls.smpls_label = MPLS_LABEL2SHIM(0); 227 228 return (0); 229 } 230 231 static int 232 mpip_set_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 233 { 234 struct mpip_neighbor *n, *o; 235 struct sockaddr *sa = (struct sockaddr *)&req->addr; 236 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 237 uint32_t label; 238 239 if (smpls->smpls_family != AF_MPLS) 240 return (EINVAL); 241 label = smpls->smpls_label; 242 if (label > MPLS_LABEL_MAX || label <= MPLS_LABEL_RESERVED_MAX) 243 return (EINVAL); 244 245 switch (sa->sa_family) { 246 case AF_INET: { 247 struct sockaddr_in *sin = (struct sockaddr_in *)sa; 248 249 if (in_nullhost(sin->sin_addr) || 250 IN_MULTICAST(sin->sin_addr.s_addr)) 251 return (EINVAL); 252 253 break; 254 } 255 #ifdef INET6 256 case AF_INET6: { 257 struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)sa; 258 259 if (IN6_IS_ADDR_UNSPECIFIED(&sin6->sin6_addr) || 260 IN6_IS_ADDR_MULTICAST(&sin6->sin6_addr)) 261 return (EINVAL); 262 263 /* check scope */ 264 265 break; 266 } 267 #endif 268 default: 269 return (EAFNOSUPPORT); 270 } 271 272 if (sc->sc_dead) 273 return (ENXIO); 274 275 n = malloc(sizeof(*n), M_DEVBUF, M_WAITOK|M_CANFAIL|M_ZERO); 276 if (n == NULL) 277 return (ENOMEM); 278 279 n->n_rshim.shim_label = MPLS_LABEL2SHIM(label); 280 n->n_nexthop = req->addr; 281 282 o = sc->sc_neighbor; 283 sc->sc_neighbor = n; 284 285 NET_UNLOCK(); 286 ifq_barrier(&sc->sc_if.if_snd); 287 NET_LOCK(); 288 289 free(o, M_DEVBUF, sizeof(*o)); 290 291 return (0); 292 } 293 294 static int 295 mpip_get_neighbor(struct mpip_softc *sc, struct if_laddrreq *req) 296 { 297 struct sockaddr_mpls *smpls = (struct sockaddr_mpls *)&req->dstaddr; 298 struct mpip_neighbor *n = sc->sc_neighbor; 299 300 if (n == NULL) 301 return (EADDRNOTAVAIL); 302 303 smpls->smpls_len = sizeof(*smpls); 304 smpls->smpls_family = AF_MPLS; 305 smpls->smpls_label = MPLS_SHIM2LABEL(n->n_rshim.shim_label); 306 req->addr = n->n_nexthop; 307 308 return (0); 309 } 310 311 static int 312 mpip_del_neighbor(struct mpip_softc *sc, struct ifreq *req) 313 { 314 struct mpip_neighbor *o; 315 316 if (sc->sc_dead) 317 return (ENXIO); 318 319 o = sc->sc_neighbor; 320 sc->sc_neighbor = NULL; 321 322 NET_UNLOCK(); 323 ifq_barrier(&sc->sc_if.if_snd); 324 NET_LOCK(); 325 326 free(o, M_DEVBUF, sizeof(*o)); 327 328 return (0); 329 } 330 331 int 332 mpip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 333 { 334 struct mpip_softc *sc = ifp->if_softc; 335 struct ifreq *ifr = (struct ifreq *)data; 336 int error = 0; 337 338 switch (cmd) { 339 case SIOCSIFADDR: 340 break; 341 case SIOCSIFFLAGS: 342 if ((ifp->if_flags & IFF_UP)) 343 ifp->if_flags |= IFF_RUNNING; 344 else 345 ifp->if_flags &= ~IFF_RUNNING; 346 break; 347 case SIOCSIFMTU: 348 if (ifr->ifr_mtu < 60 || /* XXX */ 349 ifr->ifr_mtu > 65536) /* XXX */ 350 error = EINVAL; 351 else 352 ifp->if_mtu = ifr->ifr_mtu; 353 break; 354 355 case SIOCGPWE3: 356 ifr->ifr_pwe3 = IF_PWE3_IP; 357 break; 358 case SIOCSPWE3CTRLWORD: 359 sc->sc_cword = ifr->ifr_pwe3 ? 1 : 0; 360 break; 361 case SIOCGPWE3CTRLWORD: 362 ifr->ifr_pwe3 = sc->sc_cword; 363 break; 364 case SIOCSPWE3FAT: 365 sc->sc_fword = ifr->ifr_pwe3 ? 1 : 0; 366 break; 367 case SIOCGPWE3FAT: 368 ifr->ifr_pwe3 = sc->sc_fword; 369 break; 370 371 case SIOCSETLABEL: 372 error = mpip_set_label(sc, ifr); 373 break; 374 case SIOCGETLABEL: 375 error = mpip_get_label(sc, ifr); 376 break; 377 case SIOCDELLABEL: 378 error = mpip_del_label(sc); 379 break; 380 381 case SIOCSPWE3NEIGHBOR: 382 error = mpip_set_neighbor(sc, (struct if_laddrreq *)data); 383 break; 384 case SIOCGPWE3NEIGHBOR: 385 error = mpip_get_neighbor(sc, (struct if_laddrreq *)data); 386 break; 387 case SIOCDPWE3NEIGHBOR: 388 error = mpip_del_neighbor(sc, ifr); 389 break; 390 391 case SIOCSLIFPHYRTABLE: 392 if (ifr->ifr_rdomainid < 0 || 393 ifr->ifr_rdomainid > RT_TABLEID_MAX || 394 !rtable_exists(ifr->ifr_rdomainid) || 395 ifr->ifr_rdomainid != rtable_l2(ifr->ifr_rdomainid)) { 396 error = EINVAL; 397 break; 398 } 399 if (sc->sc_rdomain != ifr->ifr_rdomainid) { 400 error = mpip_set_route(sc, sc->sc_smpls.smpls_label, 401 ifr->ifr_rdomainid); 402 } 403 break; 404 case SIOCGLIFPHYRTABLE: 405 ifr->ifr_rdomainid = sc->sc_rdomain; 406 break; 407 408 case SIOCSLIFPHYTTL: 409 if (ifr->ifr_ttl != -1 && 410 (ifr->ifr_ttl < 1 || ifr->ifr_ttl > 0xff)) { 411 error = EINVAL; 412 break; 413 } 414 415 /* commit */ 416 sc->sc_ttl = ifr->ifr_ttl; 417 break; 418 case SIOCGLIFPHYTTL: 419 ifr->ifr_ttl = sc->sc_ttl; 420 break; 421 422 case SIOCSTXHPRIO: 423 error = if_txhprio_l3_check(ifr->ifr_hdrprio); 424 if (error != 0) 425 break; 426 427 sc->sc_txhprio = ifr->ifr_hdrprio; 428 break; 429 case SIOCGTXHPRIO: 430 ifr->ifr_hdrprio = sc->sc_txhprio; 431 break; 432 433 case SIOCSRXHPRIO: 434 error = if_rxhprio_l3_check(ifr->ifr_hdrprio); 435 if (error != 0) 436 break; 437 438 sc->sc_rxhprio = ifr->ifr_hdrprio; 439 break; 440 case SIOCGRXHPRIO: 441 ifr->ifr_hdrprio = sc->sc_rxhprio; 442 break; 443 444 case SIOCADDMULTI: 445 case SIOCDELMULTI: 446 break; 447 448 default: 449 error = ENOTTY; 450 break; 451 } 452 453 return (error); 454 } 455 456 static void 457 mpip_input(struct mpip_softc *sc, struct mbuf *m) 458 { 459 struct ifnet *ifp = &sc->sc_if; 460 int rxprio = sc->sc_rxhprio; 461 uint32_t shim, exp; 462 struct mbuf *n; 463 uint8_t ttl, tos; 464 void (*input)(struct ifnet *, struct mbuf *); 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 m->m_pkthdr.ph_flowid = M_FLOWID_VALID | 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 #if NPF > 0 606 pf_pkt_addr_changed(m); 607 #endif 608 609 #if NBPFILTER > 0 610 { 611 caddr_t if_bpf = ifp->if_bpf; 612 if (if_bpf) { 613 bpf_mtap_af(if_bpf, m->m_pkthdr.ph_family, 614 m, BPF_DIRECTION_IN); 615 } 616 } 617 #endif 618 619 (*input)(ifp, m); 620 return; 621 drop: 622 m_freem(m); 623 } 624 625 int 626 mpip_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst, 627 struct rtentry *rt) 628 { 629 struct mpip_softc *sc = ifp->if_softc; 630 int error; 631 632 if (dst->sa_family == AF_LINK && 633 rt != NULL && ISSET(rt->rt_flags, RTF_LOCAL)) { 634 mpip_input(sc, m); 635 return (0); 636 } 637 638 if (!ISSET(ifp->if_flags, IFF_RUNNING)) { 639 error = ENETDOWN; 640 goto drop; 641 } 642 643 switch (dst->sa_family) { 644 case AF_INET: 645 #ifdef INET6 646 case AF_INET6: 647 #endif 648 break; 649 default: 650 error = EAFNOSUPPORT; 651 goto drop; 652 } 653 654 m->m_pkthdr.ph_family = dst->sa_family; 655 656 error = if_enqueue(ifp, m); 657 if (error) 658 counters_inc(ifp->if_counters, ifc_oerrors); 659 return (error); 660 661 drop: 662 m_freem(m); 663 return (error); 664 } 665 666 void 667 mpip_start(struct ifnet *ifp) 668 { 669 struct mpip_softc *sc = ifp->if_softc; 670 struct mpip_neighbor *n = sc->sc_neighbor; 671 struct rtentry *rt; 672 struct ifnet *ifp0; 673 struct mbuf *m; 674 uint32_t shim; 675 struct sockaddr_mpls smpls = { 676 .smpls_len = sizeof(smpls), 677 .smpls_family = AF_MPLS, 678 }; 679 int txprio = sc->sc_txhprio; 680 uint32_t exp, bos; 681 uint8_t tos, prio, ttl; 682 683 if (!ISSET(ifp->if_flags, IFF_RUNNING) || n == NULL) { 684 IFQ_PURGE(&ifp->if_snd); 685 return; 686 } 687 688 rt = rtalloc(sstosa(&n->n_nexthop), RT_RESOLVE, sc->sc_rdomain); 689 if (!rtisvalid(rt)) { 690 IFQ_PURGE(&ifp->if_snd); 691 goto rtfree; 692 } 693 694 ifp0 = if_get(rt->rt_ifidx); 695 if (ifp0 == NULL) { 696 IFQ_PURGE(&ifp->if_snd); 697 goto rtfree; 698 } 699 700 while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) { 701 #if NBPFILTER > 0 702 caddr_t if_bpf = sc->sc_if.if_bpf; 703 if (if_bpf) { 704 bpf_mtap_af(if_bpf, m->m_pkthdr.ph_family, 705 m, BPF_DIRECTION_OUT); 706 } 707 #endif /* NBPFILTER */ 708 709 if (sc->sc_ttl == -1) { 710 switch (m->m_pkthdr.ph_family) { 711 case AF_INET: { 712 struct ip *ip; 713 ip = mtod(m, struct ip *); 714 ttl = ip->ip_ttl; 715 break; 716 } 717 #ifdef INET6 718 case AF_INET6: { 719 struct ip6_hdr *ip6; 720 ip6 = mtod(m, struct ip6_hdr *); 721 ttl = ip6->ip6_hlim; 722 break; 723 } 724 #endif 725 default: 726 unhandled_af(m->m_pkthdr.ph_family); 727 } 728 } else 729 ttl = mpls_defttl; 730 731 switch (txprio) { 732 case IF_HDRPRIO_PACKET: 733 prio = m->m_pkthdr.pf.prio; 734 break; 735 case IF_HDRPRIO_PAYLOAD: 736 switch (m->m_pkthdr.ph_family) { 737 case AF_INET: { 738 struct ip *ip; 739 ip = mtod(m, struct ip *); 740 tos = ip->ip_tos; 741 break; 742 } 743 #ifdef INET6 744 case AF_INET6: { 745 struct ip6_hdr *ip6; 746 uint32_t flow; 747 ip6 = mtod(m, struct ip6_hdr *); 748 flow = bemtoh32(&ip6->ip6_flow); 749 tos = flow >> 20; 750 break; 751 } 752 #endif 753 default: 754 unhandled_af(m->m_pkthdr.ph_family); 755 } 756 757 prio = IFQ_TOS2PRIO(tos); 758 break; 759 default: 760 prio = txprio; 761 break; 762 } 763 exp = htonl(prio << MPLS_EXP_OFFSET); 764 765 if (sc->sc_cword) { 766 m = m_prepend(m, sizeof(shim), M_NOWAIT); 767 if (m == NULL) 768 continue; 769 770 *mtod(m, uint32_t *) = 0; 771 } 772 773 bos = MPLS_BOS_MASK; 774 775 if (sc->sc_fword) { 776 uint32_t flow = 0; 777 m = m_prepend(m, sizeof(shim), M_NOWAIT); 778 if (m == NULL) 779 continue; 780 781 if (ISSET(m->m_pkthdr.ph_flowid, M_FLOWID_VALID)) 782 flow = m->m_pkthdr.ph_flowid & M_FLOWID_MASK; 783 flow ^= sc->sc_flow; 784 flow += MPLS_LABEL_RESERVED_MAX + 1; 785 786 shim = htonl(1) & MPLS_TTL_MASK; 787 shim |= htonl(flow << MPLS_LABEL_OFFSET) & 788 MPLS_LABEL_MASK; 789 shim |= exp | bos; 790 *mtod(m, uint32_t *) = shim; 791 792 bos = 0; 793 } 794 795 m = m_prepend(m, sizeof(shim), M_NOWAIT); 796 if (m == NULL) 797 continue; 798 799 shim = htonl(ttl) & MPLS_TTL_MASK; 800 shim |= n->n_rshim.shim_label; 801 shim |= exp | bos; 802 *mtod(m, uint32_t *) = shim; 803 804 m->m_pkthdr.ph_rtableid = sc->sc_rdomain; 805 CLR(m->m_flags, M_BCAST|M_MCAST); 806 807 mpls_output(ifp0, m, (struct sockaddr *)&smpls, rt); 808 } 809 810 if_put(ifp0); 811 rtfree: 812 rtfree(rt); 813 } 814