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