1/*
2 * Copyright (c) 2007-2021 Apple Inc. All rights reserved.
3 *
4 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
5 *
6 * This file contains Original Code and/or Modifications of Original Code
7 * as defined in and that are subject to the Apple Public Source License
8 * Version 2.0 (the 'License'). You may not use this file except in
9 * compliance with the License. The rights granted to you under the License
10 * may not be used to create, or enable the creation or redistribution of,
11 * unlawful or unlicensed copies of an Apple operating system, or to
12 * circumvent, violate, or enable the circumvention or violation of, any
13 * terms of an Apple operating system software license agreement.
14 *
15 * Please obtain a copy of the License at
16 * http://www.opensource.apple.com/apsl/ and read it before using this file.
17 *
18 * The Original Code and all software distributed under the License are
19 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
20 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
21 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
22 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
23 * Please see the License for the specific language governing rights and
24 * limitations under the License.
25 *
26 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
27 */
28
29/*
30 * Copyright (c) 1991-1997 Regents of the University of California.
31 * All rights reserved.
32 *
33 * Redistribution and use in source and binary forms, with or without
34 * modification, are permitted provided that the following conditions
35 * are met:
36 * 1. Redistributions of source code must retain the above copyright
37 * notice, this list of conditions and the following disclaimer.
38 * 2. Redistributions in binary form must reproduce the above copyright
39 * notice, this list of conditions and the following disclaimer in the
40 * documentation and/or other materials provided with the distribution.
41 * 3. All advertising materials mentioning features or use of this software
42 * must display the following acknowledgement:
43 * This product includes software developed by the Network Research
44 * Group at Lawrence Berkeley Laboratory.
45 * 4. Neither the name of the University nor of the Laboratory may be used
46 * to endorse or promote products derived from this software without
47 * specific prior written permission.
48 *
49 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
50 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
51 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
52 * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
53 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
54 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
55 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
56 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
57 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
58 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
59 * SUCH DAMAGE.
60 */
61
62#include <sys/cdefs.h>
63#include <sys/param.h>
64#include <sys/mbuf.h>
65#include <sys/errno.h>
66#include <sys/random.h>
67#include <sys/kernel_types.h>
68#include <sys/sysctl.h>
69
70#include <net/if.h>
71#include <net/net_osdep.h>
72#include <net/classq/classq.h>
73
74#include <libkern/libkern.h>
75
76#if SKYWALK
77#include <skywalk/os_skywalk_private.h>
78#endif /* SKYWALK */
79
80u_int32_t classq_verbose = 0; /* more noise if greater than 1 */
81
82SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW | CTLFLAG_LOCKED, 0, "classq");
83
84SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW | CTLFLAG_LOCKED,
85 &classq_verbose, 0, "Class queue verbosity level");
86
87void
88_qinit(class_queue_t *q, int type, int lim, classq_pkt_type_t ptype)
89{
90 switch (ptype) {
91 case QP_MBUF:
92 MBUFQ_INIT(&qmbufq(q));
93 break;
94
95#if SKYWALK
96 case QP_PACKET:
97 KPKTQ_INIT(&qkpktq(q));
98 break;
99#endif /* SKYWALK */
100
101 default:
102 VERIFY(0);
103 /* NOTREACHED */
104 }
105
106 qlimit(q) = lim;
107 qlen(q) = 0;
108 qsize(q) = 0;
109 qtype(q) = type;
110 qptype(q) = ptype;
111 qstate(q) = QS_RUNNING;
112}
113
114/* add a packet at the tail of the queue */
115void
116_addq(class_queue_t *q, classq_pkt_t *pkt)
117{
118 uint32_t size = 0;
119
120 ASSERT(pkt->cp_ptype == qptype(q));
121
122 switch (qptype(q)) {
123 case QP_MBUF: {
124 struct mbuf *m = pkt->cp_mbuf;
125 MBUFQ_ENQUEUE(&qmbufq(q), m);
126 size = m_length(m);
127 break;
128 }
129
130#if SKYWALK
131 case QP_PACKET: {
132 struct __kern_packet *kp = pkt->cp_kpkt;
133 KPKTQ_ENQUEUE(&qkpktq(q), kp);
134 size = kp->pkt_length;
135 break;
136 }
137#endif /* SKYWALK */
138
139 default:
140 VERIFY(0);
141 /* NOTREACHED */
142 __builtin_unreachable();
143 }
144
145 qlen(q)++;
146 VERIFY(qlen(q) != 0);
147 qsize(q) += size;
148}
149
150/* add one or more packets at the tail of the queue */
151void
152_addq_multi(class_queue_t *q, classq_pkt_t *pkt_head, classq_pkt_t *pkt_tail,
153 u_int32_t cnt, u_int64_t size)
154{
155 ASSERT(pkt_head->cp_ptype == qptype(q));
156 ASSERT(pkt_tail->cp_ptype == qptype(q));
157 switch (qptype(q)) {
158 case QP_MBUF: {
159 struct mbuf *m_head = pkt_head->cp_mbuf;
160 struct mbuf *m_tail = pkt_tail->cp_mbuf;
161 MBUFQ_ENQUEUE_MULTI(&qmbufq(q), m_head, m_tail);
162 break;
163 }
164
165#if SKYWALK
166 case QP_PACKET: {
167 struct __kern_packet *kp_head = pkt_head->cp_kpkt;
168 struct __kern_packet *kp_tail = pkt_tail->cp_kpkt;
169 KPKTQ_ENQUEUE_MULTI(&qkpktq(q), kp_head, kp_tail, cnt);
170 break;
171 }
172#endif /* SKYWALK */
173
174 default:
175 VERIFY(0);
176 /* NOTREACHED */
177 __builtin_unreachable();
178 }
179
180 qlen(q) += cnt;
181 qsize(q) += size;
182}
183
184/* get a packet at the head of the queue */
185void
186_getq(class_queue_t *q, classq_pkt_t *pkt)
187{
188 uint32_t pkt_len = 0;
189
190 switch (qptype(q)) {
191 case QP_MBUF: {
192 MBUFQ_DEQUEUE(&qmbufq(q), pkt->cp_mbuf);
193 if (__probable(pkt->cp_mbuf != NULL)) {
194 CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
195 pkt_len = m_length(pkt->cp_mbuf);
196 }
197 break;
198 }
199
200#if SKYWALK
201 case QP_PACKET: {
202 KPKTQ_DEQUEUE(&qkpktq(q), pkt->cp_kpkt);
203 if (__probable(pkt->cp_kpkt != NULL)) {
204 CLASSQ_PKT_INIT_PACKET(pkt, pkt->cp_kpkt);
205 pkt_len = pkt->cp_kpkt->pkt_length;
206 }
207 break;
208 }
209#endif /* SKYWALK */
210
211 default:
212 VERIFY(0);
213 /* NOTREACHED */
214 __builtin_unreachable();
215 }
216
217 if (pkt->cp_mbuf == NULL) {
218 VERIFY(qlen(q) == 0);
219 if (qsize(q) > 0) {
220 qsize(q) = 0;
221 }
222 return;
223 }
224 VERIFY(qlen(q) > 0);
225 qlen(q)--;
226
227 /* qsize is an approximation, so adjust if necessary */
228 if (((int)qsize(q) - pkt_len) > 0) {
229 qsize(q) -= pkt_len;
230 } else if (qsize(q) != 0) {
231 qsize(q) = 0;
232 }
233}
234
235static void
236_getq_flow_or_scidx(class_queue_t *q, classq_pkt_t *pkt, u_int32_t val,
237 boolean_t isflowid)
238{
239 uint32_t pkt_len = 0;
240
241 switch (qptype(q)) {
242 case QP_MBUF: {
243 struct mbuf *m, *m_tmp;
244
245 MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
246 if ((isflowid && (val == 0 ||
247 ((m->m_flags & M_PKTHDR) &&
248 m->m_pkthdr.pkt_flowid == val))) ||
249 (!isflowid &&
250 MBUF_SCIDX(mbuf_get_service_class(m)) < val)) {
251 /* remove it from the class queue */
252 MBUFQ_REMOVE(&qmbufq(q), m);
253 MBUFQ_NEXT(m) = NULL;
254 break;
255 }
256 }
257 if (__probable(m != NULL)) {
258 CLASSQ_PKT_INIT_MBUF(pkt, m);
259 pkt_len = m_length(m);
260 }
261 break;
262 }
263
264#if SKYWALK
265 case QP_PACKET: {
266 struct __kern_packet *kp, *kp_tmp;
267
268 KPKTQ_FOREACH_SAFE(kp, &qkpktq(q), kp_tmp) {
269 if ((isflowid && (val == 0 ||
270 (kp->pkt_flow_token == val))) || (!isflowid &&
271 MBUF_SCIDX(kp->pkt_svc_class) < val)) {
272 /* remove it from the class queue */
273 KPKTQ_REMOVE(&qkpktq(q), kp);
274 KPKTQ_NEXT(kp) = NULL;
275 break;
276 }
277 }
278 if (__probable(kp != NULL)) {
279 CLASSQ_PKT_INIT_PACKET(pkt, kp);
280 pkt_len = kp->pkt_length;
281 }
282 break;
283 }
284#endif /* SKYWALK */
285
286 default:
287 VERIFY(0);
288 /* NOTREACHED */
289 __builtin_unreachable();
290 }
291
292 if (pkt->cp_mbuf != NULL) {
293 VERIFY(qlen(q) > 0);
294 qlen(q)--;
295
296 /* qsize is an approximation, so adjust if necessary */
297 if (((int)qsize(q) - pkt_len) > 0) {
298 qsize(q) -= pkt_len;
299 } else if (qsize(q) != 0) {
300 qsize(q) = 0;
301 }
302 }
303}
304
305/* get a packet of a specific flow beginning from the head of the queue */
306void
307_getq_flow(class_queue_t *q, classq_pkt_t *pkt, u_int32_t flow)
308{
309 return _getq_flow_or_scidx(q, pkt, val: flow, TRUE);
310}
311
312/* Get a packet whose MBUF_SCIDX() < scidx from head of queue */
313void
314_getq_scidx_lt(class_queue_t *q, classq_pkt_t *pkt, u_int32_t scidx)
315{
316 return _getq_flow_or_scidx(q, pkt, val: scidx, FALSE);
317}
318
319/* get all packets (chained) starting from the head of the queue */
320void
321_getq_all(class_queue_t *q, classq_pkt_t *first, classq_pkt_t *last,
322 u_int32_t *qlenp, u_int64_t *qsizep)
323{
324 switch (qptype(q)) {
325 case QP_MBUF:
326 first->cp_mbuf = MBUFQ_FIRST(&qmbufq(q));
327 if (__probable(first->cp_mbuf != NULL)) {
328 CLASSQ_PKT_INIT_MBUF(first, first->cp_mbuf);
329 }
330 if (last != NULL) {
331 last->cp_mbuf = MBUFQ_LAST(&qmbufq(q));
332 if (__probable(last->cp_mbuf != NULL)) {
333 CLASSQ_PKT_INIT_MBUF(last, last->cp_mbuf);
334 }
335 }
336 MBUFQ_INIT(&qmbufq(q));
337 break;
338
339#if SKYWALK
340 case QP_PACKET:
341 first->cp_kpkt = KPKTQ_FIRST(&qkpktq(q));
342 if (__probable(first->cp_kpkt != NULL)) {
343 CLASSQ_PKT_INIT_PACKET(first, first->cp_kpkt);
344 }
345 if (last != NULL) {
346 last->cp_kpkt = KPKTQ_LAST(&qkpktq(q));
347 if (__probable(last->cp_kpkt != NULL)) {
348 CLASSQ_PKT_INIT_PACKET(last, last->cp_kpkt);
349 }
350 }
351 KPKTQ_INIT(&qkpktq(q));
352 break;
353#endif /* SKYWALK */
354
355 default:
356 VERIFY(0);
357 /* NOTREACHED */
358 __builtin_unreachable();
359 }
360
361 if (qlenp != NULL) {
362 *qlenp = qlen(q);
363 }
364 if (qsizep != NULL) {
365 *qsizep = qsize(q);
366 }
367
368 qlen(q) = 0;
369 qsize(q) = 0;
370}
371
372static inline struct mbuf *
373_getq_tail_mbuf(class_queue_t *q)
374{
375 struct mq_head *head = &qmbufq(q);
376 struct mbuf *m = MBUFQ_LAST(head);
377
378 if (m != NULL) {
379 struct mbuf *n = MBUFQ_FIRST(head);
380
381 while (n != NULL) {
382 struct mbuf *next = MBUFQ_NEXT(n);
383 if (next == m) {
384 MBUFQ_NEXT(n) = NULL;
385 break;
386 }
387 n = next;
388 }
389 VERIFY(n != NULL ||
390 (qlen(q) == 1 && m == MBUFQ_FIRST(head)));
391 VERIFY(qlen(q) > 0);
392 --qlen(q);
393
394 /* qsize is an approximation, so adjust if necessary */
395 if (((int)qsize(q) - m_length(m)) > 0) {
396 qsize(q) -= m_length(m);
397 } else if (qsize(q) != 0) {
398 qsize(q) = 0;
399 }
400
401 if (qempty(q)) {
402 VERIFY(m == MBUFQ_FIRST(head));
403 MBUFQ_INIT(head);
404 } else {
405 VERIFY(n != NULL);
406 head->mq_last = &MBUFQ_NEXT(n);
407 }
408 }
409 return m;
410}
411
412/* drop a packet at the tail of the queue */
413void
414_getq_tail(class_queue_t *q, classq_pkt_t *pkt)
415{
416 switch (qptype(q)) {
417 case QP_MBUF:
418 pkt->cp_mbuf = _getq_tail_mbuf(q);
419 if (__probable(pkt->cp_mbuf != NULL)) {
420 CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
421 }
422 break;
423
424#if SKYWALK
425 case QP_PACKET: /* XXX: Add support for Kernel packet when needed */
426#endif /* SKYWALK */
427 default:
428 VERIFY(0);
429 /* NOTREACHED */
430 __builtin_unreachable();
431 }
432}
433
434static inline struct mbuf *
435_getq_random_mbuf(class_queue_t *q)
436{
437 struct mq_head *head = &qmbufq(q);
438 struct mbuf *m = NULL;
439 unsigned int n;
440 u_int32_t rnd;
441
442 /* XXX: Add support for Kernel packet when needed */
443 VERIFY((qptype(q) == QP_MBUF));
444
445 n = qlen(q);
446 if (n == 0) {
447 VERIFY(MBUFQ_EMPTY(head));
448 if (qsize(q) > 0) {
449 qsize(q) = 0;
450 }
451 return NULL;
452 }
453
454 m = MBUFQ_FIRST(head);
455 read_frandom(buffer: &rnd, numBytes: sizeof(rnd));
456 n = (rnd % n) + 1;
457
458 if (n == 1) {
459 if ((MBUFQ_FIRST(head) = MBUFQ_NEXT(m)) == NULL) {
460 (head)->mq_last = &MBUFQ_FIRST(head);
461 }
462 } else {
463 struct mbuf *p = NULL;
464
465 VERIFY(n > 1);
466 while (n--) {
467 if (MBUFQ_NEXT(m) == NULL) {
468 break;
469 }
470 p = m;
471 m = MBUFQ_NEXT(m);
472 }
473 VERIFY(p != NULL && MBUFQ_NEXT(p) == m);
474
475 if ((MBUFQ_NEXT(p) = MBUFQ_NEXT(m)) == NULL) {
476 (head)->mq_last = &MBUFQ_NEXT(p);
477 }
478 }
479
480 VERIFY(qlen(q) > 0);
481 --qlen(q);
482
483 /* qsize is an approximation, so adjust if necessary */
484 if (((int)qsize(q) - m_length(m)) > 0) {
485 qsize(q) -= m_length(m);
486 } else if (qsize(q) != 0) {
487 qsize(q) = 0;
488 }
489
490 MBUFQ_NEXT(m) = NULL;
491
492 return m;
493}
494
495/* randomly select a packet in the queue */
496void
497_getq_random(class_queue_t *q, classq_pkt_t *pkt)
498{
499 switch (qptype(q)) {
500 case QP_MBUF:
501 pkt->cp_mbuf = _getq_random_mbuf(q);
502 if (__probable(pkt->cp_mbuf != NULL)) {
503 CLASSQ_PKT_INIT_MBUF(pkt, pkt->cp_mbuf);
504 }
505 break;
506
507#if SKYWALK
508 case QP_PACKET: /* XXX: Add support for Kernel packet when needed */
509#endif /* SKYWALK */
510 default:
511 VERIFY(0);
512 /* NOTREACHED */
513 __builtin_unreachable();
514 }
515}
516
517static inline void
518_removeq_mbuf(class_queue_t *q, struct mbuf *m)
519{
520 struct mq_head *head = &qmbufq(q);
521 struct mbuf *m0, **mtail;
522
523 m0 = MBUFQ_FIRST(head);
524 if (m0 == NULL) {
525 return;
526 }
527
528 if (m0 != m) {
529 while (m0 != NULL && MBUFQ_NEXT(m0) != m) {
530 m0 = MBUFQ_NEXT(m0);
531 }
532 if (m0 == NULL) {
533 return;
534 }
535
536 mtail = &MBUFQ_NEXT(m0);
537 } else {
538 mtail = &MBUFQ_FIRST(head);
539 }
540
541 *mtail = MBUFQ_NEXT(m);
542 if (*mtail == NULL) {
543 head->mq_last = mtail;
544 }
545
546 VERIFY(qlen(q) > 0);
547 --qlen(q);
548
549 /* qsize is an approximation, so adjust if necessary */
550 if (((int)qsize(q) - m_length(m)) > 0) {
551 qsize(q) -= m_length(m);
552 } else if (qsize(q) != 0) {
553 qsize(q) = 0;
554 }
555
556 MBUFQ_NEXT(m) = NULL;
557}
558
559/* remove a packet from the queue */
560void
561_removeq(class_queue_t *q, classq_pkt_t *pkt)
562{
563 switch (qptype(q)) {
564 case QP_MBUF:
565 ASSERT(pkt->cp_ptype == QP_MBUF);
566 _removeq_mbuf(q, m: pkt->cp_mbuf);
567 break;
568
569#if SKYWALK
570 case QP_PACKET: /* XXX: Add support for Kernel packet when needed */
571#endif /* SKYWALK */
572 default:
573 VERIFY(0);
574 /* NOTREACHED */
575 __builtin_unreachable();
576 }
577}
578
579void
580_flushq(class_queue_t *q)
581{
582 (void) _flushq_flow(q, 0, NULL, NULL);
583}
584
585static inline void
586_flushq_flow_mbuf(class_queue_t *q, u_int32_t flow, u_int32_t *cnt,
587 u_int32_t *len)
588{
589 MBUFQ_HEAD(mq_freeq) freeq;
590 struct mbuf *m, *m_tmp;
591 u_int32_t c = 0, l = 0;
592
593 MBUFQ_INIT(&freeq);
594
595 MBUFQ_FOREACH_SAFE(m, &qmbufq(q), m_tmp) {
596 if (flow == 0 || ((m->m_flags & M_PKTHDR) &&
597 m->m_pkthdr.pkt_flowid == flow)) {
598 /* remove it from the class queue */
599 MBUFQ_REMOVE(&qmbufq(q), m);
600 MBUFQ_NEXT(m) = NULL;
601
602 /* and add it to the free queue */
603 MBUFQ_ENQUEUE(&freeq, m);
604
605 l += m_length(m);
606 c++;
607 }
608 }
609 VERIFY(c == 0 || !MBUFQ_EMPTY(&freeq));
610
611 if (c > 0) {
612 VERIFY(qlen(q) >= c);
613 qlen(q) -= c;
614
615 /* qsize is an approximation, so adjust if necessary */
616 if (((int)qsize(q) - l) > 0) {
617 qsize(q) -= l;
618 } else if (qsize(q) != 0) {
619 qsize(q) = 0;
620 }
621 }
622
623 if (!MBUFQ_EMPTY(&freeq)) {
624 m_freem_list(MBUFQ_FIRST(&freeq));
625 }
626
627 if (cnt != NULL) {
628 *cnt = c;
629 }
630 if (len != NULL) {
631 *len = l;
632 }
633}
634
635#if SKYWALK
636static inline void
637_flushq_flow_kpkt(class_queue_t *q, u_int32_t flow, u_int32_t *cnt,
638 u_int32_t *len)
639{
640 KPKTQ_HEAD(pq_freeq) freeq;
641 struct __kern_packet *p, *p_tmp;
642 u_int32_t c = 0, l = 0;
643
644 KPKTQ_INIT(&freeq);
645
646 KPKTQ_FOREACH_SAFE(p, &qkpktq(q), p_tmp) {
647 if (flow == 0 || p->pkt_flow_token == flow) {
648 /* remove it from the class queue */
649 KPKTQ_REMOVE(&qkpktq(q), p);
650 KPKTQ_NEXT(p) = NULL;
651
652 /* and add it to the free queue */
653 KPKTQ_ENQUEUE(&freeq, p);
654
655 l += p->pkt_length;
656 c++;
657 }
658 }
659 VERIFY(c == 0 || !KPKTQ_EMPTY(&freeq));
660
661 if (c > 0) {
662 VERIFY(qlen(q) >= c);
663 qlen(q) -= c;
664
665 /* qsize is an approximation, so adjust if necessary */
666 if (((int)qsize(q) - l) > 0) {
667 qsize(q) -= l;
668 } else if (qsize(q) != 0) {
669 qsize(q) = 0;
670 }
671 }
672
673 if (!KPKTQ_EMPTY(&freeq)) {
674 pp_free_packet_chain(KPKTQ_FIRST(&freeq), NULL);
675 }
676
677 if (cnt != NULL) {
678 *cnt = c;
679 }
680 if (len != NULL) {
681 *len = l;
682 }
683}
684#endif /* SKYWALK */
685
686void
687_flushq_flow(class_queue_t *q, u_int32_t flow, u_int32_t *cnt, u_int32_t *len)
688{
689 switch (qptype(q)) {
690 case QP_MBUF:
691 _flushq_flow_mbuf(q, flow, cnt, len);
692 break;
693
694#if SKYWALK
695 case QP_PACKET:
696 _flushq_flow_kpkt(q, flow, cnt, len);
697 break;
698
699#endif /* SKYWALK */
700 default:
701 VERIFY(0);
702 /* NOTREACHED */
703 __builtin_unreachable();
704 }
705}
706