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