1 /*
2 * HND generic pktq operation primitives
3 *
4 * Portions of this code are copyright (c) 2021 Cypress Semiconductor Corporation
5 *
6 * Copyright (C) 1999-2017, Broadcom Corporation
7 *
8 * Unless you and Broadcom execute a separate written software license
9 * agreement governing use of this software, this software is licensed to you
10 * under the terms of the GNU General Public License version 2 (the "GPL"),
11 * available at http://www.broadcom.com/licenses/GPLv2.php, with the
12 * following added to such license:
13 *
14 * As a special exception, the copyright holders of this software give you
15 * permission to link this software with independent modules, and to copy and
16 * distribute the resulting executable under terms of your choice, provided that
17 * you also meet, for each linked independent module, the terms and conditions of
18 * the license of that module. An independent module is a module which is not
19 * derived from this software. The special exception does not apply to any
20 * modifications of the software.
21 *
22 * Notwithstanding the above, under no circumstances may you combine this
23 * software in any way with any other Broadcom software provided under a license
24 * other than the GPL, without Broadcom's express prior written consent.
25 *
26 *
27 * <<Broadcom-WL-IPTag/Open:>>
28 *
29 * $Id: hnd_pktq.c 698847 2017-05-11 00:10:48Z $
30 */
31
32 #include <typedefs.h>
33 #include <osl.h>
34 #include <osl_ext.h>
35 #include <bcmutils.h>
36 #include <hnd_pktq.h>
37
38 /* mutex macros for thread safe */
39 #ifdef HND_PKTQ_THREAD_SAFE
40 #define HND_PKTQ_MUTEX_CREATE(name, mutex) osl_ext_mutex_create(name, mutex)
41 #define HND_PKTQ_MUTEX_DELETE(mutex) osl_ext_mutex_delete(mutex)
42 #define HND_PKTQ_MUTEX_ACQUIRE(mutex, msec) osl_ext_mutex_acquire(mutex, msec)
43 #define HND_PKTQ_MUTEX_RELEASE(mutex) osl_ext_mutex_release(mutex)
44 #else
45 #define HND_PKTQ_MUTEX_CREATE(name, mutex) OSL_EXT_SUCCESS
46 #define HND_PKTQ_MUTEX_DELETE(mutex) OSL_EXT_SUCCESS
47 #define HND_PKTQ_MUTEX_ACQUIRE(mutex, msec) OSL_EXT_SUCCESS
48 #define HND_PKTQ_MUTEX_RELEASE(mutex) OSL_EXT_SUCCESS
49 #endif /* HND_PKTQ_THREAD_SAFE */
50
51 /* status during txfifo sync */
52 #if defined(WLAMPDU_MAC) && defined(PROP_TXSTATUS)
53 #define TXQ_PKT_DEL 0x01
54 #define HEAD_PKT_FLUSHED 0xFF
55 #endif /* defined(WLAMPDU_MAC) && defined(PROP_TXSTATUS) */
56 /*
57 * osl multiple-precedence packet queue
58 * hi_prec is always >= the number of the highest non-empty precedence
59 */
60 void * BCMFASTPATH
pktq_penq(struct pktq * pq,int prec,void * p)61 pktq_penq(struct pktq *pq, int prec, void *p)
62 {
63 struct pktq_prec *q;
64
65 /* protect shared resource */
66 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
67 return NULL;
68
69 ASSERT(prec >= 0 && prec < pq->num_prec);
70 ASSERT(PKTLINK(p) == NULL); /* queueing chains not allowed */
71
72 ASSERT(!pktq_full(pq));
73 ASSERT(!pktqprec_full(pq, prec));
74
75 q = &pq->q[prec];
76
77 if (q->head)
78 PKTSETLINK(q->tail, p);
79 else
80 q->head = p;
81
82 q->tail = p;
83 q->n_pkts++;
84
85 pq->n_pkts_tot++;
86
87 if (pq->hi_prec < prec)
88 pq->hi_prec = (uint8)prec;
89
90 /* protect shared resource */
91 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
92 return NULL;
93
94 return p;
95 }
96
97 /*
98 * osl simple, non-priority packet queue
99 */
100 void * BCMFASTPATH
spktq_enq(struct spktq * spq,void * p)101 spktq_enq(struct spktq *spq, void *p)
102 {
103 struct pktq_prec *q;
104
105 /* protect shared resource */
106 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
107 return NULL;
108
109 ASSERT(!spktq_full(spq));
110
111 PKTSETLINK(p, NULL);
112
113 q = &spq->q;
114
115 if (q->head)
116 PKTSETLINK(q->tail, p);
117 else
118 q->head = p;
119
120 q->tail = p;
121 q->n_pkts++;
122
123 /* protect shared resource */
124 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
125 return NULL;
126
127 return p;
128 }
129
130 void * BCMFASTPATH
pktq_penq_head(struct pktq * pq,int prec,void * p)131 pktq_penq_head(struct pktq *pq, int prec, void *p)
132 {
133 struct pktq_prec *q;
134
135 /* protect shared resource */
136 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
137 return NULL;
138
139 ASSERT(prec >= 0 && prec < pq->num_prec);
140 ASSERT(PKTLINK(p) == NULL); /* queueing chains not allowed */
141
142 ASSERT(!pktq_full(pq));
143 ASSERT(!pktqprec_full(pq, prec));
144
145 q = &pq->q[prec];
146
147 if (q->head == NULL)
148 q->tail = p;
149
150 PKTSETLINK(p, q->head);
151 q->head = p;
152 q->n_pkts++;
153
154 pq->n_pkts_tot++;
155
156 if (pq->hi_prec < prec)
157 pq->hi_prec = (uint8)prec;
158
159 /* protect shared resource */
160 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
161 return NULL;
162
163 return p;
164 }
165
166 void * BCMFASTPATH
spktq_enq_head(struct spktq * spq,void * p)167 spktq_enq_head(struct spktq *spq, void *p)
168 {
169 struct pktq_prec *q;
170
171 /* protect shared resource */
172 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
173 return NULL;
174
175 ASSERT(!spktq_full(spq));
176
177 PKTSETLINK(p, NULL);
178
179 q = &spq->q;
180
181 if (q->head == NULL)
182 q->tail = p;
183
184 PKTSETLINK(p, q->head);
185 q->head = p;
186 q->n_pkts++;
187
188 /* protect shared resource */
189 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
190 return NULL;
191
192 return p;
193 }
194
195 void * BCMFASTPATH
pktq_pdeq(struct pktq * pq,int prec)196 pktq_pdeq(struct pktq *pq, int prec)
197 {
198 struct pktq_prec *q;
199 void *p;
200
201 /* protect shared resource */
202 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
203 return NULL;
204
205 ASSERT(prec >= 0 && prec < pq->num_prec);
206
207 q = &pq->q[prec];
208
209 if ((p = q->head) == NULL)
210 goto done;
211
212 if ((q->head = PKTLINK(p)) == NULL)
213 q->tail = NULL;
214
215 q->n_pkts--;
216
217 pq->n_pkts_tot--;
218
219 #ifdef WL_TXQ_STALL
220 q->dequeue_count++;
221 #endif // endif
222
223 PKTSETLINK(p, NULL);
224
225 done:
226 /* protect shared resource */
227 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
228 return NULL;
229
230 return p;
231 }
232
233 void * BCMFASTPATH
spktq_deq(struct spktq * spq)234 spktq_deq(struct spktq *spq)
235 {
236 struct pktq_prec *q;
237 void *p;
238
239 /* protect shared resource */
240 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
241 return NULL;
242
243 q = &spq->q;
244
245 if ((p = q->head) == NULL)
246 goto done;
247
248 if ((q->head = PKTLINK(p)) == NULL)
249 q->tail = NULL;
250
251 q->n_pkts--;
252
253 #ifdef WL_TXQ_STALL
254 q->dequeue_count++;
255 #endif // endif
256
257 PKTSETLINK(p, NULL);
258
259 done:
260 /* protect shared resource */
261 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
262 return NULL;
263
264 return p;
265 }
266
267 void * BCMFASTPATH
pktq_pdeq_tail(struct pktq * pq,int prec)268 pktq_pdeq_tail(struct pktq *pq, int prec)
269 {
270 struct pktq_prec *q;
271 void *p, *prev;
272
273 /* protect shared resource */
274 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
275 return NULL;
276
277 ASSERT(prec >= 0 && prec < pq->num_prec);
278
279 q = &pq->q[prec];
280
281 if ((p = q->head) == NULL)
282 goto done;
283
284 for (prev = NULL; p != q->tail; p = PKTLINK(p))
285 prev = p;
286
287 if (prev)
288 PKTSETLINK(prev, NULL);
289 else
290 q->head = NULL;
291
292 q->tail = prev;
293 q->n_pkts--;
294
295 pq->n_pkts_tot--;
296
297 #ifdef WL_TXQ_STALL
298 q->dequeue_count++;
299 #endif // endif
300 done:
301 /* protect shared resource */
302 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
303 return NULL;
304
305 return p;
306 }
307
308 void * BCMFASTPATH
spktq_deq_tail(struct spktq * spq)309 spktq_deq_tail(struct spktq *spq)
310 {
311 struct pktq_prec *q;
312 void *p, *prev;
313
314 /* protect shared resource */
315 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
316 return NULL;
317
318 q = &spq->q;
319
320 if ((p = q->head) == NULL)
321 goto done;
322
323 for (prev = NULL; p != q->tail; p = PKTLINK(p))
324 prev = p;
325
326 if (prev)
327 PKTSETLINK(prev, NULL);
328 else
329 q->head = NULL;
330
331 q->tail = prev;
332 q->n_pkts--;
333
334 #ifdef WL_TXQ_STALL
335 q->dequeue_count++;
336 #endif // endif
337 done:
338 /* protect shared resource */
339 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
340 return NULL;
341
342 return p;
343 }
344
345 void *
pktq_peek_tail(struct pktq * pq,int * prec_out)346 pktq_peek_tail(struct pktq *pq, int *prec_out)
347 {
348 int prec;
349 void *p = NULL;
350
351 /* protect shared resource */
352 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
353 return NULL;
354
355 if (pq->n_pkts_tot == 0)
356 goto done;
357
358 for (prec = 0; prec < pq->hi_prec; prec++)
359 if (pq->q[prec].head)
360 break;
361
362 if (prec_out)
363 *prec_out = prec;
364
365 p = pq->q[prec].tail;
366
367 done:
368 /* protect shared resource */
369 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
370 return NULL;
371
372 return p;
373 }
374
375 /*
376 * Append spktq 'list' to the tail of pktq 'pq'
377 */
378 void BCMFASTPATH
pktq_append(struct pktq * pq,int prec,struct spktq * list)379 pktq_append(struct pktq *pq, int prec, struct spktq *list)
380 {
381 struct pktq_prec *q;
382 struct pktq_prec *list_q;
383
384 /* protect shared resource */
385 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
386 return;
387
388 list_q = &list->q;
389
390 /* empty list check */
391 if (list_q->head == NULL)
392 goto done;
393
394 ASSERT(prec >= 0 && prec < pq->num_prec);
395 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
396
397 ASSERT(!pktq_full(pq));
398 ASSERT(!pktqprec_full(pq, prec));
399
400 q = &pq->q[prec];
401
402 if (q->head)
403 PKTSETLINK(q->tail, list_q->head);
404 else
405 q->head = list_q->head;
406
407 q->tail = list_q->tail;
408 q->n_pkts += list_q->n_pkts;
409 pq->n_pkts_tot += list_q->n_pkts;
410
411 if (pq->hi_prec < prec)
412 pq->hi_prec = (uint8)prec;
413
414 #ifdef WL_TXQ_STALL
415 list_q->dequeue_count += list_q->n_pkts;
416 #endif // endif
417
418 list_q->head = NULL;
419 list_q->tail = NULL;
420 list_q->n_pkts = 0;
421
422 done:
423 /* protect shared resource */
424 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
425 return;
426 }
427
428 /*
429 * Append spktq 'list' to the tail of spktq 'spq'
430 */
431 void BCMFASTPATH
spktq_append(struct spktq * spq,struct spktq * list)432 spktq_append(struct spktq *spq, struct spktq *list)
433 {
434 struct pktq_prec *q;
435 struct pktq_prec *list_q;
436
437 /* protect shared resource */
438 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
439 return;
440
441 list_q = &list->q;
442
443 /* empty list check */
444 if (list_q->head == NULL)
445 goto done;
446
447 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
448
449 ASSERT(!spktq_full(spq));
450
451 q = &spq->q;
452
453 if (q->head)
454 PKTSETLINK(q->tail, list_q->head);
455 else
456 q->head = list_q->head;
457
458 q->tail = list_q->tail;
459 q->n_pkts += list_q->n_pkts;
460
461 #ifdef WL_TXQ_STALL
462 list_q->dequeue_count += list_q->n_pkts;
463 #endif // endif
464
465 list_q->head = NULL;
466 list_q->tail = NULL;
467 list_q->n_pkts = 0;
468
469 done:
470 /* protect shared resource */
471 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
472 return;
473 }
474
475 /*
476 * Prepend spktq 'list' to the head of pktq 'pq'
477 */
478 void BCMFASTPATH
pktq_prepend(struct pktq * pq,int prec,struct spktq * list)479 pktq_prepend(struct pktq *pq, int prec, struct spktq *list)
480 {
481 struct pktq_prec *q;
482 struct pktq_prec *list_q;
483
484 /* protect shared resource */
485 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
486 return;
487
488 list_q = &list->q;
489
490 /* empty list check */
491 if (list_q->head == NULL)
492 goto done;
493
494 ASSERT(prec >= 0 && prec < pq->num_prec);
495 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
496
497 ASSERT(!pktq_full(pq));
498 ASSERT(!pktqprec_full(pq, prec));
499
500 q = &pq->q[prec];
501
502 /* set the tail packet of list to point at the former pq head */
503 PKTSETLINK(list_q->tail, q->head);
504 /* the new q head is the head of list */
505 q->head = list_q->head;
506
507 /* If the q tail was non-null, then it stays as is.
508 * If the q tail was null, it is now the tail of list
509 */
510 if (q->tail == NULL) {
511 q->tail = list_q->tail;
512 }
513
514 q->n_pkts += list_q->n_pkts;
515 pq->n_pkts_tot += list_q->n_pkts;
516
517 if (pq->hi_prec < prec)
518 pq->hi_prec = (uint8)prec;
519
520 #ifdef WL_TXQ_STALL
521 list_q->dequeue_count += list_q->n_pkts;
522 #endif // endif
523
524 list_q->head = NULL;
525 list_q->tail = NULL;
526 list_q->n_pkts = 0;
527
528 done:
529 /* protect shared resource */
530 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
531 return;
532 }
533
534 /*
535 * Prepend spktq 'list' to the head of spktq 'spq'
536 */
537 void BCMFASTPATH
spktq_prepend(struct spktq * spq,struct spktq * list)538 spktq_prepend(struct spktq *spq, struct spktq *list)
539 {
540 struct pktq_prec *q;
541 struct pktq_prec *list_q;
542
543 /* protect shared resource */
544 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
545 return;
546
547 list_q = &list->q;
548
549 /* empty list check */
550 if (list_q->head == NULL)
551 goto done;
552
553 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
554
555 ASSERT(!spktq_full(spq));
556
557 q = &spq->q;
558
559 /* set the tail packet of list to point at the former pq head */
560 PKTSETLINK(list_q->tail, q->head);
561 /* the new q head is the head of list */
562 q->head = list_q->head;
563
564 /* If the q tail was non-null, then it stays as is.
565 * If the q tail was null, it is now the tail of list
566 */
567 if (q->tail == NULL) {
568 q->tail = list_q->tail;
569 }
570
571 q->n_pkts += list_q->n_pkts;
572
573 #ifdef WL_TXQ_STALL
574 list_q->dequeue_count += list_q->n_pkts;
575 #endif // endif
576
577 list_q->head = NULL;
578 list_q->tail = NULL;
579 list_q->n_pkts = 0;
580
581 done:
582 /* protect shared resource */
583 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
584 return;
585 }
586
587 void * BCMFASTPATH
pktq_pdeq_prev(struct pktq * pq,int prec,void * prev_p)588 pktq_pdeq_prev(struct pktq *pq, int prec, void *prev_p)
589 {
590 struct pktq_prec *q;
591 void *p = NULL;
592
593 /* protect shared resource */
594 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
595 return NULL;
596
597 ASSERT(prec >= 0 && prec < pq->num_prec);
598
599 q = &pq->q[prec];
600
601 if (prev_p == NULL)
602 goto done;
603
604 if ((p = PKTLINK(prev_p)) == NULL)
605 goto done;
606
607 q->n_pkts--;
608
609 pq->n_pkts_tot--;
610
611 #ifdef WL_TXQ_STALL
612 q->dequeue_count++;
613 #endif // endif
614 PKTSETLINK(prev_p, PKTLINK(p));
615 PKTSETLINK(p, NULL);
616
617 done:
618 /* protect shared resource */
619 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
620 return NULL;
621
622 return p;
623 }
624
625 void * BCMFASTPATH
pktq_pdeq_with_fn(struct pktq * pq,int prec,ifpkt_cb_t fn,int arg)626 pktq_pdeq_with_fn(struct pktq *pq, int prec, ifpkt_cb_t fn, int arg)
627 {
628 struct pktq_prec *q;
629 void *p, *prev = NULL;
630
631 /* protect shared resource */
632 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
633 return NULL;
634
635 ASSERT(prec >= 0 && prec < pq->num_prec);
636
637 q = &pq->q[prec];
638 p = q->head;
639
640 while (p) {
641 if (fn == NULL || (*fn)(p, arg)) {
642 break;
643 } else {
644 prev = p;
645 p = PKTLINK(p);
646 }
647 }
648 if (p == NULL)
649 goto done;
650
651 if (prev == NULL) {
652 if ((q->head = PKTLINK(p)) == NULL) {
653 q->tail = NULL;
654 }
655 } else {
656 PKTSETLINK(prev, PKTLINK(p));
657 if (q->tail == p) {
658 q->tail = prev;
659 }
660 }
661
662 q->n_pkts--;
663
664 pq->n_pkts_tot--;
665
666 #ifdef WL_TXQ_STALL
667 q->dequeue_count++;
668 #endif // endif
669 PKTSETLINK(p, NULL);
670
671 done:
672 /* protect shared resource */
673 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
674 return NULL;
675
676 return p;
677 }
678
679 bool BCMFASTPATH
pktq_pdel(struct pktq * pq,void * pktbuf,int prec)680 pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
681 {
682 bool ret = FALSE;
683 struct pktq_prec *q;
684 void *p = NULL;
685
686 /* protect shared resource */
687 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
688 return FALSE;
689
690 ASSERT(prec >= 0 && prec < pq->num_prec);
691
692 /* Should this just assert pktbuf? */
693 if (!pktbuf)
694 goto done;
695
696 q = &pq->q[prec];
697
698 if (q->head == pktbuf) {
699 if ((q->head = PKTLINK(pktbuf)) == NULL)
700 q->tail = NULL;
701 } else {
702 for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
703 ;
704 if (p == NULL)
705 goto done;
706
707 PKTSETLINK(p, PKTLINK(pktbuf));
708 if (q->tail == pktbuf)
709 q->tail = p;
710 }
711
712 q->n_pkts--;
713 pq->n_pkts_tot--;
714
715 #ifdef WL_TXQ_STALL
716 q->dequeue_count++;
717 #endif // endif
718
719 PKTSETLINK(pktbuf, NULL);
720 ret = TRUE;
721
722 done:
723 /* protect shared resource */
724 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
725 return FALSE;
726
727 return ret;
728 }
729
730 static void
_pktq_pfilter(struct pktq * pq,int prec,pktq_filter_t fltr,void * fltr_ctx,defer_free_pkt_fn_t defer,void * defer_ctx)731 _pktq_pfilter(struct pktq *pq, int prec, pktq_filter_t fltr, void* fltr_ctx,
732 defer_free_pkt_fn_t defer, void *defer_ctx)
733 {
734 struct pktq_prec wq;
735 struct pktq_prec *q;
736 void *p;
737
738 /* protect shared resource */
739 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
740 return;
741
742 /* move the prec queue aside to a work queue */
743 q = &pq->q[prec];
744
745 wq = *q;
746
747 q->head = NULL;
748 q->tail = NULL;
749 q->n_pkts = 0;
750
751 #ifdef WL_TXQ_STALL
752 q->dequeue_count += wq.n_pkts;
753 #endif // endif
754
755 pq->n_pkts_tot -= wq.n_pkts;
756
757 /* protect shared resource */
758 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
759 return;
760
761 /* start with the head of the work queue */
762 while ((p = wq.head) != NULL) {
763 /* unlink the current packet from the list */
764 wq.head = PKTLINK(p);
765 PKTSETLINK(p, NULL);
766 wq.n_pkts--;
767
768 #ifdef WL_TXQ_STALL
769 wq.dequeue_count++;
770 #endif // endif
771
772 /* call the filter function on current packet */
773 ASSERT(fltr != NULL);
774 switch ((*fltr)(fltr_ctx, p)) {
775 case PKT_FILTER_NOACTION:
776 /* put this packet back */
777 pktq_penq(pq, prec, p);
778 break;
779
780 case PKT_FILTER_DELETE:
781 /* delete this packet */
782 ASSERT(defer != NULL);
783 (*defer)(defer_ctx, p);
784 break;
785
786 case PKT_FILTER_REMOVE:
787 /* pkt already removed from list */
788 break;
789
790 default:
791 ASSERT(0);
792 break;
793 }
794 }
795
796 ASSERT(wq.n_pkts == 0);
797 }
798
799 void
pktq_pfilter(struct pktq * pq,int prec,pktq_filter_t fltr,void * fltr_ctx,defer_free_pkt_fn_t defer,void * defer_ctx,flush_free_pkt_fn_t flush,void * flush_ctx)800 pktq_pfilter(struct pktq *pq, int prec, pktq_filter_t fltr, void* fltr_ctx,
801 defer_free_pkt_fn_t defer, void *defer_ctx, flush_free_pkt_fn_t flush, void *flush_ctx)
802 {
803 _pktq_pfilter(pq, prec, fltr, fltr_ctx, defer, defer_ctx);
804
805 ASSERT(flush != NULL);
806 (*flush)(flush_ctx);
807 }
808
809 void
pktq_filter(struct pktq * pq,pktq_filter_t fltr,void * fltr_ctx,defer_free_pkt_fn_t defer,void * defer_ctx,flush_free_pkt_fn_t flush,void * flush_ctx)810 pktq_filter(struct pktq *pq, pktq_filter_t fltr, void* fltr_ctx,
811 defer_free_pkt_fn_t defer, void *defer_ctx, flush_free_pkt_fn_t flush, void *flush_ctx)
812 {
813 bool filter = FALSE;
814
815 /* protect shared resource */
816 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
817 return;
818
819 /* Optimize if pktq n_pkts = 0, just return.
820 * pktq len of 0 means pktq's prec q's are all empty.
821 */
822 if (pq->n_pkts_tot > 0) {
823 filter = TRUE;
824 }
825
826 /* protect shared resource */
827 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
828 return;
829
830 if (filter) {
831 int prec;
832
833 PKTQ_PREC_ITER(pq, prec) {
834 _pktq_pfilter(pq, prec, fltr, fltr_ctx, defer, defer_ctx);
835 }
836
837 ASSERT(flush != NULL);
838 (*flush)(flush_ctx);
839 }
840 }
841
842 void
spktq_filter(struct spktq * spq,pktq_filter_t fltr,void * fltr_ctx,defer_free_pkt_fn_t defer,void * defer_ctx,flush_free_pkt_fn_t flush,void * flush_ctx)843 spktq_filter(struct spktq *spq, pktq_filter_t fltr, void* fltr_ctx,
844 defer_free_pkt_fn_t defer, void *defer_ctx, flush_free_pkt_fn_t flush, void *flush_ctx)
845 {
846 struct pktq_prec wq;
847 struct pktq_prec *q;
848 void *p = NULL;
849
850 /* protect shared resource */
851 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
852 return;
853
854 q = &spq->q;
855
856 /* Optimize if pktq_prec n_pkts = 0, just return. */
857 if (q->n_pkts == 0) {
858 (void)HND_PKTQ_MUTEX_RELEASE(&spq->mutex);
859 return;
860 }
861
862 wq = *q;
863
864 q->head = NULL;
865 q->tail = NULL;
866 q->n_pkts = 0;
867
868 #ifdef WL_TXQ_STALL
869 q->dequeue_count += wq.n_pkts;
870 #endif // endif
871
872 /* protect shared resource */
873 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
874 return;
875
876 /* start with the head of the work queue */
877
878 while ((p = wq.head) != NULL) {
879 /* unlink the current packet from the list */
880 wq.head = PKTLINK(p);
881 PKTSETLINK(p, NULL);
882 wq.n_pkts--;
883
884 #ifdef WL_TXQ_STALL
885 wq.dequeue_count++;
886 #endif // endif
887
888 /* call the filter function on current packet */
889 ASSERT(fltr != NULL);
890 switch ((*fltr)(fltr_ctx, p)) {
891 case PKT_FILTER_NOACTION:
892 /* put this packet back */
893 spktq_enq(spq, p);
894 break;
895
896 case PKT_FILTER_DELETE:
897 /* delete this packet */
898 ASSERT(defer != NULL);
899 (*defer)(defer_ctx, p);
900 break;
901
902 case PKT_FILTER_REMOVE:
903 /* pkt already removed from list */
904 break;
905
906 default:
907 ASSERT(0);
908 break;
909 }
910 }
911
912 ASSERT(wq.n_pkts == 0);
913
914 ASSERT(flush != NULL);
915 (*flush)(flush_ctx);
916 }
917
918 bool
pktq_init(struct pktq * pq,int num_prec,int max_pkts)919 pktq_init(struct pktq *pq, int num_prec, int max_pkts)
920 {
921 int prec;
922
923 ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
924
925 /* pq is variable size; only zero out what's requested */
926 bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
927
928 if (HND_PKTQ_MUTEX_CREATE("pktq", &pq->mutex) != OSL_EXT_SUCCESS)
929 return FALSE;
930
931 pq->num_prec = (uint16)num_prec;
932
933 pq->max_pkts = (uint16)max_pkts;
934
935 for (prec = 0; prec < num_prec; prec++)
936 pq->q[prec].max_pkts = pq->max_pkts;
937
938 return TRUE;
939 }
940
941 bool
spktq_init(struct spktq * spq,int max_pkts)942 spktq_init(struct spktq *spq, int max_pkts)
943 {
944 bzero(spq, sizeof(struct spktq));
945
946 if (HND_PKTQ_MUTEX_CREATE("spktq", &spq->mutex) != OSL_EXT_SUCCESS)
947 return FALSE;
948
949 spq->q.max_pkts = (uint16)max_pkts;
950
951 return TRUE;
952 }
953
954 bool
pktq_deinit(struct pktq * pq)955 pktq_deinit(struct pktq *pq)
956 {
957 BCM_REFERENCE(pq);
958 if (HND_PKTQ_MUTEX_DELETE(&pq->mutex) != OSL_EXT_SUCCESS)
959 return FALSE;
960
961 return TRUE;
962 }
963
964 bool
spktq_deinit(struct spktq * spq)965 spktq_deinit(struct spktq *spq)
966 {
967 BCM_REFERENCE(spq);
968 if (HND_PKTQ_MUTEX_DELETE(&spq->mutex) != OSL_EXT_SUCCESS)
969 return FALSE;
970
971 return TRUE;
972 }
973
974 void
pktq_set_max_plen(struct pktq * pq,int prec,int max_pkts)975 pktq_set_max_plen(struct pktq *pq, int prec, int max_pkts)
976 {
977 ASSERT(prec >= 0 && prec < pq->num_prec);
978
979 /* protect shared resource */
980 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
981 return;
982
983 if (prec < pq->num_prec)
984 pq->q[prec].max_pkts = (uint16)max_pkts;
985
986 /* protect shared resource */
987 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
988 return;
989 }
990
991 void * BCMFASTPATH
pktq_deq(struct pktq * pq,int * prec_out)992 pktq_deq(struct pktq *pq, int *prec_out)
993 {
994 struct pktq_prec *q;
995 void *p = NULL;
996 int prec;
997
998 /* protect shared resource */
999 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1000 return NULL;
1001
1002 if (pq->n_pkts_tot == 0)
1003 goto done;
1004
1005 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
1006 pq->hi_prec--;
1007
1008 q = &pq->q[prec];
1009
1010 if ((p = q->head) == NULL)
1011 goto done;
1012
1013 if ((q->head = PKTLINK(p)) == NULL)
1014 q->tail = NULL;
1015
1016 q->n_pkts--;
1017
1018 pq->n_pkts_tot--;
1019
1020 #ifdef WL_TXQ_STALL
1021 q->dequeue_count++;
1022 #endif // endif
1023
1024 if (prec_out)
1025 *prec_out = prec;
1026
1027 PKTSETLINK(p, NULL);
1028
1029 done:
1030 /* protect shared resource */
1031 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1032 return NULL;
1033
1034 return p;
1035 }
1036
1037 void * BCMFASTPATH
pktq_deq_tail(struct pktq * pq,int * prec_out)1038 pktq_deq_tail(struct pktq *pq, int *prec_out)
1039 {
1040 struct pktq_prec *q;
1041 void *p = NULL, *prev;
1042 int prec;
1043
1044 /* protect shared resource */
1045 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1046 return NULL;
1047
1048 if (pq->n_pkts_tot == 0)
1049 goto done;
1050
1051 for (prec = 0; prec < pq->hi_prec; prec++)
1052 if (pq->q[prec].head)
1053 break;
1054
1055 q = &pq->q[prec];
1056
1057 if ((p = q->head) == NULL)
1058 goto done;
1059
1060 for (prev = NULL; p != q->tail; p = PKTLINK(p))
1061 prev = p;
1062
1063 if (prev)
1064 PKTSETLINK(prev, NULL);
1065 else
1066 q->head = NULL;
1067
1068 q->tail = prev;
1069 q->n_pkts--;
1070
1071 pq->n_pkts_tot--;
1072
1073 #ifdef WL_TXQ_STALL
1074 q->dequeue_count++;
1075 #endif // endif
1076
1077 if (prec_out)
1078 *prec_out = prec;
1079
1080 PKTSETLINK(p, NULL);
1081
1082 done:
1083 /* protect shared resource */
1084 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1085 return NULL;
1086
1087 return p;
1088 }
1089
1090 void *
pktq_peek(struct pktq * pq,int * prec_out)1091 pktq_peek(struct pktq *pq, int *prec_out)
1092 {
1093 int prec;
1094 void *p = NULL;
1095
1096 /* protect shared resource */
1097 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1098 return NULL;
1099
1100 if (pq->n_pkts_tot == 0)
1101 goto done;
1102
1103 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
1104 pq->hi_prec--;
1105
1106 if (prec_out)
1107 *prec_out = prec;
1108
1109 p = pq->q[prec].head;
1110
1111 done:
1112 /* protect shared resource */
1113 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1114 return NULL;
1115
1116 return p;
1117 }
1118
1119 void *
spktq_peek(struct spktq * spq)1120 spktq_peek(struct spktq *spq)
1121 {
1122 void *p = NULL;
1123
1124 /* protect shared resource */
1125 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1126 return NULL;
1127
1128 if (spq->q.n_pkts == 0)
1129 goto done;
1130
1131 p = spq->q.head;
1132
1133 done:
1134 /* protect shared resource */
1135 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
1136 return NULL;
1137
1138 return p;
1139 }
1140
1141 void
pktq_pflush(osl_t * osh,struct pktq * pq,int prec,bool dir)1142 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir)
1143 {
1144 void *p;
1145
1146 /* no need for a mutex protection! */
1147
1148 /* start with the head of the list */
1149 while ((p = pktq_pdeq(pq, prec)) != NULL) {
1150
1151 /* delete this packet */
1152 PKTFREE(osh, p, dir);
1153 }
1154 }
1155
1156 void
spktq_flush(osl_t * osh,struct spktq * spq,bool dir)1157 spktq_flush(osl_t *osh, struct spktq *spq, bool dir)
1158 {
1159 void *p;
1160
1161 /* no need for a mutex protection! */
1162
1163 /* start with the head of the list */
1164 while ((p = spktq_deq(spq)) != NULL) {
1165
1166 /* delete this packet */
1167 PKTFREE(osh, p, dir);
1168 }
1169 }
1170
1171 void
pktq_flush(osl_t * osh,struct pktq * pq,bool dir)1172 pktq_flush(osl_t *osh, struct pktq *pq, bool dir)
1173 {
1174 bool flush = FALSE;
1175
1176 /* protect shared resource */
1177 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1178 return;
1179
1180 /* Optimize flush, if pktq n_pkts_tot = 0, just return.
1181 * pktq len of 0 means pktq's prec q's are all empty.
1182 */
1183 if (pq->n_pkts_tot > 0) {
1184 flush = TRUE;
1185 }
1186
1187 /* protect shared resource */
1188 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1189 return;
1190
1191 if (flush) {
1192 int prec;
1193
1194 PKTQ_PREC_ITER(pq, prec) {
1195 pktq_pflush(osh, pq, prec, dir);
1196 }
1197 }
1198 }
1199
1200 /* Return sum of lengths of a specific set of precedences */
1201 int
pktq_mlen(struct pktq * pq,uint prec_bmp)1202 pktq_mlen(struct pktq *pq, uint prec_bmp)
1203 {
1204 int prec, len;
1205
1206 /* protect shared resource */
1207 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1208 return 0;
1209
1210 len = 0;
1211
1212 for (prec = 0; prec <= pq->hi_prec; prec++)
1213 if (prec_bmp & (1 << prec))
1214 len += pq->q[prec].n_pkts;
1215
1216 /* protect shared resource */
1217 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1218 return 0;
1219
1220 return len;
1221 }
1222
1223 /* Priority peek from a specific set of precedences */
1224 void * BCMFASTPATH
pktq_mpeek(struct pktq * pq,uint prec_bmp,int * prec_out)1225 pktq_mpeek(struct pktq *pq, uint prec_bmp, int *prec_out)
1226 {
1227 struct pktq_prec *q;
1228 void *p = NULL;
1229 int prec;
1230
1231 /* protect shared resource */
1232 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1233 return NULL;
1234
1235 if (pq->n_pkts_tot == 0)
1236 goto done;
1237
1238 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
1239 pq->hi_prec--;
1240
1241 while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
1242 if (prec-- == 0)
1243 goto done;
1244
1245 q = &pq->q[prec];
1246
1247 if ((p = q->head) == NULL)
1248 goto done;
1249
1250 if (prec_out)
1251 *prec_out = prec;
1252
1253 done:
1254 /* protect shared resource */
1255 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1256 return NULL;
1257
1258 return p;
1259 }
1260 /* Priority dequeue from a specific set of precedences */
1261 void * BCMFASTPATH
pktq_mdeq(struct pktq * pq,uint prec_bmp,int * prec_out)1262 pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
1263 {
1264 struct pktq_prec *q;
1265 void *p = NULL;
1266 int prec;
1267
1268 /* protect shared resource */
1269 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1270 return NULL;
1271
1272 if (pq->n_pkts_tot == 0)
1273 goto done;
1274
1275 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
1276 pq->hi_prec--;
1277
1278 while ((pq->q[prec].head == NULL) || ((prec_bmp & (1 << prec)) == 0))
1279 if (prec-- == 0)
1280 goto done;
1281
1282 q = &pq->q[prec];
1283
1284 if ((p = q->head) == NULL)
1285 goto done;
1286
1287 if ((q->head = PKTLINK(p)) == NULL)
1288 q->tail = NULL;
1289
1290 q->n_pkts--;
1291
1292 #ifdef WL_TXQ_STALL
1293 q->dequeue_count++;
1294 #endif // endif
1295
1296 if (prec_out)
1297 *prec_out = prec;
1298
1299 pq->n_pkts_tot--;
1300
1301 PKTSETLINK(p, NULL);
1302
1303 done:
1304 /* protect shared resource */
1305 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1306 return NULL;
1307
1308 return p;
1309 }
1310
1311 #ifdef HND_PKTQ_THREAD_SAFE
1312 int
pktqprec_avail_pkts(struct pktq * pq,int prec)1313 pktqprec_avail_pkts(struct pktq *pq, int prec)
1314 {
1315 int ret;
1316
1317 /* protect shared resource */
1318 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1319 return 0;
1320
1321 ASSERT(prec >= 0 && prec < pq->num_prec);
1322
1323 ret = pq->q[prec].max_pkts - pq->q[prec].n_pkts;
1324
1325 /* protect shared resource */
1326 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1327 return 0;
1328
1329 return ret;
1330 }
1331
1332 bool
pktqprec_full(struct pktq * pq,int prec)1333 pktqprec_full(struct pktq *pq, int prec)
1334 {
1335 bool ret;
1336
1337 /* protect shared resource */
1338 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1339 return FALSE;
1340
1341 ASSERT(prec >= 0 && prec < pq->num_prec);
1342
1343 ret = pq->q[prec].n_pkts >= pq->q[prec].max_pkts;
1344
1345 /* protect shared resource */
1346 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1347 return FALSE;
1348
1349 return ret;
1350 }
1351
1352 int
pktq_avail(struct pktq * pq)1353 pktq_avail(struct pktq *pq)
1354 {
1355 int ret;
1356
1357 /* protect shared resource */
1358 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1359 return 0;
1360
1361 ret = pq->max_pkts - pq->n_pkts_tot;
1362
1363 /* protect shared resource */
1364 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1365 return 0;
1366
1367 return ret;
1368 }
1369
1370 int
spktq_avail(struct spktq * spq)1371 spktq_avail(struct spktq *spq)
1372 {
1373 int ret;
1374
1375 /* protect shared resource */
1376 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1377 return 0;
1378
1379 ret = spq->q.max_pkts - spq->q.n_pkts;
1380
1381 /* protect shared resource */
1382 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
1383 return 0;
1384
1385 return ret;
1386 }
1387
1388 bool
pktq_full(struct pktq * pq)1389 pktq_full(struct pktq *pq)
1390 {
1391 bool ret;
1392
1393 /* protect shared resource */
1394 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1395 return FALSE;
1396
1397 ret = pq->n_pkts_tot >= pq->max_pkts;
1398
1399 /* protect shared resource */
1400 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
1401 return FALSE;
1402
1403 return ret;
1404 }
1405
1406 bool
spktq_full(struct spktq * spq)1407 spktq_full(struct spktq *spq)
1408 {
1409 bool ret;
1410
1411 /* protect shared resource */
1412 if (HND_PKTQ_MUTEX_ACQUIRE(&spq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
1413 return FALSE;
1414
1415 ret = spq->q.n_pkts >= spq->q.max_pkts;
1416
1417 /* protect shared resource */
1418 if (HND_PKTQ_MUTEX_RELEASE(&spq->mutex) != OSL_EXT_SUCCESS)
1419 return FALSE;
1420
1421 return ret;
1422 }
1423
1424 #endif /* HND_PKTQ_THREAD_SAFE */
1425