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 | |
80 | u_int32_t classq_verbose = 0; /* more noise if greater than 1 */ |
81 | |
82 | SYSCTL_NODE(_net, OID_AUTO, classq, CTLFLAG_RW | CTLFLAG_LOCKED, 0, "classq" ); |
83 | |
84 | SYSCTL_UINT(_net_classq, OID_AUTO, verbose, CTLFLAG_RW | CTLFLAG_LOCKED, |
85 | &classq_verbose, 0, "Class queue verbosity level" ); |
86 | |
87 | void |
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 */ |
115 | void |
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 */ |
151 | void |
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 */ |
185 | void |
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 | |
235 | static 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 */ |
306 | void |
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 */ |
313 | void |
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 */ |
320 | void |
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 | |
372 | static 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 */ |
413 | void |
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 | |
434 | static 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 */ |
496 | void |
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 | |
517 | static 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 */ |
560 | void |
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 | |
579 | void |
580 | _flushq(class_queue_t *q) |
581 | { |
582 | (void) _flushq_flow(q, 0, NULL, NULL); |
583 | } |
584 | |
585 | static 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 |
636 | static 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 | |
686 | void |
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 | |