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