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