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