1 /* $OpenBSD: if_bpe.c,v 1.19 2021/11/08 04:54:44 dlg 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 /* commit */ 635 sc->sc_key.k_if = ifp0->if_index; 636 etherbridge_flush(&sc->sc_eb, IFBF_FLUSHALL); 637 638 put: 639 if_put(ifp0); 640 return (error); 641 } 642 643 static int 644 bpe_get_parent(struct bpe_softc *sc, struct if_parent *p) 645 { 646 struct ifnet *ifp0; 647 int error = 0; 648 649 ifp0 = if_get(sc->sc_key.k_if); 650 if (ifp0 == NULL) 651 error = EADDRNOTAVAIL; 652 else 653 memcpy(p->ifp_parent, ifp0->if_xname, sizeof(p->ifp_parent)); 654 if_put(ifp0); 655 656 return (error); 657 } 658 659 static int 660 bpe_del_parent(struct bpe_softc *sc) 661 { 662 struct ifnet *ifp = &sc->sc_ac.ac_if; 663 664 if (ISSET(ifp->if_flags, IFF_RUNNING)) 665 return (EBUSY); 666 667 /* commit */ 668 sc->sc_key.k_if = 0; 669 etherbridge_flush(&sc->sc_eb, IFBF_FLUSHALL); 670 671 return (0); 672 } 673 674 static int 675 bpe_add_addr(struct bpe_softc *sc, const struct ifbareq *ifba) 676 { 677 const struct sockaddr_dl *sdl; 678 const struct ether_addr *endpoint; 679 unsigned int type; 680 681 /* ignore ifba_ifsname */ 682 683 if (ISSET(ifba->ifba_flags, ~IFBAF_TYPEMASK)) 684 return (EINVAL); 685 switch (ifba->ifba_flags & IFBAF_TYPEMASK) { 686 case IFBAF_DYNAMIC: 687 type = EBE_DYNAMIC; 688 break; 689 case IFBAF_STATIC: 690 type = EBE_STATIC; 691 break; 692 default: 693 return (EINVAL); 694 } 695 696 if (ifba->ifba_dstsa.ss_family != AF_LINK) 697 return (EAFNOSUPPORT); 698 sdl = (struct sockaddr_dl *)&ifba->ifba_dstsa; 699 if (sdl->sdl_type != IFT_ETHER) 700 return (EAFNOSUPPORT); 701 if (sdl->sdl_alen != ETHER_ADDR_LEN) 702 return (EINVAL); 703 endpoint = (struct ether_addr *)LLADDR(sdl); 704 /* check endpoint for multicast or broadcast? */ 705 706 return (etherbridge_add_addr(&sc->sc_eb, (void *)endpoint, 707 &ifba->ifba_dst, type)); 708 } 709 710 static int 711 bpe_del_addr(struct bpe_softc *sc, const struct ifbareq *ifba) 712 { 713 return (etherbridge_del_addr(&sc->sc_eb, &ifba->ifba_dst)); 714 } 715 716 static inline struct bpe_softc * 717 bpe_find(struct ifnet *ifp0, uint32_t isid) 718 { 719 struct bpe_key k = { .k_if = ifp0->if_index, .k_isid = isid }; 720 struct bpe_softc *sc; 721 722 rw_enter_read(&bpe_lock); 723 sc = (struct bpe_softc *)RBT_FIND(bpe_tree, &bpe_interfaces, &k); 724 rw_exit_read(&bpe_lock); 725 726 return (sc); 727 } 728 729 void 730 bpe_input(struct ifnet *ifp0, struct mbuf *m) 731 { 732 struct bpe_softc *sc; 733 struct ifnet *ifp; 734 struct ether_header *beh, *ceh; 735 uint32_t *itagp, itag; 736 unsigned int hlen = sizeof(*beh) + sizeof(*itagp) + sizeof(*ceh); 737 struct mbuf *n; 738 int off; 739 int prio; 740 741 if (m->m_len < hlen) { 742 m = m_pullup(m, hlen); 743 if (m == NULL) { 744 /* pbb short ++ */ 745 return; 746 } 747 } 748 749 beh = mtod(m, struct ether_header *); 750 itagp = (uint32_t *)(beh + 1); 751 itag = bemtoh32(itagp); 752 753 if (itag & PBB_ITAG_RES2) { 754 /* dropped by res2 ++ */ 755 goto drop; 756 } 757 758 sc = bpe_find(ifp0, itag & PBB_ITAG_ISID); 759 if (sc == NULL) { 760 /* no interface found */ 761 goto drop; 762 } 763 764 ceh = (struct ether_header *)(itagp + 1); 765 766 etherbridge_map_ea(&sc->sc_eb, ceh->ether_shost, 767 (struct ether_addr *)beh->ether_shost); 768 769 m_adj(m, sizeof(*beh) + sizeof(*itagp)); 770 771 n = m_getptr(m, sizeof(*ceh), &off); 772 if (n == NULL) { 773 /* no data ++ */ 774 goto drop; 775 } 776 777 if (!ALIGNED_POINTER(mtod(n, caddr_t) + off, uint32_t)) { 778 /* unaligned ++ */ 779 n = m_dup_pkt(m, ETHER_ALIGN, M_NOWAIT); 780 m_freem(m); 781 if (n == NULL) 782 return; 783 784 m = n; 785 } 786 787 ifp = &sc->sc_ac.ac_if; 788 789 prio = sc->sc_rxhprio; 790 switch (prio) { 791 case IF_HDRPRIO_PACKET: 792 break; 793 case IF_HDRPRIO_OUTER: 794 m->m_pkthdr.pf.prio = (itag & PBB_ITAG_PCP_MASK) >> 795 PBB_ITAG_PCP_SHIFT; 796 break; 797 default: 798 m->m_pkthdr.pf.prio = prio; 799 break; 800 } 801 802 m->m_flags &= ~(M_BCAST|M_MCAST); 803 m->m_pkthdr.ph_ifidx = ifp->if_index; 804 m->m_pkthdr.ph_rtableid = ifp->if_rdomain; 805 806 #if NPF > 0 807 pf_pkt_addr_changed(m); 808 #endif 809 810 if_vinput(ifp, m); 811 return; 812 813 drop: 814 m_freem(m); 815 } 816 817 void 818 bpe_detach_hook(void *arg) 819 { 820 struct bpe_softc *sc = arg; 821 struct ifnet *ifp = &sc->sc_ac.ac_if; 822 823 if (ISSET(ifp->if_flags, IFF_RUNNING)) { 824 bpe_down(sc); 825 CLR(ifp->if_flags, IFF_UP); 826 } 827 828 sc->sc_key.k_if = 0; 829 } 830 831 static void 832 bpe_link_hook(void *arg) 833 { 834 struct bpe_softc *sc = arg; 835 struct ifnet *ifp0; 836 u_char link = LINK_STATE_DOWN; 837 uint64_t baud = 0; 838 839 ifp0 = if_get(sc->sc_key.k_if); 840 if (ifp0 != NULL) { 841 link = ifp0->if_link_state; 842 baud = ifp0->if_baudrate; 843 } 844 if_put(ifp0); 845 846 bpe_link_state(sc, link, baud); 847 } 848 849 void 850 bpe_link_state(struct bpe_softc *sc, u_char link, uint64_t baud) 851 { 852 struct ifnet *ifp = &sc->sc_ac.ac_if; 853 854 if (ifp->if_link_state == link) 855 return; 856 857 ifp->if_link_state = link; 858 ifp->if_baudrate = baud; 859 860 if_link_state_change(ifp); 861 } 862 863 static inline int 864 bpe_cmp(const struct bpe_key *a, const struct bpe_key *b) 865 { 866 if (a->k_if > b->k_if) 867 return (1); 868 if (a->k_if < b->k_if) 869 return (-1); 870 if (a->k_isid > b->k_isid) 871 return (1); 872 if (a->k_isid < b->k_isid) 873 return (-1); 874 875 return (0); 876 } 877 878 static int 879 bpe_eb_port_eq(void *arg, void *a, void *b) 880 { 881 struct ether_addr *ea = a, *eb = b; 882 883 return (memcmp(ea, eb, sizeof(*ea)) == 0); 884 } 885 886 static void * 887 bpe_eb_port_take(void *arg, void *port) 888 { 889 struct ether_addr *ea = port; 890 struct ether_addr *endpoint; 891 892 endpoint = pool_get(&bpe_endpoint_pool, PR_NOWAIT); 893 if (endpoint == NULL) 894 return (NULL); 895 896 memcpy(endpoint, ea, sizeof(*endpoint)); 897 898 return (endpoint); 899 } 900 901 static void 902 bpe_eb_port_rele(void *arg, void *port) 903 { 904 struct ether_addr *endpoint = port; 905 906 pool_put(&bpe_endpoint_pool, endpoint); 907 } 908 909 static size_t 910 bpe_eb_port_ifname(void *arg, char *dst, size_t len, void *port) 911 { 912 struct bpe_softc *sc = arg; 913 914 return (strlcpy(dst, sc->sc_ac.ac_if.if_xname, len)); 915 } 916 917 static void 918 bpe_eb_port_sa(void *arg, struct sockaddr_storage *ss, void *port) 919 { 920 struct ether_addr *endpoint = port; 921 struct sockaddr_dl *sdl; 922 923 sdl = (struct sockaddr_dl *)ss; 924 sdl->sdl_len = sizeof(sdl); 925 sdl->sdl_family = AF_LINK; 926 sdl->sdl_index = 0; 927 sdl->sdl_type = IFT_ETHER; 928 sdl->sdl_nlen = 0; 929 sdl->sdl_alen = sizeof(*endpoint); 930 CTASSERT(sizeof(sdl->sdl_data) >= sizeof(*endpoint)); 931 memcpy(sdl->sdl_data, endpoint, sizeof(*endpoint)); 932 } 933