1 /* $OpenBSD: if_bpe.c,v 1.22 2023/12/23 10:52:54 bluhm Exp $ */ 2 /* 3 * Copyright (c) 2018 David Gwynne <dlg@openbsd.org> 4 * 5 * Permission to use, copy, modify, and distribute this software for any 6 * purpose with or without fee is hereby granted, provided that the above 7 * copyright notice and this permission notice appear in all copies. 8 * 9 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 10 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 11 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 12 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 13 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 14 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 15 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 16 */ 17 18 #include "bpfilter.h" 19 #include "pf.h" 20 21 #include <sys/param.h> 22 #include <sys/systm.h> 23 #include <sys/kernel.h> 24 #include <sys/mbuf.h> 25 #include <sys/socket.h> 26 #include <sys/ioctl.h> 27 #include <sys/timeout.h> 28 #include <sys/pool.h> 29 #include <sys/tree.h> 30 #include <sys/smr.h> 31 32 #include <net/if.h> 33 #include <net/if_var.h> 34 #include <net/if_dl.h> 35 #include <net/if_media.h> 36 #include <net/if_types.h> 37 #include <net/rtable.h> 38 39 #include <netinet/in.h> 40 #include <netinet/if_ether.h> 41 42 /* for bridge stuff */ 43 #include <net/if_bridge.h> 44 #include <net/if_etherbridge.h> 45 46 #if NBPFILTER > 0 47 #include <net/bpf.h> 48 #endif 49 50 #include <net/if_bpe.h> 51 52 #define PBB_ITAG_ISID 0x00ffffff 53 #define PBB_ITAG_ISID_MIN 0x00000000 54 #define PBB_ITAG_ISID_MAX 0x00ffffff 55 #define PBB_ITAG_RES2 0x03000000 /* must be zero on input */ 56 #define PBB_ITAG_RES1 0x04000000 /* ignore on input */ 57 #define PBB_ITAG_UCA 0x08000000 58 #define PBB_ITAG_DEI 0x10000000 59 #define PBB_ITAG_PCP_SHIFT 29 60 #define PBB_ITAG_PCP_MASK (0x7U << PBB_ITAG_PCP_SHIFT) 61 62 #define BPE_BRIDGE_AGE_TMO 100 /* seconds */ 63 64 struct bpe_key { 65 int k_if; 66 uint32_t k_isid; 67 68 RBT_ENTRY(bpe_tunnel) k_entry; 69 }; 70 71 RBT_HEAD(bpe_tree, bpe_key); 72 73 static inline int bpe_cmp(const struct bpe_key *, const struct bpe_key *); 74 75 RBT_PROTOTYPE(bpe_tree, bpe_key, k_entry, bpe_cmp); 76 RBT_GENERATE(bpe_tree, bpe_key, k_entry, bpe_cmp); 77 78 struct bpe_softc { 79 struct bpe_key sc_key; /* must be first */ 80 struct arpcom sc_ac; 81 int sc_txhprio; 82 int sc_rxhprio; 83 struct ether_addr sc_group; 84 85 struct task sc_ltask; 86 struct task sc_dtask; 87 88 struct etherbridge sc_eb; 89 }; 90 91 void bpeattach(int); 92 93 static int bpe_clone_create(struct if_clone *, int); 94 static int bpe_clone_destroy(struct ifnet *); 95 96 static void bpe_start(struct ifnet *); 97 static int bpe_ioctl(struct ifnet *, u_long, caddr_t); 98 static int bpe_media_get(struct bpe_softc *, struct ifreq *); 99 static int bpe_up(struct bpe_softc *); 100 static int bpe_down(struct bpe_softc *); 101 static int bpe_multi(struct bpe_softc *, struct ifnet *, u_long); 102 static int bpe_set_vnetid(struct bpe_softc *, const struct ifreq *); 103 static void bpe_set_group(struct bpe_softc *, uint32_t); 104 static int bpe_set_parent(struct bpe_softc *, const struct if_parent *); 105 static int bpe_get_parent(struct bpe_softc *, struct if_parent *); 106 static int bpe_del_parent(struct bpe_softc *); 107 static int bpe_add_addr(struct bpe_softc *, const struct ifbareq *); 108 static int bpe_del_addr(struct bpe_softc *, const struct ifbareq *); 109 110 static void bpe_link_hook(void *); 111 static void bpe_link_state(struct bpe_softc *, u_char, uint64_t); 112 static void bpe_detach_hook(void *); 113 114 static struct if_clone bpe_cloner = 115 IF_CLONE_INITIALIZER("bpe", bpe_clone_create, bpe_clone_destroy); 116 117 static int bpe_eb_port_eq(void *, void *, void *); 118 static void *bpe_eb_port_take(void *, void *); 119 static void bpe_eb_port_rele(void *, void *); 120 static size_t bpe_eb_port_ifname(void *, char *, size_t, void *); 121 static void bpe_eb_port_sa(void *, struct sockaddr_storage *, void *); 122 123 static const struct etherbridge_ops bpe_etherbridge_ops = { 124 bpe_eb_port_eq, 125 bpe_eb_port_take, 126 bpe_eb_port_rele, 127 bpe_eb_port_ifname, 128 bpe_eb_port_sa, 129 }; 130 131 static struct bpe_tree bpe_interfaces = RBT_INITIALIZER(); 132 static struct rwlock bpe_lock = RWLOCK_INITIALIZER("bpeifs"); 133 static struct pool bpe_endpoint_pool; 134 135 void 136 bpeattach(int count) 137 { 138 if_clone_attach(&bpe_cloner); 139 } 140 141 static int 142 bpe_clone_create(struct if_clone *ifc, int unit) 143 { 144 struct bpe_softc *sc; 145 struct ifnet *ifp; 146 int error; 147 148 if (bpe_endpoint_pool.pr_size == 0) { 149 pool_init(&bpe_endpoint_pool, sizeof(struct ether_addr), 0, 150 IPL_NONE, 0, "bpepl", NULL); 151 } 152 153 sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK|M_ZERO); 154 155 ifp = &sc->sc_ac.ac_if; 156 157 snprintf(ifp->if_xname, sizeof(ifp->if_xname), "%s%d", 158 ifc->ifc_name, unit); 159 160 error = etherbridge_init(&sc->sc_eb, ifp->if_xname, 161 &bpe_etherbridge_ops, sc); 162 if (error == -1) { 163 free(sc, M_DEVBUF, sizeof(*sc)); 164 return (error); 165 } 166 167 sc->sc_key.k_if = 0; 168 sc->sc_key.k_isid = 0; 169 bpe_set_group(sc, 0); 170 171 sc->sc_txhprio = IF_HDRPRIO_PACKET; 172 sc->sc_rxhprio = IF_HDRPRIO_OUTER; 173 174 task_set(&sc->sc_ltask, bpe_link_hook, sc); 175 task_set(&sc->sc_dtask, bpe_detach_hook, sc); 176 177 ifp->if_softc = sc; 178 ifp->if_hardmtu = ETHER_MAX_HARDMTU_LEN; 179 ifp->if_ioctl = bpe_ioctl; 180 ifp->if_start = bpe_start; 181 ifp->if_flags = IFF_BROADCAST | IFF_MULTICAST; 182 ifp->if_xflags = IFXF_CLONED; 183 ether_fakeaddr(ifp); 184 185 if_counters_alloc(ifp); 186 if_attach(ifp); 187 ether_ifattach(ifp); 188 189 return (0); 190 } 191 192 static int 193 bpe_clone_destroy(struct ifnet *ifp) 194 { 195 struct bpe_softc *sc = ifp->if_softc; 196 197 NET_LOCK(); 198 if (ISSET(ifp->if_flags, IFF_RUNNING)) 199 bpe_down(sc); 200 NET_UNLOCK(); 201 202 ether_ifdetach(ifp); 203 if_detach(ifp); 204 205 etherbridge_destroy(&sc->sc_eb); 206 207 free(sc, M_DEVBUF, sizeof(*sc)); 208 209 return (0); 210 } 211 212 static void 213 bpe_start(struct ifnet *ifp) 214 { 215 struct bpe_softc *sc = ifp->if_softc; 216 struct ifnet *ifp0; 217 struct mbuf *m0, *m; 218 struct ether_header *ceh; 219 struct ether_header *beh; 220 uint32_t itag, *itagp; 221 int hlen = sizeof(*beh) + sizeof(*itagp); 222 #if NBPFILTER > 0 223 caddr_t if_bpf; 224 #endif 225 int txprio; 226 uint8_t prio; 227 228 ifp0 = if_get(sc->sc_key.k_if); 229 if (ifp0 == NULL || !ISSET(ifp0->if_flags, IFF_RUNNING)) { 230 ifq_purge(&ifp->if_snd); 231 goto done; 232 } 233 234 txprio = sc->sc_txhprio; 235 236 while ((m0 = ifq_dequeue(&ifp->if_snd)) != NULL) { 237 #if NBPFILTER > 0 238 if_bpf = ifp->if_bpf; 239 if (if_bpf) 240 bpf_mtap_ether(if_bpf, m0, BPF_DIRECTION_OUT); 241 #endif 242 243 ceh = mtod(m0, struct ether_header *); 244 245 /* force prepend of a whole mbuf because of alignment */ 246 m = m_get(M_DONTWAIT, m0->m_type); 247 if (m == NULL) { 248 m_freem(m0); 249 continue; 250 } 251 252 M_MOVE_PKTHDR(m, m0); 253 m->m_next = m0; 254 255 m_align(m, 0); 256 m->m_len = 0; 257 258 m = m_prepend(m, hlen, M_DONTWAIT); 259 if (m == NULL) 260 continue; 261 262 beh = mtod(m, struct ether_header *); 263 264 if (ETHER_IS_BROADCAST(ceh->ether_dhost)) { 265 memcpy(beh->ether_dhost, &sc->sc_group, 266 sizeof(beh->ether_dhost)); 267 } else { 268 struct ether_addr *endpoint; 269 270 smr_read_enter(); 271 endpoint = etherbridge_resolve_ea(&sc->sc_eb, 272 (struct ether_addr *)ceh->ether_dhost); 273 if (endpoint == NULL) { 274 /* "flood" to unknown hosts */ 275 endpoint = &sc->sc_group; 276 } 277 memcpy(beh->ether_dhost, endpoint, 278 sizeof(beh->ether_dhost)); 279 smr_read_leave(); 280 } 281 282 memcpy(beh->ether_shost, ((struct arpcom *)ifp0)->ac_enaddr, 283 sizeof(beh->ether_shost)); 284 beh->ether_type = htons(ETHERTYPE_PBB); 285 286 prio = (txprio == IF_HDRPRIO_PACKET) ? 287 m->m_pkthdr.pf.prio : txprio; 288 289 itag = sc->sc_key.k_isid; 290 itag |= prio << PBB_ITAG_PCP_SHIFT; 291 itagp = (uint32_t *)(beh + 1); 292 293 htobem32(itagp, itag); 294 295 if_enqueue(ifp0, m); 296 } 297 298 done: 299 if_put(ifp0); 300 } 301 302 static int 303 bpe_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 304 { 305 struct bpe_softc *sc = ifp->if_softc; 306 struct ifreq *ifr = (struct ifreq *)data; 307 struct ifbrparam *bparam = (struct ifbrparam *)data; 308 int error = 0; 309 310 switch (cmd) { 311 case SIOCSIFFLAGS: 312 if (ISSET(ifp->if_flags, IFF_UP)) { 313 if (!ISSET(ifp->if_flags, IFF_RUNNING)) 314 error = bpe_up(sc); 315 else 316 error = 0; 317 } else { 318 if (ISSET(ifp->if_flags, IFF_RUNNING)) 319 error = bpe_down(sc); 320 } 321 break; 322 323 case SIOCSVNETID: 324 error = bpe_set_vnetid(sc, ifr); 325 break; 326 case SIOCGVNETID: 327 ifr->ifr_vnetid = sc->sc_key.k_isid; 328 break; 329 330 case SIOCSIFPARENT: 331 error = bpe_set_parent(sc, (struct if_parent *)data); 332 break; 333 case SIOCGIFPARENT: 334 error = bpe_get_parent(sc, (struct if_parent *)data); 335 break; 336 case SIOCDIFPARENT: 337 error = bpe_del_parent(sc); 338 break; 339 340 case SIOCSTXHPRIO: 341 error = if_txhprio_l2_check(ifr->ifr_hdrprio); 342 if (error != 0) 343 break; 344 345 sc->sc_txhprio = ifr->ifr_hdrprio; 346 break; 347 case SIOCGTXHPRIO: 348 ifr->ifr_hdrprio = sc->sc_txhprio; 349 break; 350 351 case SIOCSRXHPRIO: 352 error = if_rxhprio_l2_check(ifr->ifr_hdrprio); 353 if (error != 0) 354 break; 355 356 sc->sc_rxhprio = ifr->ifr_hdrprio; 357 break; 358 case SIOCGRXHPRIO: 359 ifr->ifr_hdrprio = sc->sc_rxhprio; 360 break; 361 362 case SIOCGIFMEDIA: 363 error = bpe_media_get(sc, ifr); 364 break; 365 366 case SIOCBRDGSCACHE: 367 error = suser(curproc); 368 if (error != 0) 369 break; 370 371 error = etherbridge_set_max(&sc->sc_eb, bparam); 372 break; 373 case SIOCBRDGGCACHE: 374 error = etherbridge_get_max(&sc->sc_eb, bparam); 375 break; 376 377 case SIOCBRDGSTO: 378 error = suser(curproc); 379 if (error != 0) 380 break; 381 382 error = etherbridge_set_tmo(&sc->sc_eb, bparam); 383 break; 384 case SIOCBRDGGTO: 385 error = etherbridge_get_tmo(&sc->sc_eb, bparam); 386 break; 387 388 case SIOCBRDGRTS: 389 error = etherbridge_rtfind(&sc->sc_eb, 390 (struct ifbaconf *)data); 391 break; 392 case SIOCBRDGFLUSH: 393 error = suser(curproc); 394 if (error != 0) 395 break; 396 397 etherbridge_flush(&sc->sc_eb, 398 ((struct ifbreq *)data)->ifbr_ifsflags); 399 break; 400 case SIOCBRDGSADDR: 401 error = bpe_add_addr(sc, (struct ifbareq *)data); 402 break; 403 case SIOCBRDGDADDR: 404 error = bpe_del_addr(sc, (struct ifbareq *)data); 405 break; 406 407 default: 408 error = ether_ioctl(ifp, &sc->sc_ac, cmd, data); 409 break; 410 } 411 412 return (error); 413 } 414 415 static int 416 bpe_media_get(struct bpe_softc *sc, struct ifreq *ifr) 417 { 418 struct ifnet *ifp0; 419 int error; 420 421 ifp0 = if_get(sc->sc_key.k_if); 422 if (ifp0 != NULL) 423 error = (*ifp0->if_ioctl)(ifp0, SIOCGIFMEDIA, (caddr_t)ifr); 424 else 425 error = ENOTTY; 426 if_put(ifp0); 427 428 return (error); 429 } 430 431 static int 432 bpe_up(struct bpe_softc *sc) 433 { 434 struct ifnet *ifp = &sc->sc_ac.ac_if; 435 struct ifnet *ifp0; 436 struct bpe_softc *osc; 437 int error; 438 u_int hardmtu; 439 u_int hlen = sizeof(struct ether_header) + sizeof(uint32_t); 440 441 KASSERT(!ISSET(ifp->if_flags, IFF_RUNNING)); 442 NET_ASSERT_LOCKED(); 443 444 error = etherbridge_up(&sc->sc_eb); 445 if (error != 0) 446 return (error); 447 448 ifp0 = if_get(sc->sc_key.k_if); 449 if (ifp0 == NULL) { 450 error = ENXIO; 451 goto down; 452 } 453 454 /* check again if bpe will work on top of the parent */ 455 if (ifp0->if_type != IFT_ETHER) { 456 error = EPROTONOSUPPORT; 457 goto put; 458 } 459 460 hardmtu = ifp0->if_hardmtu; 461 if (hardmtu < hlen) { 462 error = ENOBUFS; 463 goto put; 464 } 465 hardmtu -= hlen; 466 if (ifp->if_mtu > hardmtu) { 467 error = ENOBUFS; 468 goto put; 469 } 470 471 /* parent is fine, let's prepare the bpe to handle packets */ 472 ifp->if_hardmtu = hardmtu; 473 SET(ifp->if_flags, ifp0->if_flags & IFF_SIMPLEX); 474 475 /* commit the interface */ 476 error = rw_enter(&bpe_lock, RW_WRITE | RW_INTR); 477 if (error != 0) 478 goto scrub; 479 480 osc = (struct bpe_softc *)RBT_INSERT(bpe_tree, &bpe_interfaces, 481 (struct bpe_key *)sc); 482 rw_exit(&bpe_lock); 483 484 if (osc != NULL) { 485 error = EADDRINUSE; 486 goto scrub; 487 } 488 489 if (bpe_multi(sc, ifp0, SIOCADDMULTI) != 0) { 490 error = ENOTCONN; 491 goto remove; 492 } 493 494 /* Register callback for physical link state changes */ 495 if_linkstatehook_add(ifp0, &sc->sc_ltask); 496 497 /* Register callback if parent wants to unregister */ 498 if_detachhook_add(ifp0, &sc->sc_dtask); 499 500 /* we're running now */ 501 SET(ifp->if_flags, IFF_RUNNING); 502 bpe_link_state(sc, ifp0->if_link_state, ifp0->if_baudrate); 503 504 if_put(ifp0); 505 506 return (0); 507 508 remove: 509 rw_enter(&bpe_lock, RW_WRITE); 510 RBT_REMOVE(bpe_tree, &bpe_interfaces, (struct bpe_key *)sc); 511 rw_exit(&bpe_lock); 512 scrub: 513 CLR(ifp->if_flags, IFF_SIMPLEX); 514 ifp->if_hardmtu = 0xffff; 515 put: 516 if_put(ifp0); 517 down: 518 etherbridge_down(&sc->sc_eb); 519 520 return (error); 521 } 522 523 static int 524 bpe_down(struct bpe_softc *sc) 525 { 526 struct ifnet *ifp = &sc->sc_ac.ac_if; 527 struct ifnet *ifp0; 528 529 NET_ASSERT_LOCKED(); 530 531 CLR(ifp->if_flags, IFF_RUNNING); 532 533 ifp0 = if_get(sc->sc_key.k_if); 534 if (ifp0 != NULL) { 535 if_detachhook_del(ifp0, &sc->sc_dtask); 536 if_linkstatehook_del(ifp0, &sc->sc_ltask); 537 bpe_multi(sc, ifp0, SIOCDELMULTI); 538 } 539 if_put(ifp0); 540 541 rw_enter(&bpe_lock, RW_WRITE); 542 RBT_REMOVE(bpe_tree, &bpe_interfaces, (struct bpe_key *)sc); 543 rw_exit(&bpe_lock); 544 545 CLR(ifp->if_flags, IFF_SIMPLEX); 546 ifp->if_hardmtu = 0xffff; 547 548 etherbridge_down(&sc->sc_eb); 549 550 return (0); 551 } 552 553 static int 554 bpe_multi(struct bpe_softc *sc, struct ifnet *ifp0, u_long cmd) 555 { 556 struct ifreq ifr; 557 struct sockaddr *sa; 558 559 /* make it convincing */ 560 CTASSERT(sizeof(ifr.ifr_name) == sizeof(ifp0->if_xname)); 561 memcpy(ifr.ifr_name, ifp0->if_xname, sizeof(ifr.ifr_name)); 562 563 sa = &ifr.ifr_addr; 564 CTASSERT(sizeof(sa->sa_data) >= sizeof(sc->sc_group)); 565 566 sa->sa_family = AF_UNSPEC; 567 memcpy(sa->sa_data, &sc->sc_group, sizeof(sc->sc_group)); 568 569 return ((*ifp0->if_ioctl)(ifp0, cmd, (caddr_t)&ifr)); 570 } 571 572 static void 573 bpe_set_group(struct bpe_softc *sc, uint32_t isid) 574 { 575 uint8_t *group = sc->sc_group.ether_addr_octet; 576 577 group[0] = 0x01; 578 group[1] = 0x1e; 579 group[2] = 0x83; 580 group[3] = isid >> 16; 581 group[4] = isid >> 8; 582 group[5] = isid >> 0; 583 } 584 585 static int 586 bpe_set_vnetid(struct bpe_softc *sc, const struct ifreq *ifr) 587 { 588 struct ifnet *ifp = &sc->sc_ac.ac_if; 589 uint32_t isid; 590 591 if (ifr->ifr_vnetid < PBB_ITAG_ISID_MIN || 592 ifr->ifr_vnetid > PBB_ITAG_ISID_MAX) 593 return (EINVAL); 594 595 isid = ifr->ifr_vnetid; 596 if (isid == sc->sc_key.k_isid) 597 return (0); 598 599 if (ISSET(ifp->if_flags, IFF_RUNNING)) 600 return (EBUSY); 601 602 /* commit */ 603 sc->sc_key.k_isid = isid; 604 bpe_set_group(sc, isid); 605 etherbridge_flush(&sc->sc_eb, IFBF_FLUSHALL); 606 607 return (0); 608 } 609 610 static int 611 bpe_set_parent(struct bpe_softc *sc, const struct if_parent *p) 612 { 613 struct ifnet *ifp = &sc->sc_ac.ac_if; 614 struct ifnet *ifp0; 615 int error = 0; 616 617 ifp0 = if_unit(p->ifp_parent); 618 if (ifp0 == NULL) 619 return (ENXIO); 620 621 if (ifp0->if_type != IFT_ETHER) { 622 error = ENXIO; 623 goto put; 624 } 625 626 if (ifp0->if_index == sc->sc_key.k_if) 627 goto put; 628 629 if (ISSET(ifp->if_flags, IFF_RUNNING)) { 630 error = EBUSY; 631 goto put; 632 } 633 634 ifsetlro(ifp0, 0); 635 636 /* commit */ 637 sc->sc_key.k_if = ifp0->if_index; 638 etherbridge_flush(&sc->sc_eb, IFBF_FLUSHALL); 639 640 put: 641 if_put(ifp0); 642 return (error); 643 } 644 645 static int 646 bpe_get_parent(struct bpe_softc *sc, struct if_parent *p) 647 { 648 struct ifnet *ifp0; 649 int error = 0; 650 651 ifp0 = if_get(sc->sc_key.k_if); 652 if (ifp0 == NULL) 653 error = EADDRNOTAVAIL; 654 else 655 memcpy(p->ifp_parent, ifp0->if_xname, sizeof(p->ifp_parent)); 656 if_put(ifp0); 657 658 return (error); 659 } 660 661 static int 662 bpe_del_parent(struct bpe_softc *sc) 663 { 664 struct ifnet *ifp = &sc->sc_ac.ac_if; 665 666 if (ISSET(ifp->if_flags, IFF_RUNNING)) 667 return (EBUSY); 668 669 /* commit */ 670 sc->sc_key.k_if = 0; 671 etherbridge_flush(&sc->sc_eb, IFBF_FLUSHALL); 672 673 return (0); 674 } 675 676 static int 677 bpe_add_addr(struct bpe_softc *sc, const struct ifbareq *ifba) 678 { 679 const struct sockaddr_dl *sdl; 680 const struct ether_addr *endpoint; 681 unsigned int type; 682 683 /* ignore ifba_ifsname */ 684 685 if (ISSET(ifba->ifba_flags, ~IFBAF_TYPEMASK)) 686 return (EINVAL); 687 switch (ifba->ifba_flags & IFBAF_TYPEMASK) { 688 case IFBAF_DYNAMIC: 689 type = EBE_DYNAMIC; 690 break; 691 case IFBAF_STATIC: 692 type = EBE_STATIC; 693 break; 694 default: 695 return (EINVAL); 696 } 697 698 if (ifba->ifba_dstsa.ss_family != AF_LINK) 699 return (EAFNOSUPPORT); 700 sdl = (struct sockaddr_dl *)&ifba->ifba_dstsa; 701 if (sdl->sdl_type != IFT_ETHER) 702 return (EAFNOSUPPORT); 703 if (sdl->sdl_alen != ETHER_ADDR_LEN) 704 return (EINVAL); 705 endpoint = (struct ether_addr *)LLADDR(sdl); 706 /* check endpoint for multicast or broadcast? */ 707 708 return (etherbridge_add_addr(&sc->sc_eb, (void *)endpoint, 709 &ifba->ifba_dst, type)); 710 } 711 712 static int 713 bpe_del_addr(struct bpe_softc *sc, const struct ifbareq *ifba) 714 { 715 return (etherbridge_del_addr(&sc->sc_eb, &ifba->ifba_dst)); 716 } 717 718 static inline struct bpe_softc * 719 bpe_find(struct ifnet *ifp0, uint32_t isid) 720 { 721 struct bpe_key k = { .k_if = ifp0->if_index, .k_isid = isid }; 722 struct bpe_softc *sc; 723 724 rw_enter_read(&bpe_lock); 725 sc = (struct bpe_softc *)RBT_FIND(bpe_tree, &bpe_interfaces, &k); 726 rw_exit_read(&bpe_lock); 727 728 return (sc); 729 } 730 731 void 732 bpe_input(struct ifnet *ifp0, struct mbuf *m) 733 { 734 struct bpe_softc *sc; 735 struct ifnet *ifp; 736 struct ether_header *beh, *ceh; 737 uint32_t *itagp, itag; 738 unsigned int hlen = sizeof(*beh) + sizeof(*itagp) + sizeof(*ceh); 739 struct mbuf *n; 740 int off; 741 int prio; 742 743 if (m->m_len < hlen) { 744 m = m_pullup(m, hlen); 745 if (m == NULL) { 746 /* pbb short ++ */ 747 return; 748 } 749 } 750 751 beh = mtod(m, struct ether_header *); 752 itagp = (uint32_t *)(beh + 1); 753 itag = bemtoh32(itagp); 754 755 if (itag & PBB_ITAG_RES2) { 756 /* dropped by res2 ++ */ 757 goto drop; 758 } 759 760 sc = bpe_find(ifp0, itag & PBB_ITAG_ISID); 761 if (sc == NULL) { 762 /* no interface found */ 763 goto drop; 764 } 765 766 ceh = (struct ether_header *)(itagp + 1); 767 768 etherbridge_map_ea(&sc->sc_eb, ceh->ether_shost, 769 (struct ether_addr *)beh->ether_shost); 770 771 m_adj(m, sizeof(*beh) + sizeof(*itagp)); 772 773 n = m_getptr(m, sizeof(*ceh), &off); 774 if (n == NULL) { 775 /* no data ++ */ 776 goto drop; 777 } 778 779 if (!ALIGNED_POINTER(mtod(n, caddr_t) + off, uint32_t)) { 780 /* unaligned ++ */ 781 n = m_dup_pkt(m, ETHER_ALIGN, M_NOWAIT); 782 m_freem(m); 783 if (n == NULL) 784 return; 785 786 m = n; 787 } 788 789 ifp = &sc->sc_ac.ac_if; 790 791 prio = sc->sc_rxhprio; 792 switch (prio) { 793 case IF_HDRPRIO_PACKET: 794 break; 795 case IF_HDRPRIO_OUTER: 796 m->m_pkthdr.pf.prio = (itag & PBB_ITAG_PCP_MASK) >> 797 PBB_ITAG_PCP_SHIFT; 798 break; 799 default: 800 m->m_pkthdr.pf.prio = prio; 801 break; 802 } 803 804 m->m_flags &= ~(M_BCAST|M_MCAST); 805 m->m_pkthdr.ph_ifidx = ifp->if_index; 806 m->m_pkthdr.ph_rtableid = ifp->if_rdomain; 807 808 #if NPF > 0 809 pf_pkt_addr_changed(m); 810 #endif 811 812 if_vinput(ifp, m); 813 return; 814 815 drop: 816 m_freem(m); 817 } 818 819 void 820 bpe_detach_hook(void *arg) 821 { 822 struct bpe_softc *sc = arg; 823 struct ifnet *ifp = &sc->sc_ac.ac_if; 824 825 if (ISSET(ifp->if_flags, IFF_RUNNING)) { 826 bpe_down(sc); 827 CLR(ifp->if_flags, IFF_UP); 828 } 829 830 sc->sc_key.k_if = 0; 831 } 832 833 static void 834 bpe_link_hook(void *arg) 835 { 836 struct bpe_softc *sc = arg; 837 struct ifnet *ifp0; 838 u_char link = LINK_STATE_DOWN; 839 uint64_t baud = 0; 840 841 ifp0 = if_get(sc->sc_key.k_if); 842 if (ifp0 != NULL) { 843 link = ifp0->if_link_state; 844 baud = ifp0->if_baudrate; 845 } 846 if_put(ifp0); 847 848 bpe_link_state(sc, link, baud); 849 } 850 851 void 852 bpe_link_state(struct bpe_softc *sc, u_char link, uint64_t baud) 853 { 854 struct ifnet *ifp = &sc->sc_ac.ac_if; 855 856 if (ifp->if_link_state == link) 857 return; 858 859 ifp->if_link_state = link; 860 ifp->if_baudrate = baud; 861 862 if_link_state_change(ifp); 863 } 864 865 static inline int 866 bpe_cmp(const struct bpe_key *a, const struct bpe_key *b) 867 { 868 if (a->k_if > b->k_if) 869 return (1); 870 if (a->k_if < b->k_if) 871 return (-1); 872 if (a->k_isid > b->k_isid) 873 return (1); 874 if (a->k_isid < b->k_isid) 875 return (-1); 876 877 return (0); 878 } 879 880 static int 881 bpe_eb_port_eq(void *arg, void *a, void *b) 882 { 883 struct ether_addr *ea = a, *eb = b; 884 885 return (memcmp(ea, eb, sizeof(*ea)) == 0); 886 } 887 888 static void * 889 bpe_eb_port_take(void *arg, void *port) 890 { 891 struct ether_addr *ea = port; 892 struct ether_addr *endpoint; 893 894 endpoint = pool_get(&bpe_endpoint_pool, PR_NOWAIT); 895 if (endpoint == NULL) 896 return (NULL); 897 898 memcpy(endpoint, ea, sizeof(*endpoint)); 899 900 return (endpoint); 901 } 902 903 static void 904 bpe_eb_port_rele(void *arg, void *port) 905 { 906 struct ether_addr *endpoint = port; 907 908 pool_put(&bpe_endpoint_pool, endpoint); 909 } 910 911 static size_t 912 bpe_eb_port_ifname(void *arg, char *dst, size_t len, void *port) 913 { 914 struct bpe_softc *sc = arg; 915 916 return (strlcpy(dst, sc->sc_ac.ac_if.if_xname, len)); 917 } 918 919 static void 920 bpe_eb_port_sa(void *arg, struct sockaddr_storage *ss, void *port) 921 { 922 struct ether_addr *endpoint = port; 923 struct sockaddr_dl *sdl; 924 925 sdl = (struct sockaddr_dl *)ss; 926 sdl->sdl_len = sizeof(sdl); 927 sdl->sdl_family = AF_LINK; 928 sdl->sdl_index = 0; 929 sdl->sdl_type = IFT_ETHER; 930 sdl->sdl_nlen = 0; 931 sdl->sdl_alen = sizeof(*endpoint); 932 CTASSERT(sizeof(sdl->sdl_data) >= sizeof(*endpoint)); 933 memcpy(sdl->sdl_data, endpoint, sizeof(*endpoint)); 934 } 935