1 /*-
2 * Copyright (C) 1997-2003
3 * Sony Computer Science Laboratories Inc. All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY SONY CSL AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED. IN NO EVENT SHALL SONY CSL OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 *
26 * $KAME: altq_subr.c,v 1.21 2003/11/06 06:32:53 kjc Exp $
27 * $FreeBSD: stable/12/sys/net/altq/altq_subr.c 370250 2021-07-31 13:05:25Z kp $
28 */
29
30 #include "opt_altq.h"
31 #include "opt_inet.h"
32 #include "opt_inet6.h"
33
34 #include <sys/param.h>
35 #include <sys/malloc.h>
36 #include <sys/mbuf.h>
37 #include <sys/systm.h>
38 #include <sys/proc.h>
39 #include <sys/socket.h>
40 #include <sys/socketvar.h>
41 #include <sys/kernel.h>
42 #include <sys/errno.h>
43 #include <sys/syslog.h>
44 #include <sys/sysctl.h>
45 #include <sys/queue.h>
46
47 #include <net/if.h>
48 #include <net/if_var.h>
49 #include <net/if_dl.h>
50 #include <net/if_types.h>
51 #include <net/vnet.h>
52
53 #include <netinet/in.h>
54 #include <netinet/in_systm.h>
55 #include <netinet/ip.h>
56 #ifdef INET6
57 #include <netinet/ip6.h>
58 #endif
59 #include <netinet/tcp.h>
60 #include <netinet/udp.h>
61
62 #include <netpfil/pf/pf.h>
63 #include <netpfil/pf/pf_altq.h>
64 #include <net/altq/altq.h>
65 #ifdef ALTQ3_COMPAT
66 #include <net/altq/altq_conf.h>
67 #endif
68
69 /* machine dependent clock related includes */
70 #include <sys/bus.h>
71 #include <sys/cpu.h>
72 #include <sys/eventhandler.h>
73 #include <machine/clock.h>
74 #if defined(__amd64__) || defined(__i386__)
75 #include <machine/cpufunc.h> /* for pentium tsc */
76 #include <machine/specialreg.h> /* for CPUID_TSC */
77 #include <machine/md_var.h> /* for cpu_feature */
78 #endif /* __amd64 || __i386__ */
79
80 /*
81 * internal function prototypes
82 */
83 static void tbr_timeout(void *);
84 int (*altq_input)(struct mbuf *, int) = NULL;
85 static struct mbuf *tbr_dequeue(struct ifaltq *, int);
86 static int tbr_timer = 0; /* token bucket regulator timer */
87 #if !defined(__FreeBSD__) || (__FreeBSD_version < 600000)
88 static struct callout tbr_callout = CALLOUT_INITIALIZER;
89 #else
90 static struct callout tbr_callout;
91 #endif
92
93 #ifdef ALTQ3_CLFIER_COMPAT
94 static int extract_ports4(struct mbuf *, struct ip *, struct flowinfo_in *);
95 #ifdef INET6
96 static int extract_ports6(struct mbuf *, struct ip6_hdr *,
97 struct flowinfo_in6 *);
98 #endif
99 static int apply_filter4(u_int32_t, struct flow_filter *,
100 struct flowinfo_in *);
101 static int apply_ppfilter4(u_int32_t, struct flow_filter *,
102 struct flowinfo_in *);
103 #ifdef INET6
104 static int apply_filter6(u_int32_t, struct flow_filter6 *,
105 struct flowinfo_in6 *);
106 #endif
107 static int apply_tosfilter4(u_int32_t, struct flow_filter *,
108 struct flowinfo_in *);
109 static u_long get_filt_handle(struct acc_classifier *, int);
110 static struct acc_filter *filth_to_filtp(struct acc_classifier *, u_long);
111 static u_int32_t filt2fibmask(struct flow_filter *);
112
113 static void ip4f_cache(struct ip *, struct flowinfo_in *);
114 static int ip4f_lookup(struct ip *, struct flowinfo_in *);
115 static int ip4f_init(void);
116 static struct ip4_frag *ip4f_alloc(void);
117 static void ip4f_free(struct ip4_frag *);
118 #endif /* ALTQ3_CLFIER_COMPAT */
119
120 #ifdef ALTQ
121 SYSCTL_NODE(_kern_features, OID_AUTO, altq, CTLFLAG_RD | CTLFLAG_CAPRD, 0,
122 "ALTQ packet queuing");
123
124 #define ALTQ_FEATURE(name, desc) \
125 SYSCTL_INT_WITH_LABEL(_kern_features_altq, OID_AUTO, name, \
126 CTLFLAG_RD | CTLFLAG_CAPRD, SYSCTL_NULL_INT_PTR, 1, \
127 desc, "feature")
128
129 #ifdef ALTQ_CBQ
130 ALTQ_FEATURE(cbq, "ALTQ Class Based Queuing discipline");
131 #endif
132 #ifdef ALTQ_CODEL
133 ALTQ_FEATURE(codel, "ALTQ Controlled Delay discipline");
134 #endif
135 #ifdef ALTQ_RED
136 ALTQ_FEATURE(red, "ALTQ Random Early Detection discipline");
137 #endif
138 #ifdef ALTQ_RIO
139 ALTQ_FEATURE(rio, "ALTQ Random Early Drop discipline");
140 #endif
141 #ifdef ALTQ_HFSC
142 ALTQ_FEATURE(hfsc, "ALTQ Hierarchical Packet Scheduler discipline");
143 #endif
144 #ifdef ALTQ_PRIQ
145 ALTQ_FEATURE(priq, "ATLQ Priority Queuing discipline");
146 #endif
147 #ifdef ALTQ_FAIRQ
148 ALTQ_FEATURE(fairq, "ALTQ Fair Queuing discipline");
149 #endif
150 #endif
151
152 /*
153 * alternate queueing support routines
154 */
155
156 /* look up the queue state by the interface name and the queueing type. */
157 void *
altq_lookup(name,type)158 altq_lookup(name, type)
159 char *name;
160 int type;
161 {
162 struct ifnet *ifp;
163
164 if ((ifp = ifunit(name)) != NULL) {
165 /* read if_snd unlocked */
166 if (type != ALTQT_NONE && ifp->if_snd.altq_type == type)
167 return (ifp->if_snd.altq_disc);
168 }
169
170 return NULL;
171 }
172
173 int
altq_attach(ifq,type,discipline,enqueue,dequeue,request,clfier,classify)174 altq_attach(ifq, type, discipline, enqueue, dequeue, request, clfier, classify)
175 struct ifaltq *ifq;
176 int type;
177 void *discipline;
178 int (*enqueue)(struct ifaltq *, struct mbuf *, struct altq_pktattr *);
179 struct mbuf *(*dequeue)(struct ifaltq *, int);
180 int (*request)(struct ifaltq *, int, void *);
181 void *clfier;
182 void *(*classify)(void *, struct mbuf *, int);
183 {
184 IFQ_LOCK(ifq);
185 if (!ALTQ_IS_READY(ifq)) {
186 IFQ_UNLOCK(ifq);
187 return ENXIO;
188 }
189
190 #ifdef ALTQ3_COMPAT
191 /*
192 * pfaltq can override the existing discipline, but altq3 cannot.
193 * check these if clfier is not NULL (which implies altq3).
194 */
195 if (clfier != NULL) {
196 if (ALTQ_IS_ENABLED(ifq)) {
197 IFQ_UNLOCK(ifq);
198 return EBUSY;
199 }
200 if (ALTQ_IS_ATTACHED(ifq)) {
201 IFQ_UNLOCK(ifq);
202 return EEXIST;
203 }
204 }
205 #endif
206 ifq->altq_type = type;
207 ifq->altq_disc = discipline;
208 ifq->altq_enqueue = enqueue;
209 ifq->altq_dequeue = dequeue;
210 ifq->altq_request = request;
211 ifq->altq_clfier = clfier;
212 ifq->altq_classify = classify;
213 ifq->altq_flags &= (ALTQF_CANTCHANGE|ALTQF_ENABLED);
214 #ifdef ALTQ3_COMPAT
215 #ifdef ALTQ_KLD
216 altq_module_incref(type);
217 #endif
218 #endif
219 IFQ_UNLOCK(ifq);
220 return 0;
221 }
222
223 int
altq_detach(ifq)224 altq_detach(ifq)
225 struct ifaltq *ifq;
226 {
227 IFQ_LOCK(ifq);
228
229 if (!ALTQ_IS_READY(ifq)) {
230 IFQ_UNLOCK(ifq);
231 return ENXIO;
232 }
233 if (ALTQ_IS_ENABLED(ifq)) {
234 IFQ_UNLOCK(ifq);
235 return EBUSY;
236 }
237 if (!ALTQ_IS_ATTACHED(ifq)) {
238 IFQ_UNLOCK(ifq);
239 return (0);
240 }
241 #ifdef ALTQ3_COMPAT
242 #ifdef ALTQ_KLD
243 altq_module_declref(ifq->altq_type);
244 #endif
245 #endif
246
247 ifq->altq_type = ALTQT_NONE;
248 ifq->altq_disc = NULL;
249 ifq->altq_enqueue = NULL;
250 ifq->altq_dequeue = NULL;
251 ifq->altq_request = NULL;
252 ifq->altq_clfier = NULL;
253 ifq->altq_classify = NULL;
254 ifq->altq_flags &= ALTQF_CANTCHANGE;
255
256 IFQ_UNLOCK(ifq);
257 return 0;
258 }
259
260 int
altq_enable(ifq)261 altq_enable(ifq)
262 struct ifaltq *ifq;
263 {
264 int s;
265
266 IFQ_LOCK(ifq);
267
268 if (!ALTQ_IS_READY(ifq)) {
269 IFQ_UNLOCK(ifq);
270 return ENXIO;
271 }
272 if (ALTQ_IS_ENABLED(ifq)) {
273 IFQ_UNLOCK(ifq);
274 return 0;
275 }
276
277 s = splnet();
278 IFQ_PURGE_NOLOCK(ifq);
279 ASSERT(ifq->ifq_len == 0);
280 ifq->ifq_drv_maxlen = 0; /* disable bulk dequeue */
281 ifq->altq_flags |= ALTQF_ENABLED;
282 if (ifq->altq_clfier != NULL)
283 ifq->altq_flags |= ALTQF_CLASSIFY;
284 splx(s);
285
286 IFQ_UNLOCK(ifq);
287 return 0;
288 }
289
290 int
altq_disable(ifq)291 altq_disable(ifq)
292 struct ifaltq *ifq;
293 {
294 int s;
295
296 IFQ_LOCK(ifq);
297 if (!ALTQ_IS_ENABLED(ifq)) {
298 IFQ_UNLOCK(ifq);
299 return 0;
300 }
301
302 s = splnet();
303 IFQ_PURGE_NOLOCK(ifq);
304 ASSERT(ifq->ifq_len == 0);
305 ifq->altq_flags &= ~(ALTQF_ENABLED|ALTQF_CLASSIFY);
306 splx(s);
307
308 IFQ_UNLOCK(ifq);
309 return 0;
310 }
311
312 #ifdef ALTQ_DEBUG
313 void
altq_assert(file,line,failedexpr)314 altq_assert(file, line, failedexpr)
315 const char *file, *failedexpr;
316 int line;
317 {
318 (void)printf("altq assertion \"%s\" failed: file \"%s\", line %d\n",
319 failedexpr, file, line);
320 panic("altq assertion");
321 /* NOTREACHED */
322 }
323 #endif
324
325 /*
326 * internal representation of token bucket parameters
327 * rate: (byte_per_unittime << TBR_SHIFT) / machclk_freq
328 * (((bits_per_sec) / 8) << TBR_SHIFT) / machclk_freq
329 * depth: byte << TBR_SHIFT
330 *
331 */
332 #define TBR_SHIFT 29
333 #define TBR_SCALE(x) ((int64_t)(x) << TBR_SHIFT)
334 #define TBR_UNSCALE(x) ((x) >> TBR_SHIFT)
335
336 static struct mbuf *
tbr_dequeue(ifq,op)337 tbr_dequeue(ifq, op)
338 struct ifaltq *ifq;
339 int op;
340 {
341 struct tb_regulator *tbr;
342 struct mbuf *m;
343 int64_t interval;
344 u_int64_t now;
345
346 IFQ_LOCK_ASSERT(ifq);
347 tbr = ifq->altq_tbr;
348 if (op == ALTDQ_REMOVE && tbr->tbr_lastop == ALTDQ_POLL) {
349 /* if this is a remove after poll, bypass tbr check */
350 } else {
351 /* update token only when it is negative */
352 if (tbr->tbr_token <= 0) {
353 now = read_machclk();
354 interval = now - tbr->tbr_last;
355 if (interval >= tbr->tbr_filluptime)
356 tbr->tbr_token = tbr->tbr_depth;
357 else {
358 tbr->tbr_token += interval * tbr->tbr_rate;
359 if (tbr->tbr_token > tbr->tbr_depth)
360 tbr->tbr_token = tbr->tbr_depth;
361 }
362 tbr->tbr_last = now;
363 }
364 /* if token is still negative, don't allow dequeue */
365 if (tbr->tbr_token <= 0)
366 return (NULL);
367 }
368
369 if (ALTQ_IS_ENABLED(ifq))
370 m = (*ifq->altq_dequeue)(ifq, op);
371 else {
372 if (op == ALTDQ_POLL)
373 _IF_POLL(ifq, m);
374 else
375 _IF_DEQUEUE(ifq, m);
376 }
377
378 if (m != NULL && op == ALTDQ_REMOVE)
379 tbr->tbr_token -= TBR_SCALE(m_pktlen(m));
380 tbr->tbr_lastop = op;
381 return (m);
382 }
383
384 /*
385 * set a token bucket regulator.
386 * if the specified rate is zero, the token bucket regulator is deleted.
387 */
388 int
tbr_set(ifq,profile)389 tbr_set(ifq, profile)
390 struct ifaltq *ifq;
391 struct tb_profile *profile;
392 {
393 struct tb_regulator *tbr, *otbr;
394
395 if (tbr_dequeue_ptr == NULL)
396 tbr_dequeue_ptr = tbr_dequeue;
397
398 if (machclk_freq == 0)
399 init_machclk();
400 if (machclk_freq == 0) {
401 printf("tbr_set: no cpu clock available!\n");
402 return (ENXIO);
403 }
404
405 IFQ_LOCK(ifq);
406 if (profile->rate == 0) {
407 /* delete this tbr */
408 if ((tbr = ifq->altq_tbr) == NULL) {
409 IFQ_UNLOCK(ifq);
410 return (ENOENT);
411 }
412 ifq->altq_tbr = NULL;
413 free(tbr, M_DEVBUF);
414 IFQ_UNLOCK(ifq);
415 return (0);
416 }
417
418 tbr = malloc(sizeof(struct tb_regulator), M_DEVBUF, M_NOWAIT | M_ZERO);
419 if (tbr == NULL) {
420 IFQ_UNLOCK(ifq);
421 return (ENOMEM);
422 }
423
424 tbr->tbr_rate = TBR_SCALE(profile->rate / 8) / machclk_freq;
425 tbr->tbr_depth = TBR_SCALE(profile->depth);
426 if (tbr->tbr_rate > 0)
427 tbr->tbr_filluptime = tbr->tbr_depth / tbr->tbr_rate;
428 else
429 tbr->tbr_filluptime = LLONG_MAX;
430 /*
431 * The longest time between tbr_dequeue() calls will be about 1
432 * system tick, as the callout that drives it is scheduled once per
433 * tick. The refill-time detection logic in tbr_dequeue() can only
434 * properly detect the passage of up to LLONG_MAX machclk ticks.
435 * Therefore, in order for this logic to function properly in the
436 * extreme case, the maximum value of tbr_filluptime should be
437 * LLONG_MAX less one system tick's worth of machclk ticks less
438 * some additional slop factor (here one more system tick's worth
439 * of machclk ticks).
440 */
441 if (tbr->tbr_filluptime > (LLONG_MAX - 2 * machclk_per_tick))
442 tbr->tbr_filluptime = LLONG_MAX - 2 * machclk_per_tick;
443 tbr->tbr_token = tbr->tbr_depth;
444 tbr->tbr_last = read_machclk();
445 tbr->tbr_lastop = ALTDQ_REMOVE;
446
447 otbr = ifq->altq_tbr;
448 ifq->altq_tbr = tbr; /* set the new tbr */
449
450 if (otbr != NULL)
451 free(otbr, M_DEVBUF);
452 else {
453 if (tbr_timer == 0) {
454 CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
455 tbr_timer = 1;
456 }
457 }
458 IFQ_UNLOCK(ifq);
459 return (0);
460 }
461
462 /*
463 * tbr_timeout goes through the interface list, and kicks the drivers
464 * if necessary.
465 *
466 * MPSAFE
467 */
468 static void
tbr_timeout(arg)469 tbr_timeout(arg)
470 void *arg;
471 {
472 VNET_ITERATOR_DECL(vnet_iter);
473 struct ifnet *ifp;
474 int active, s;
475
476 active = 0;
477 s = splnet();
478 IFNET_RLOCK_NOSLEEP();
479 VNET_LIST_RLOCK_NOSLEEP();
480 VNET_FOREACH(vnet_iter) {
481 CURVNET_SET(vnet_iter);
482 for (ifp = CK_STAILQ_FIRST(&V_ifnet); ifp;
483 ifp = CK_STAILQ_NEXT(ifp, if_link)) {
484 /* read from if_snd unlocked */
485 if (!TBR_IS_ENABLED(&ifp->if_snd))
486 continue;
487 active++;
488 if (!IFQ_IS_EMPTY(&ifp->if_snd) &&
489 ifp->if_start != NULL)
490 (*ifp->if_start)(ifp);
491 }
492 CURVNET_RESTORE();
493 }
494 VNET_LIST_RUNLOCK_NOSLEEP();
495 IFNET_RUNLOCK_NOSLEEP();
496 splx(s);
497 if (active > 0)
498 CALLOUT_RESET(&tbr_callout, 1, tbr_timeout, (void *)0);
499 else
500 tbr_timer = 0; /* don't need tbr_timer anymore */
501 }
502
503 /*
504 * attach a discipline to the interface. if one already exists, it is
505 * overridden.
506 * Locking is done in the discipline specific attach functions. Basically
507 * they call back to altq_attach which takes care of the attach and locking.
508 */
509 int
altq_pfattach(struct pf_altq * a)510 altq_pfattach(struct pf_altq *a)
511 {
512 int error = 0;
513
514 switch (a->scheduler) {
515 case ALTQT_NONE:
516 break;
517 #ifdef ALTQ_CBQ
518 case ALTQT_CBQ:
519 error = cbq_pfattach(a);
520 break;
521 #endif
522 #ifdef ALTQ_PRIQ
523 case ALTQT_PRIQ:
524 error = priq_pfattach(a);
525 break;
526 #endif
527 #ifdef ALTQ_HFSC
528 case ALTQT_HFSC:
529 error = hfsc_pfattach(a);
530 break;
531 #endif
532 #ifdef ALTQ_FAIRQ
533 case ALTQT_FAIRQ:
534 error = fairq_pfattach(a);
535 break;
536 #endif
537 #ifdef ALTQ_CODEL
538 case ALTQT_CODEL:
539 error = codel_pfattach(a);
540 break;
541 #endif
542 default:
543 error = ENXIO;
544 }
545
546 return (error);
547 }
548
549 /*
550 * detach a discipline from the interface.
551 * it is possible that the discipline was already overridden by another
552 * discipline.
553 */
554 int
altq_pfdetach(struct pf_altq * a)555 altq_pfdetach(struct pf_altq *a)
556 {
557 struct ifnet *ifp;
558 int s, error = 0;
559
560 if ((ifp = ifunit(a->ifname)) == NULL)
561 return (EINVAL);
562
563 /* if this discipline is no longer referenced, just return */
564 /* read unlocked from if_snd */
565 if (a->altq_disc == NULL || a->altq_disc != ifp->if_snd.altq_disc)
566 return (0);
567
568 s = splnet();
569 /* read unlocked from if_snd, _disable and _detach take care */
570 if (ALTQ_IS_ENABLED(&ifp->if_snd))
571 error = altq_disable(&ifp->if_snd);
572 if (error == 0)
573 error = altq_detach(&ifp->if_snd);
574 splx(s);
575
576 return (error);
577 }
578
579 /*
580 * add a discipline or a queue
581 * Locking is done in the discipline specific functions with regards to
582 * malloc with WAITOK, also it is not yet clear which lock to use.
583 */
584 int
altq_add(struct ifnet * ifp,struct pf_altq * a)585 altq_add(struct ifnet *ifp, struct pf_altq *a)
586 {
587 int error = 0;
588
589 if (a->qname[0] != 0)
590 return (altq_add_queue(a));
591
592 if (machclk_freq == 0)
593 init_machclk();
594 if (machclk_freq == 0)
595 panic("altq_add: no cpu clock");
596
597 switch (a->scheduler) {
598 #ifdef ALTQ_CBQ
599 case ALTQT_CBQ:
600 error = cbq_add_altq(ifp, a);
601 break;
602 #endif
603 #ifdef ALTQ_PRIQ
604 case ALTQT_PRIQ:
605 error = priq_add_altq(ifp, a);
606 break;
607 #endif
608 #ifdef ALTQ_HFSC
609 case ALTQT_HFSC:
610 error = hfsc_add_altq(ifp, a);
611 break;
612 #endif
613 #ifdef ALTQ_FAIRQ
614 case ALTQT_FAIRQ:
615 error = fairq_add_altq(ifp, a);
616 break;
617 #endif
618 #ifdef ALTQ_CODEL
619 case ALTQT_CODEL:
620 error = codel_add_altq(ifp, a);
621 break;
622 #endif
623 default:
624 error = ENXIO;
625 }
626
627 return (error);
628 }
629
630 /*
631 * remove a discipline or a queue
632 * It is yet unclear what lock to use to protect this operation, the
633 * discipline specific functions will determine and grab it
634 */
635 int
altq_remove(struct pf_altq * a)636 altq_remove(struct pf_altq *a)
637 {
638 int error = 0;
639
640 if (a->qname[0] != 0)
641 return (altq_remove_queue(a));
642
643 switch (a->scheduler) {
644 #ifdef ALTQ_CBQ
645 case ALTQT_CBQ:
646 error = cbq_remove_altq(a);
647 break;
648 #endif
649 #ifdef ALTQ_PRIQ
650 case ALTQT_PRIQ:
651 error = priq_remove_altq(a);
652 break;
653 #endif
654 #ifdef ALTQ_HFSC
655 case ALTQT_HFSC:
656 error = hfsc_remove_altq(a);
657 break;
658 #endif
659 #ifdef ALTQ_FAIRQ
660 case ALTQT_FAIRQ:
661 error = fairq_remove_altq(a);
662 break;
663 #endif
664 #ifdef ALTQ_CODEL
665 case ALTQT_CODEL:
666 error = codel_remove_altq(a);
667 break;
668 #endif
669 default:
670 error = ENXIO;
671 }
672
673 return (error);
674 }
675
676 /*
677 * add a queue to the discipline
678 * It is yet unclear what lock to use to protect this operation, the
679 * discipline specific functions will determine and grab it
680 */
681 int
altq_add_queue(struct pf_altq * a)682 altq_add_queue(struct pf_altq *a)
683 {
684 int error = 0;
685
686 switch (a->scheduler) {
687 #ifdef ALTQ_CBQ
688 case ALTQT_CBQ:
689 error = cbq_add_queue(a);
690 break;
691 #endif
692 #ifdef ALTQ_PRIQ
693 case ALTQT_PRIQ:
694 error = priq_add_queue(a);
695 break;
696 #endif
697 #ifdef ALTQ_HFSC
698 case ALTQT_HFSC:
699 error = hfsc_add_queue(a);
700 break;
701 #endif
702 #ifdef ALTQ_FAIRQ
703 case ALTQT_FAIRQ:
704 error = fairq_add_queue(a);
705 break;
706 #endif
707 default:
708 error = ENXIO;
709 }
710
711 return (error);
712 }
713
714 /*
715 * remove a queue from the discipline
716 * It is yet unclear what lock to use to protect this operation, the
717 * discipline specific functions will determine and grab it
718 */
719 int
altq_remove_queue(struct pf_altq * a)720 altq_remove_queue(struct pf_altq *a)
721 {
722 int error = 0;
723
724 switch (a->scheduler) {
725 #ifdef ALTQ_CBQ
726 case ALTQT_CBQ:
727 error = cbq_remove_queue(a);
728 break;
729 #endif
730 #ifdef ALTQ_PRIQ
731 case ALTQT_PRIQ:
732 error = priq_remove_queue(a);
733 break;
734 #endif
735 #ifdef ALTQ_HFSC
736 case ALTQT_HFSC:
737 error = hfsc_remove_queue(a);
738 break;
739 #endif
740 #ifdef ALTQ_FAIRQ
741 case ALTQT_FAIRQ:
742 error = fairq_remove_queue(a);
743 break;
744 #endif
745 default:
746 error = ENXIO;
747 }
748
749 return (error);
750 }
751
752 /*
753 * get queue statistics
754 * Locking is done in the discipline specific functions with regards to
755 * copyout operations, also it is not yet clear which lock to use.
756 */
757 int
altq_getqstats(struct pf_altq * a,void * ubuf,int * nbytes,int version)758 altq_getqstats(struct pf_altq *a, void *ubuf, int *nbytes, int version)
759 {
760 int error = 0;
761
762 switch (a->scheduler) {
763 #ifdef ALTQ_CBQ
764 case ALTQT_CBQ:
765 error = cbq_getqstats(a, ubuf, nbytes, version);
766 break;
767 #endif
768 #ifdef ALTQ_PRIQ
769 case ALTQT_PRIQ:
770 error = priq_getqstats(a, ubuf, nbytes, version);
771 break;
772 #endif
773 #ifdef ALTQ_HFSC
774 case ALTQT_HFSC:
775 error = hfsc_getqstats(a, ubuf, nbytes, version);
776 break;
777 #endif
778 #ifdef ALTQ_FAIRQ
779 case ALTQT_FAIRQ:
780 error = fairq_getqstats(a, ubuf, nbytes, version);
781 break;
782 #endif
783 #ifdef ALTQ_CODEL
784 case ALTQT_CODEL:
785 error = codel_getqstats(a, ubuf, nbytes, version);
786 break;
787 #endif
788 default:
789 error = ENXIO;
790 }
791
792 return (error);
793 }
794
795 /*
796 * read and write diffserv field in IPv4 or IPv6 header
797 */
798 u_int8_t
read_dsfield(m,pktattr)799 read_dsfield(m, pktattr)
800 struct mbuf *m;
801 struct altq_pktattr *pktattr;
802 {
803 struct mbuf *m0;
804 u_int8_t ds_field = 0;
805
806 if (pktattr == NULL ||
807 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
808 return ((u_int8_t)0);
809
810 /* verify that pattr_hdr is within the mbuf data */
811 for (m0 = m; m0 != NULL; m0 = m0->m_next)
812 if ((pktattr->pattr_hdr >= m0->m_data) &&
813 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
814 break;
815 if (m0 == NULL) {
816 /* ick, pattr_hdr is stale */
817 pktattr->pattr_af = AF_UNSPEC;
818 #ifdef ALTQ_DEBUG
819 printf("read_dsfield: can't locate header!\n");
820 #endif
821 return ((u_int8_t)0);
822 }
823
824 if (pktattr->pattr_af == AF_INET) {
825 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
826
827 if (ip->ip_v != 4)
828 return ((u_int8_t)0); /* version mismatch! */
829 ds_field = ip->ip_tos;
830 }
831 #ifdef INET6
832 else if (pktattr->pattr_af == AF_INET6) {
833 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
834 u_int32_t flowlabel;
835
836 flowlabel = ntohl(ip6->ip6_flow);
837 if ((flowlabel >> 28) != 6)
838 return ((u_int8_t)0); /* version mismatch! */
839 ds_field = (flowlabel >> 20) & 0xff;
840 }
841 #endif
842 return (ds_field);
843 }
844
845 void
write_dsfield(struct mbuf * m,struct altq_pktattr * pktattr,u_int8_t dsfield)846 write_dsfield(struct mbuf *m, struct altq_pktattr *pktattr, u_int8_t dsfield)
847 {
848 struct mbuf *m0;
849
850 if (pktattr == NULL ||
851 (pktattr->pattr_af != AF_INET && pktattr->pattr_af != AF_INET6))
852 return;
853
854 /* verify that pattr_hdr is within the mbuf data */
855 for (m0 = m; m0 != NULL; m0 = m0->m_next)
856 if ((pktattr->pattr_hdr >= m0->m_data) &&
857 (pktattr->pattr_hdr < m0->m_data + m0->m_len))
858 break;
859 if (m0 == NULL) {
860 /* ick, pattr_hdr is stale */
861 pktattr->pattr_af = AF_UNSPEC;
862 #ifdef ALTQ_DEBUG
863 printf("write_dsfield: can't locate header!\n");
864 #endif
865 return;
866 }
867
868 if (pktattr->pattr_af == AF_INET) {
869 struct ip *ip = (struct ip *)pktattr->pattr_hdr;
870 u_int8_t old;
871 int32_t sum;
872
873 if (ip->ip_v != 4)
874 return; /* version mismatch! */
875 old = ip->ip_tos;
876 dsfield |= old & 3; /* leave CU bits */
877 if (old == dsfield)
878 return;
879 ip->ip_tos = dsfield;
880 /*
881 * update checksum (from RFC1624)
882 * HC' = ~(~HC + ~m + m')
883 */
884 sum = ~ntohs(ip->ip_sum) & 0xffff;
885 sum += 0xff00 + (~old & 0xff) + dsfield;
886 sum = (sum >> 16) + (sum & 0xffff);
887 sum += (sum >> 16); /* add carry */
888
889 ip->ip_sum = htons(~sum & 0xffff);
890 }
891 #ifdef INET6
892 else if (pktattr->pattr_af == AF_INET6) {
893 struct ip6_hdr *ip6 = (struct ip6_hdr *)pktattr->pattr_hdr;
894 u_int32_t flowlabel;
895
896 flowlabel = ntohl(ip6->ip6_flow);
897 if ((flowlabel >> 28) != 6)
898 return; /* version mismatch! */
899 flowlabel = (flowlabel & 0xf03fffff) | (dsfield << 20);
900 ip6->ip6_flow = htonl(flowlabel);
901 }
902 #endif
903 return;
904 }
905
906
907 /*
908 * high resolution clock support taking advantage of a machine dependent
909 * high resolution time counter (e.g., timestamp counter of intel pentium).
910 * we assume
911 * - 64-bit-long monotonically-increasing counter
912 * - frequency range is 100M-4GHz (CPU speed)
913 */
914 /* if pcc is not available or disabled, emulate 256MHz using microtime() */
915 #define MACHCLK_SHIFT 8
916
917 int machclk_usepcc;
918 u_int32_t machclk_freq;
919 u_int32_t machclk_per_tick;
920
921 #if defined(__i386__) && defined(__NetBSD__)
922 extern u_int64_t cpu_tsc_freq;
923 #endif
924
925 #if (__FreeBSD_version >= 700035)
926 /* Update TSC freq with the value indicated by the caller. */
927 static void
tsc_freq_changed(void * arg,const struct cf_level * level,int status)928 tsc_freq_changed(void *arg, const struct cf_level *level, int status)
929 {
930 /* If there was an error during the transition, don't do anything. */
931 if (status != 0)
932 return;
933
934 #if (__FreeBSD_version >= 701102) && (defined(__amd64__) || defined(__i386__))
935 /* If TSC is P-state invariant, don't do anything. */
936 if (tsc_is_invariant)
937 return;
938 #endif
939
940 /* Total setting for this level gives the new frequency in MHz. */
941 init_machclk();
942 }
943 EVENTHANDLER_DEFINE(cpufreq_post_change, tsc_freq_changed, NULL,
944 EVENTHANDLER_PRI_LAST);
945 #endif /* __FreeBSD_version >= 700035 */
946
947 static void
init_machclk_setup(void)948 init_machclk_setup(void)
949 {
950 #if (__FreeBSD_version >= 600000)
951 callout_init(&tbr_callout, 0);
952 #endif
953
954 machclk_usepcc = 1;
955
956 #if (!defined(__amd64__) && !defined(__i386__)) || defined(ALTQ_NOPCC)
957 machclk_usepcc = 0;
958 #endif
959 #if defined(__FreeBSD__) && defined(SMP)
960 machclk_usepcc = 0;
961 #endif
962 #if defined(__NetBSD__) && defined(MULTIPROCESSOR)
963 machclk_usepcc = 0;
964 #endif
965 #if defined(__amd64__) || defined(__i386__)
966 /* check if TSC is available */
967 if ((cpu_feature & CPUID_TSC) == 0 ||
968 atomic_load_acq_64(&tsc_freq) == 0)
969 machclk_usepcc = 0;
970 #endif
971 }
972
973 void
init_machclk(void)974 init_machclk(void)
975 {
976 static int called;
977
978 /* Call one-time initialization function. */
979 if (!called) {
980 init_machclk_setup();
981 called = 1;
982 }
983
984 if (machclk_usepcc == 0) {
985 /* emulate 256MHz using microtime() */
986 machclk_freq = 1000000 << MACHCLK_SHIFT;
987 machclk_per_tick = machclk_freq / hz;
988 #ifdef ALTQ_DEBUG
989 printf("altq: emulate %uHz cpu clock\n", machclk_freq);
990 #endif
991 return;
992 }
993
994 /*
995 * if the clock frequency (of Pentium TSC or Alpha PCC) is
996 * accessible, just use it.
997 */
998 #if defined(__amd64__) || defined(__i386__)
999 machclk_freq = atomic_load_acq_64(&tsc_freq);
1000 #endif
1001
1002 /*
1003 * if we don't know the clock frequency, measure it.
1004 */
1005 if (machclk_freq == 0) {
1006 static int wait;
1007 struct timeval tv_start, tv_end;
1008 u_int64_t start, end, diff;
1009 int timo;
1010
1011 microtime(&tv_start);
1012 start = read_machclk();
1013 timo = hz; /* 1 sec */
1014 (void)tsleep(&wait, PWAIT | PCATCH, "init_machclk", timo);
1015 microtime(&tv_end);
1016 end = read_machclk();
1017 diff = (u_int64_t)(tv_end.tv_sec - tv_start.tv_sec) * 1000000
1018 + tv_end.tv_usec - tv_start.tv_usec;
1019 if (diff != 0)
1020 machclk_freq = (u_int)((end - start) * 1000000 / diff);
1021 }
1022
1023 machclk_per_tick = machclk_freq / hz;
1024
1025 #ifdef ALTQ_DEBUG
1026 printf("altq: CPU clock: %uHz\n", machclk_freq);
1027 #endif
1028 }
1029
1030 #if defined(__OpenBSD__) && defined(__i386__)
1031 static __inline u_int64_t
rdtsc(void)1032 rdtsc(void)
1033 {
1034 u_int64_t rv;
1035 __asm __volatile(".byte 0x0f, 0x31" : "=A" (rv));
1036 return (rv);
1037 }
1038 #endif /* __OpenBSD__ && __i386__ */
1039
1040 u_int64_t
read_machclk(void)1041 read_machclk(void)
1042 {
1043 u_int64_t val;
1044
1045 if (machclk_usepcc) {
1046 #if defined(__amd64__) || defined(__i386__)
1047 val = rdtsc();
1048 #else
1049 panic("read_machclk");
1050 #endif
1051 } else {
1052 struct timeval tv, boottime;
1053
1054 microtime(&tv);
1055 getboottime(&boottime);
1056 val = (((u_int64_t)(tv.tv_sec - boottime.tv_sec) * 1000000
1057 + tv.tv_usec) << MACHCLK_SHIFT);
1058 }
1059 return (val);
1060 }
1061
1062 #ifdef ALTQ3_CLFIER_COMPAT
1063
1064 #ifndef IPPROTO_ESP
1065 #define IPPROTO_ESP 50 /* encapsulating security payload */
1066 #endif
1067 #ifndef IPPROTO_AH
1068 #define IPPROTO_AH 51 /* authentication header */
1069 #endif
1070
1071 /*
1072 * extract flow information from a given packet.
1073 * filt_mask shows flowinfo fields required.
1074 * we assume the ip header is in one mbuf, and addresses and ports are
1075 * in network byte order.
1076 */
1077 int
altq_extractflow(m,af,flow,filt_bmask)1078 altq_extractflow(m, af, flow, filt_bmask)
1079 struct mbuf *m;
1080 int af;
1081 struct flowinfo *flow;
1082 u_int32_t filt_bmask;
1083 {
1084
1085 switch (af) {
1086 case PF_INET: {
1087 struct flowinfo_in *fin;
1088 struct ip *ip;
1089
1090 ip = mtod(m, struct ip *);
1091
1092 if (ip->ip_v != 4)
1093 break;
1094
1095 fin = (struct flowinfo_in *)flow;
1096 fin->fi_len = sizeof(struct flowinfo_in);
1097 fin->fi_family = AF_INET;
1098
1099 fin->fi_proto = ip->ip_p;
1100 fin->fi_tos = ip->ip_tos;
1101
1102 fin->fi_src.s_addr = ip->ip_src.s_addr;
1103 fin->fi_dst.s_addr = ip->ip_dst.s_addr;
1104
1105 if (filt_bmask & FIMB4_PORTS)
1106 /* if port info is required, extract port numbers */
1107 extract_ports4(m, ip, fin);
1108 else {
1109 fin->fi_sport = 0;
1110 fin->fi_dport = 0;
1111 fin->fi_gpi = 0;
1112 }
1113 return (1);
1114 }
1115
1116 #ifdef INET6
1117 case PF_INET6: {
1118 struct flowinfo_in6 *fin6;
1119 struct ip6_hdr *ip6;
1120
1121 ip6 = mtod(m, struct ip6_hdr *);
1122 /* should we check the ip version? */
1123
1124 fin6 = (struct flowinfo_in6 *)flow;
1125 fin6->fi6_len = sizeof(struct flowinfo_in6);
1126 fin6->fi6_family = AF_INET6;
1127
1128 fin6->fi6_proto = ip6->ip6_nxt;
1129 fin6->fi6_tclass = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
1130
1131 fin6->fi6_flowlabel = ip6->ip6_flow & htonl(0x000fffff);
1132 fin6->fi6_src = ip6->ip6_src;
1133 fin6->fi6_dst = ip6->ip6_dst;
1134
1135 if ((filt_bmask & FIMB6_PORTS) ||
1136 ((filt_bmask & FIMB6_PROTO)
1137 && ip6->ip6_nxt > IPPROTO_IPV6))
1138 /*
1139 * if port info is required, or proto is required
1140 * but there are option headers, extract port
1141 * and protocol numbers.
1142 */
1143 extract_ports6(m, ip6, fin6);
1144 else {
1145 fin6->fi6_sport = 0;
1146 fin6->fi6_dport = 0;
1147 fin6->fi6_gpi = 0;
1148 }
1149 return (1);
1150 }
1151 #endif /* INET6 */
1152
1153 default:
1154 break;
1155 }
1156
1157 /* failed */
1158 flow->fi_len = sizeof(struct flowinfo);
1159 flow->fi_family = AF_UNSPEC;
1160 return (0);
1161 }
1162
1163 /*
1164 * helper routine to extract port numbers
1165 */
1166 /* structure for ipsec and ipv6 option header template */
1167 struct _opt6 {
1168 u_int8_t opt6_nxt; /* next header */
1169 u_int8_t opt6_hlen; /* header extension length */
1170 u_int16_t _pad;
1171 u_int32_t ah_spi; /* security parameter index
1172 for authentication header */
1173 };
1174
1175 /*
1176 * extract port numbers from a ipv4 packet.
1177 */
1178 static int
extract_ports4(m,ip,fin)1179 extract_ports4(m, ip, fin)
1180 struct mbuf *m;
1181 struct ip *ip;
1182 struct flowinfo_in *fin;
1183 {
1184 struct mbuf *m0;
1185 u_short ip_off;
1186 u_int8_t proto;
1187 int off;
1188
1189 fin->fi_sport = 0;
1190 fin->fi_dport = 0;
1191 fin->fi_gpi = 0;
1192
1193 ip_off = ntohs(ip->ip_off);
1194 /* if it is a fragment, try cached fragment info */
1195 if (ip_off & IP_OFFMASK) {
1196 ip4f_lookup(ip, fin);
1197 return (1);
1198 }
1199
1200 /* locate the mbuf containing the protocol header */
1201 for (m0 = m; m0 != NULL; m0 = m0->m_next)
1202 if (((caddr_t)ip >= m0->m_data) &&
1203 ((caddr_t)ip < m0->m_data + m0->m_len))
1204 break;
1205 if (m0 == NULL) {
1206 #ifdef ALTQ_DEBUG
1207 printf("extract_ports4: can't locate header! ip=%p\n", ip);
1208 #endif
1209 return (0);
1210 }
1211 off = ((caddr_t)ip - m0->m_data) + (ip->ip_hl << 2);
1212 proto = ip->ip_p;
1213
1214 #ifdef ALTQ_IPSEC
1215 again:
1216 #endif
1217 while (off >= m0->m_len) {
1218 off -= m0->m_len;
1219 m0 = m0->m_next;
1220 if (m0 == NULL)
1221 return (0); /* bogus ip_hl! */
1222 }
1223 if (m0->m_len < off + 4)
1224 return (0);
1225
1226 switch (proto) {
1227 case IPPROTO_TCP:
1228 case IPPROTO_UDP: {
1229 struct udphdr *udp;
1230
1231 udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1232 fin->fi_sport = udp->uh_sport;
1233 fin->fi_dport = udp->uh_dport;
1234 fin->fi_proto = proto;
1235 }
1236 break;
1237
1238 #ifdef ALTQ_IPSEC
1239 case IPPROTO_ESP:
1240 if (fin->fi_gpi == 0){
1241 u_int32_t *gpi;
1242
1243 gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1244 fin->fi_gpi = *gpi;
1245 }
1246 fin->fi_proto = proto;
1247 break;
1248
1249 case IPPROTO_AH: {
1250 /* get next header and header length */
1251 struct _opt6 *opt6;
1252
1253 opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1254 proto = opt6->opt6_nxt;
1255 off += 8 + (opt6->opt6_hlen * 4);
1256 if (fin->fi_gpi == 0 && m0->m_len >= off + 8)
1257 fin->fi_gpi = opt6->ah_spi;
1258 }
1259 /* goto the next header */
1260 goto again;
1261 #endif /* ALTQ_IPSEC */
1262
1263 default:
1264 fin->fi_proto = proto;
1265 return (0);
1266 }
1267
1268 /* if this is a first fragment, cache it. */
1269 if (ip_off & IP_MF)
1270 ip4f_cache(ip, fin);
1271
1272 return (1);
1273 }
1274
1275 #ifdef INET6
1276 static int
extract_ports6(m,ip6,fin6)1277 extract_ports6(m, ip6, fin6)
1278 struct mbuf *m;
1279 struct ip6_hdr *ip6;
1280 struct flowinfo_in6 *fin6;
1281 {
1282 struct mbuf *m0;
1283 int off;
1284 u_int8_t proto;
1285
1286 fin6->fi6_gpi = 0;
1287 fin6->fi6_sport = 0;
1288 fin6->fi6_dport = 0;
1289
1290 /* locate the mbuf containing the protocol header */
1291 for (m0 = m; m0 != NULL; m0 = m0->m_next)
1292 if (((caddr_t)ip6 >= m0->m_data) &&
1293 ((caddr_t)ip6 < m0->m_data + m0->m_len))
1294 break;
1295 if (m0 == NULL) {
1296 #ifdef ALTQ_DEBUG
1297 printf("extract_ports6: can't locate header! ip6=%p\n", ip6);
1298 #endif
1299 return (0);
1300 }
1301 off = ((caddr_t)ip6 - m0->m_data) + sizeof(struct ip6_hdr);
1302
1303 proto = ip6->ip6_nxt;
1304 do {
1305 while (off >= m0->m_len) {
1306 off -= m0->m_len;
1307 m0 = m0->m_next;
1308 if (m0 == NULL)
1309 return (0);
1310 }
1311 if (m0->m_len < off + 4)
1312 return (0);
1313
1314 switch (proto) {
1315 case IPPROTO_TCP:
1316 case IPPROTO_UDP: {
1317 struct udphdr *udp;
1318
1319 udp = (struct udphdr *)(mtod(m0, caddr_t) + off);
1320 fin6->fi6_sport = udp->uh_sport;
1321 fin6->fi6_dport = udp->uh_dport;
1322 fin6->fi6_proto = proto;
1323 }
1324 return (1);
1325
1326 case IPPROTO_ESP:
1327 if (fin6->fi6_gpi == 0) {
1328 u_int32_t *gpi;
1329
1330 gpi = (u_int32_t *)(mtod(m0, caddr_t) + off);
1331 fin6->fi6_gpi = *gpi;
1332 }
1333 fin6->fi6_proto = proto;
1334 return (1);
1335
1336 case IPPROTO_AH: {
1337 /* get next header and header length */
1338 struct _opt6 *opt6;
1339
1340 opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1341 if (fin6->fi6_gpi == 0 && m0->m_len >= off + 8)
1342 fin6->fi6_gpi = opt6->ah_spi;
1343 proto = opt6->opt6_nxt;
1344 off += 8 + (opt6->opt6_hlen * 4);
1345 /* goto the next header */
1346 break;
1347 }
1348
1349 case IPPROTO_HOPOPTS:
1350 case IPPROTO_ROUTING:
1351 case IPPROTO_DSTOPTS: {
1352 /* get next header and header length */
1353 struct _opt6 *opt6;
1354
1355 opt6 = (struct _opt6 *)(mtod(m0, caddr_t) + off);
1356 proto = opt6->opt6_nxt;
1357 off += (opt6->opt6_hlen + 1) * 8;
1358 /* goto the next header */
1359 break;
1360 }
1361
1362 case IPPROTO_FRAGMENT:
1363 /* ipv6 fragmentations are not supported yet */
1364 default:
1365 fin6->fi6_proto = proto;
1366 return (0);
1367 }
1368 } while (1);
1369 /*NOTREACHED*/
1370 }
1371 #endif /* INET6 */
1372
1373 /*
1374 * altq common classifier
1375 */
1376 int
acc_add_filter(classifier,filter,class,phandle)1377 acc_add_filter(classifier, filter, class, phandle)
1378 struct acc_classifier *classifier;
1379 struct flow_filter *filter;
1380 void *class;
1381 u_long *phandle;
1382 {
1383 struct acc_filter *afp, *prev, *tmp;
1384 int i, s;
1385
1386 #ifdef INET6
1387 if (filter->ff_flow.fi_family != AF_INET &&
1388 filter->ff_flow.fi_family != AF_INET6)
1389 return (EINVAL);
1390 #else
1391 if (filter->ff_flow.fi_family != AF_INET)
1392 return (EINVAL);
1393 #endif
1394
1395 afp = malloc(sizeof(struct acc_filter),
1396 M_DEVBUF, M_WAITOK);
1397 if (afp == NULL)
1398 return (ENOMEM);
1399 bzero(afp, sizeof(struct acc_filter));
1400
1401 afp->f_filter = *filter;
1402 afp->f_class = class;
1403
1404 i = ACC_WILDCARD_INDEX;
1405 if (filter->ff_flow.fi_family == AF_INET) {
1406 struct flow_filter *filter4 = &afp->f_filter;
1407
1408 /*
1409 * if address is 0, it's a wildcard. if address mask
1410 * isn't set, use full mask.
1411 */
1412 if (filter4->ff_flow.fi_dst.s_addr == 0)
1413 filter4->ff_mask.mask_dst.s_addr = 0;
1414 else if (filter4->ff_mask.mask_dst.s_addr == 0)
1415 filter4->ff_mask.mask_dst.s_addr = 0xffffffff;
1416 if (filter4->ff_flow.fi_src.s_addr == 0)
1417 filter4->ff_mask.mask_src.s_addr = 0;
1418 else if (filter4->ff_mask.mask_src.s_addr == 0)
1419 filter4->ff_mask.mask_src.s_addr = 0xffffffff;
1420
1421 /* clear extra bits in addresses */
1422 filter4->ff_flow.fi_dst.s_addr &=
1423 filter4->ff_mask.mask_dst.s_addr;
1424 filter4->ff_flow.fi_src.s_addr &=
1425 filter4->ff_mask.mask_src.s_addr;
1426
1427 /*
1428 * if dst address is a wildcard, use hash-entry
1429 * ACC_WILDCARD_INDEX.
1430 */
1431 if (filter4->ff_mask.mask_dst.s_addr != 0xffffffff)
1432 i = ACC_WILDCARD_INDEX;
1433 else
1434 i = ACC_GET_HASH_INDEX(filter4->ff_flow.fi_dst.s_addr);
1435 }
1436 #ifdef INET6
1437 else if (filter->ff_flow.fi_family == AF_INET6) {
1438 struct flow_filter6 *filter6 =
1439 (struct flow_filter6 *)&afp->f_filter;
1440 #ifndef IN6MASK0 /* taken from kame ipv6 */
1441 #define IN6MASK0 {{{ 0, 0, 0, 0 }}}
1442 #define IN6MASK128 {{{ 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff }}}
1443 const struct in6_addr in6mask0 = IN6MASK0;
1444 const struct in6_addr in6mask128 = IN6MASK128;
1445 #endif
1446
1447 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_dst))
1448 filter6->ff_mask6.mask6_dst = in6mask0;
1449 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_dst))
1450 filter6->ff_mask6.mask6_dst = in6mask128;
1451 if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_flow6.fi6_src))
1452 filter6->ff_mask6.mask6_src = in6mask0;
1453 else if (IN6_IS_ADDR_UNSPECIFIED(&filter6->ff_mask6.mask6_src))
1454 filter6->ff_mask6.mask6_src = in6mask128;
1455
1456 /* clear extra bits in addresses */
1457 for (i = 0; i < 16; i++)
1458 filter6->ff_flow6.fi6_dst.s6_addr[i] &=
1459 filter6->ff_mask6.mask6_dst.s6_addr[i];
1460 for (i = 0; i < 16; i++)
1461 filter6->ff_flow6.fi6_src.s6_addr[i] &=
1462 filter6->ff_mask6.mask6_src.s6_addr[i];
1463
1464 if (filter6->ff_flow6.fi6_flowlabel == 0)
1465 i = ACC_WILDCARD_INDEX;
1466 else
1467 i = ACC_GET_HASH_INDEX(filter6->ff_flow6.fi6_flowlabel);
1468 }
1469 #endif /* INET6 */
1470
1471 afp->f_handle = get_filt_handle(classifier, i);
1472
1473 /* update filter bitmask */
1474 afp->f_fbmask = filt2fibmask(filter);
1475 classifier->acc_fbmask |= afp->f_fbmask;
1476
1477 /*
1478 * add this filter to the filter list.
1479 * filters are ordered from the highest rule number.
1480 */
1481 s = splnet();
1482 prev = NULL;
1483 LIST_FOREACH(tmp, &classifier->acc_filters[i], f_chain) {
1484 if (tmp->f_filter.ff_ruleno > afp->f_filter.ff_ruleno)
1485 prev = tmp;
1486 else
1487 break;
1488 }
1489 if (prev == NULL)
1490 LIST_INSERT_HEAD(&classifier->acc_filters[i], afp, f_chain);
1491 else
1492 LIST_INSERT_AFTER(prev, afp, f_chain);
1493 splx(s);
1494
1495 *phandle = afp->f_handle;
1496 return (0);
1497 }
1498
1499 int
acc_delete_filter(classifier,handle)1500 acc_delete_filter(classifier, handle)
1501 struct acc_classifier *classifier;
1502 u_long handle;
1503 {
1504 struct acc_filter *afp;
1505 int s;
1506
1507 if ((afp = filth_to_filtp(classifier, handle)) == NULL)
1508 return (EINVAL);
1509
1510 s = splnet();
1511 LIST_REMOVE(afp, f_chain);
1512 splx(s);
1513
1514 free(afp, M_DEVBUF);
1515
1516 /* todo: update filt_bmask */
1517
1518 return (0);
1519 }
1520
1521 /*
1522 * delete filters referencing to the specified class.
1523 * if the all flag is not 0, delete all the filters.
1524 */
1525 int
acc_discard_filters(classifier,class,all)1526 acc_discard_filters(classifier, class, all)
1527 struct acc_classifier *classifier;
1528 void *class;
1529 int all;
1530 {
1531 struct acc_filter *afp;
1532 int i, s;
1533
1534 s = splnet();
1535 for (i = 0; i < ACC_FILTER_TABLESIZE; i++) {
1536 do {
1537 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1538 if (all || afp->f_class == class) {
1539 LIST_REMOVE(afp, f_chain);
1540 free(afp, M_DEVBUF);
1541 /* start again from the head */
1542 break;
1543 }
1544 } while (afp != NULL);
1545 }
1546 splx(s);
1547
1548 if (all)
1549 classifier->acc_fbmask = 0;
1550
1551 return (0);
1552 }
1553
1554 void *
acc_classify(clfier,m,af)1555 acc_classify(clfier, m, af)
1556 void *clfier;
1557 struct mbuf *m;
1558 int af;
1559 {
1560 struct acc_classifier *classifier;
1561 struct flowinfo flow;
1562 struct acc_filter *afp;
1563 int i;
1564
1565 classifier = (struct acc_classifier *)clfier;
1566 altq_extractflow(m, af, &flow, classifier->acc_fbmask);
1567
1568 if (flow.fi_family == AF_INET) {
1569 struct flowinfo_in *fp = (struct flowinfo_in *)&flow;
1570
1571 if ((classifier->acc_fbmask & FIMB4_ALL) == FIMB4_TOS) {
1572 /* only tos is used */
1573 LIST_FOREACH(afp,
1574 &classifier->acc_filters[ACC_WILDCARD_INDEX],
1575 f_chain)
1576 if (apply_tosfilter4(afp->f_fbmask,
1577 &afp->f_filter, fp))
1578 /* filter matched */
1579 return (afp->f_class);
1580 } else if ((classifier->acc_fbmask &
1581 (~(FIMB4_PROTO|FIMB4_SPORT|FIMB4_DPORT) & FIMB4_ALL))
1582 == 0) {
1583 /* only proto and ports are used */
1584 LIST_FOREACH(afp,
1585 &classifier->acc_filters[ACC_WILDCARD_INDEX],
1586 f_chain)
1587 if (apply_ppfilter4(afp->f_fbmask,
1588 &afp->f_filter, fp))
1589 /* filter matched */
1590 return (afp->f_class);
1591 } else {
1592 /* get the filter hash entry from its dest address */
1593 i = ACC_GET_HASH_INDEX(fp->fi_dst.s_addr);
1594 do {
1595 /*
1596 * go through this loop twice. first for dst
1597 * hash, second for wildcards.
1598 */
1599 LIST_FOREACH(afp, &classifier->acc_filters[i],
1600 f_chain)
1601 if (apply_filter4(afp->f_fbmask,
1602 &afp->f_filter, fp))
1603 /* filter matched */
1604 return (afp->f_class);
1605
1606 /*
1607 * check again for filters with a dst addr
1608 * wildcard.
1609 * (daddr == 0 || dmask != 0xffffffff).
1610 */
1611 if (i != ACC_WILDCARD_INDEX)
1612 i = ACC_WILDCARD_INDEX;
1613 else
1614 break;
1615 } while (1);
1616 }
1617 }
1618 #ifdef INET6
1619 else if (flow.fi_family == AF_INET6) {
1620 struct flowinfo_in6 *fp6 = (struct flowinfo_in6 *)&flow;
1621
1622 /* get the filter hash entry from its flow ID */
1623 if (fp6->fi6_flowlabel != 0)
1624 i = ACC_GET_HASH_INDEX(fp6->fi6_flowlabel);
1625 else
1626 /* flowlable can be zero */
1627 i = ACC_WILDCARD_INDEX;
1628
1629 /* go through this loop twice. first for flow hash, second
1630 for wildcards. */
1631 do {
1632 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1633 if (apply_filter6(afp->f_fbmask,
1634 (struct flow_filter6 *)&afp->f_filter,
1635 fp6))
1636 /* filter matched */
1637 return (afp->f_class);
1638
1639 /*
1640 * check again for filters with a wildcard.
1641 */
1642 if (i != ACC_WILDCARD_INDEX)
1643 i = ACC_WILDCARD_INDEX;
1644 else
1645 break;
1646 } while (1);
1647 }
1648 #endif /* INET6 */
1649
1650 /* no filter matched */
1651 return (NULL);
1652 }
1653
1654 static int
apply_filter4(fbmask,filt,pkt)1655 apply_filter4(fbmask, filt, pkt)
1656 u_int32_t fbmask;
1657 struct flow_filter *filt;
1658 struct flowinfo_in *pkt;
1659 {
1660 if (filt->ff_flow.fi_family != AF_INET)
1661 return (0);
1662 if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1663 return (0);
1664 if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1665 return (0);
1666 if ((fbmask & FIMB4_DADDR) &&
1667 filt->ff_flow.fi_dst.s_addr !=
1668 (pkt->fi_dst.s_addr & filt->ff_mask.mask_dst.s_addr))
1669 return (0);
1670 if ((fbmask & FIMB4_SADDR) &&
1671 filt->ff_flow.fi_src.s_addr !=
1672 (pkt->fi_src.s_addr & filt->ff_mask.mask_src.s_addr))
1673 return (0);
1674 if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1675 return (0);
1676 if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1677 (pkt->fi_tos & filt->ff_mask.mask_tos))
1678 return (0);
1679 if ((fbmask & FIMB4_GPI) && filt->ff_flow.fi_gpi != (pkt->fi_gpi))
1680 return (0);
1681 /* match */
1682 return (1);
1683 }
1684
1685 /*
1686 * filter matching function optimized for a common case that checks
1687 * only protocol and port numbers
1688 */
1689 static int
apply_ppfilter4(fbmask,filt,pkt)1690 apply_ppfilter4(fbmask, filt, pkt)
1691 u_int32_t fbmask;
1692 struct flow_filter *filt;
1693 struct flowinfo_in *pkt;
1694 {
1695 if (filt->ff_flow.fi_family != AF_INET)
1696 return (0);
1697 if ((fbmask & FIMB4_SPORT) && filt->ff_flow.fi_sport != pkt->fi_sport)
1698 return (0);
1699 if ((fbmask & FIMB4_DPORT) && filt->ff_flow.fi_dport != pkt->fi_dport)
1700 return (0);
1701 if ((fbmask & FIMB4_PROTO) && filt->ff_flow.fi_proto != pkt->fi_proto)
1702 return (0);
1703 /* match */
1704 return (1);
1705 }
1706
1707 /*
1708 * filter matching function only for tos field.
1709 */
1710 static int
apply_tosfilter4(fbmask,filt,pkt)1711 apply_tosfilter4(fbmask, filt, pkt)
1712 u_int32_t fbmask;
1713 struct flow_filter *filt;
1714 struct flowinfo_in *pkt;
1715 {
1716 if (filt->ff_flow.fi_family != AF_INET)
1717 return (0);
1718 if ((fbmask & FIMB4_TOS) && filt->ff_flow.fi_tos !=
1719 (pkt->fi_tos & filt->ff_mask.mask_tos))
1720 return (0);
1721 /* match */
1722 return (1);
1723 }
1724
1725 #ifdef INET6
1726 static int
apply_filter6(fbmask,filt,pkt)1727 apply_filter6(fbmask, filt, pkt)
1728 u_int32_t fbmask;
1729 struct flow_filter6 *filt;
1730 struct flowinfo_in6 *pkt;
1731 {
1732 int i;
1733
1734 if (filt->ff_flow6.fi6_family != AF_INET6)
1735 return (0);
1736 if ((fbmask & FIMB6_FLABEL) &&
1737 filt->ff_flow6.fi6_flowlabel != pkt->fi6_flowlabel)
1738 return (0);
1739 if ((fbmask & FIMB6_PROTO) &&
1740 filt->ff_flow6.fi6_proto != pkt->fi6_proto)
1741 return (0);
1742 if ((fbmask & FIMB6_SPORT) &&
1743 filt->ff_flow6.fi6_sport != pkt->fi6_sport)
1744 return (0);
1745 if ((fbmask & FIMB6_DPORT) &&
1746 filt->ff_flow6.fi6_dport != pkt->fi6_dport)
1747 return (0);
1748 if (fbmask & FIMB6_SADDR) {
1749 for (i = 0; i < 4; i++)
1750 if (filt->ff_flow6.fi6_src.s6_addr32[i] !=
1751 (pkt->fi6_src.s6_addr32[i] &
1752 filt->ff_mask6.mask6_src.s6_addr32[i]))
1753 return (0);
1754 }
1755 if (fbmask & FIMB6_DADDR) {
1756 for (i = 0; i < 4; i++)
1757 if (filt->ff_flow6.fi6_dst.s6_addr32[i] !=
1758 (pkt->fi6_dst.s6_addr32[i] &
1759 filt->ff_mask6.mask6_dst.s6_addr32[i]))
1760 return (0);
1761 }
1762 if ((fbmask & FIMB6_TCLASS) &&
1763 filt->ff_flow6.fi6_tclass !=
1764 (pkt->fi6_tclass & filt->ff_mask6.mask6_tclass))
1765 return (0);
1766 if ((fbmask & FIMB6_GPI) &&
1767 filt->ff_flow6.fi6_gpi != pkt->fi6_gpi)
1768 return (0);
1769 /* match */
1770 return (1);
1771 }
1772 #endif /* INET6 */
1773
1774 /*
1775 * filter handle:
1776 * bit 20-28: index to the filter hash table
1777 * bit 0-19: unique id in the hash bucket.
1778 */
1779 static u_long
get_filt_handle(classifier,i)1780 get_filt_handle(classifier, i)
1781 struct acc_classifier *classifier;
1782 int i;
1783 {
1784 static u_long handle_number = 1;
1785 u_long handle;
1786 struct acc_filter *afp;
1787
1788 while (1) {
1789 handle = handle_number++ & 0x000fffff;
1790
1791 if (LIST_EMPTY(&classifier->acc_filters[i]))
1792 break;
1793
1794 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1795 if ((afp->f_handle & 0x000fffff) == handle)
1796 break;
1797 if (afp == NULL)
1798 break;
1799 /* this handle is already used, try again */
1800 }
1801
1802 return ((i << 20) | handle);
1803 }
1804
1805 /* convert filter handle to filter pointer */
1806 static struct acc_filter *
filth_to_filtp(classifier,handle)1807 filth_to_filtp(classifier, handle)
1808 struct acc_classifier *classifier;
1809 u_long handle;
1810 {
1811 struct acc_filter *afp;
1812 int i;
1813
1814 i = ACC_GET_HINDEX(handle);
1815
1816 LIST_FOREACH(afp, &classifier->acc_filters[i], f_chain)
1817 if (afp->f_handle == handle)
1818 return (afp);
1819
1820 return (NULL);
1821 }
1822
1823 /* create flowinfo bitmask */
1824 static u_int32_t
filt2fibmask(filt)1825 filt2fibmask(filt)
1826 struct flow_filter *filt;
1827 {
1828 u_int32_t mask = 0;
1829 #ifdef INET6
1830 struct flow_filter6 *filt6;
1831 #endif
1832
1833 switch (filt->ff_flow.fi_family) {
1834 case AF_INET:
1835 if (filt->ff_flow.fi_proto != 0)
1836 mask |= FIMB4_PROTO;
1837 if (filt->ff_flow.fi_tos != 0)
1838 mask |= FIMB4_TOS;
1839 if (filt->ff_flow.fi_dst.s_addr != 0)
1840 mask |= FIMB4_DADDR;
1841 if (filt->ff_flow.fi_src.s_addr != 0)
1842 mask |= FIMB4_SADDR;
1843 if (filt->ff_flow.fi_sport != 0)
1844 mask |= FIMB4_SPORT;
1845 if (filt->ff_flow.fi_dport != 0)
1846 mask |= FIMB4_DPORT;
1847 if (filt->ff_flow.fi_gpi != 0)
1848 mask |= FIMB4_GPI;
1849 break;
1850 #ifdef INET6
1851 case AF_INET6:
1852 filt6 = (struct flow_filter6 *)filt;
1853
1854 if (filt6->ff_flow6.fi6_proto != 0)
1855 mask |= FIMB6_PROTO;
1856 if (filt6->ff_flow6.fi6_tclass != 0)
1857 mask |= FIMB6_TCLASS;
1858 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_dst))
1859 mask |= FIMB6_DADDR;
1860 if (!IN6_IS_ADDR_UNSPECIFIED(&filt6->ff_flow6.fi6_src))
1861 mask |= FIMB6_SADDR;
1862 if (filt6->ff_flow6.fi6_sport != 0)
1863 mask |= FIMB6_SPORT;
1864 if (filt6->ff_flow6.fi6_dport != 0)
1865 mask |= FIMB6_DPORT;
1866 if (filt6->ff_flow6.fi6_gpi != 0)
1867 mask |= FIMB6_GPI;
1868 if (filt6->ff_flow6.fi6_flowlabel != 0)
1869 mask |= FIMB6_FLABEL;
1870 break;
1871 #endif /* INET6 */
1872 }
1873 return (mask);
1874 }
1875
1876
1877 /*
1878 * helper functions to handle IPv4 fragments.
1879 * currently only in-sequence fragments are handled.
1880 * - fragment info is cached in a LRU list.
1881 * - when a first fragment is found, cache its flow info.
1882 * - when a non-first fragment is found, lookup the cache.
1883 */
1884
1885 struct ip4_frag {
1886 TAILQ_ENTRY(ip4_frag) ip4f_chain;
1887 char ip4f_valid;
1888 u_short ip4f_id;
1889 struct flowinfo_in ip4f_info;
1890 };
1891
1892 static TAILQ_HEAD(ip4f_list, ip4_frag) ip4f_list; /* IPv4 fragment cache */
1893
1894 #define IP4F_TABSIZE 16 /* IPv4 fragment cache size */
1895
1896
1897 static void
ip4f_cache(ip,fin)1898 ip4f_cache(ip, fin)
1899 struct ip *ip;
1900 struct flowinfo_in *fin;
1901 {
1902 struct ip4_frag *fp;
1903
1904 if (TAILQ_EMPTY(&ip4f_list)) {
1905 /* first time call, allocate fragment cache entries. */
1906 if (ip4f_init() < 0)
1907 /* allocation failed! */
1908 return;
1909 }
1910
1911 fp = ip4f_alloc();
1912 fp->ip4f_id = ip->ip_id;
1913 fp->ip4f_info.fi_proto = ip->ip_p;
1914 fp->ip4f_info.fi_src.s_addr = ip->ip_src.s_addr;
1915 fp->ip4f_info.fi_dst.s_addr = ip->ip_dst.s_addr;
1916
1917 /* save port numbers */
1918 fp->ip4f_info.fi_sport = fin->fi_sport;
1919 fp->ip4f_info.fi_dport = fin->fi_dport;
1920 fp->ip4f_info.fi_gpi = fin->fi_gpi;
1921 }
1922
1923 static int
ip4f_lookup(ip,fin)1924 ip4f_lookup(ip, fin)
1925 struct ip *ip;
1926 struct flowinfo_in *fin;
1927 {
1928 struct ip4_frag *fp;
1929
1930 for (fp = TAILQ_FIRST(&ip4f_list); fp != NULL && fp->ip4f_valid;
1931 fp = TAILQ_NEXT(fp, ip4f_chain))
1932 if (ip->ip_id == fp->ip4f_id &&
1933 ip->ip_src.s_addr == fp->ip4f_info.fi_src.s_addr &&
1934 ip->ip_dst.s_addr == fp->ip4f_info.fi_dst.s_addr &&
1935 ip->ip_p == fp->ip4f_info.fi_proto) {
1936
1937 /* found the matching entry */
1938 fin->fi_sport = fp->ip4f_info.fi_sport;
1939 fin->fi_dport = fp->ip4f_info.fi_dport;
1940 fin->fi_gpi = fp->ip4f_info.fi_gpi;
1941
1942 if ((ntohs(ip->ip_off) & IP_MF) == 0)
1943 /* this is the last fragment,
1944 release the entry. */
1945 ip4f_free(fp);
1946
1947 return (1);
1948 }
1949
1950 /* no matching entry found */
1951 return (0);
1952 }
1953
1954 static int
ip4f_init(void)1955 ip4f_init(void)
1956 {
1957 struct ip4_frag *fp;
1958 int i;
1959
1960 TAILQ_INIT(&ip4f_list);
1961 for (i=0; i<IP4F_TABSIZE; i++) {
1962 fp = malloc(sizeof(struct ip4_frag),
1963 M_DEVBUF, M_NOWAIT);
1964 if (fp == NULL) {
1965 printf("ip4f_init: can't alloc %dth entry!\n", i);
1966 if (i == 0)
1967 return (-1);
1968 return (0);
1969 }
1970 fp->ip4f_valid = 0;
1971 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
1972 }
1973 return (0);
1974 }
1975
1976 static struct ip4_frag *
ip4f_alloc(void)1977 ip4f_alloc(void)
1978 {
1979 struct ip4_frag *fp;
1980
1981 /* reclaim an entry at the tail, put it at the head */
1982 fp = TAILQ_LAST(&ip4f_list, ip4f_list);
1983 TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
1984 fp->ip4f_valid = 1;
1985 TAILQ_INSERT_HEAD(&ip4f_list, fp, ip4f_chain);
1986 return (fp);
1987 }
1988
1989 static void
ip4f_free(fp)1990 ip4f_free(fp)
1991 struct ip4_frag *fp;
1992 {
1993 TAILQ_REMOVE(&ip4f_list, fp, ip4f_chain);
1994 fp->ip4f_valid = 0;
1995 TAILQ_INSERT_TAIL(&ip4f_list, fp, ip4f_chain);
1996 }
1997
1998 #endif /* ALTQ3_CLFIER_COMPAT */
1999