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
bpeattach(int count)136 bpeattach(int count)
137 {
138 if_clone_attach(&bpe_cloner);
139 }
140
141 static int
bpe_clone_create(struct if_clone * ifc,int unit)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
bpe_clone_destroy(struct ifnet * ifp)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
bpe_start(struct ifnet * ifp)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
bpe_ioctl(struct ifnet * ifp,u_long cmd,caddr_t data)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
bpe_media_get(struct bpe_softc * sc,struct ifreq * ifr)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
bpe_up(struct bpe_softc * sc)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
bpe_down(struct bpe_softc * sc)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
bpe_multi(struct bpe_softc * sc,struct ifnet * ifp0,u_long cmd)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
bpe_set_group(struct bpe_softc * sc,uint32_t isid)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
bpe_set_vnetid(struct bpe_softc * sc,const struct ifreq * ifr)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
bpe_set_parent(struct bpe_softc * sc,const struct if_parent * p)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
bpe_get_parent(struct bpe_softc * sc,struct if_parent * p)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
bpe_del_parent(struct bpe_softc * sc)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
bpe_add_addr(struct bpe_softc * sc,const struct ifbareq * ifba)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
bpe_del_addr(struct bpe_softc * sc,const struct ifbareq * ifba)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 *
bpe_find(struct ifnet * ifp0,uint32_t isid)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
bpe_input(struct ifnet * ifp0,struct mbuf * m)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
bpe_detach_hook(void * arg)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
bpe_link_hook(void * arg)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
bpe_link_state(struct bpe_softc * sc,u_char link,uint64_t baud)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
bpe_cmp(const struct bpe_key * a,const struct bpe_key * b)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
bpe_eb_port_eq(void * arg,void * a,void * b)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 *
bpe_eb_port_take(void * arg,void * port)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
bpe_eb_port_rele(void * arg,void * port)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
bpe_eb_port_ifname(void * arg,char * dst,size_t len,void * port)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
bpe_eb_port_sa(void * arg,struct sockaddr_storage * ss,void * port)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