1 /*
2 * HND generic pktq operation primitives
3 *
4 * Copyright (C) 1999-2017, Broadcom Corporation
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 644628 2016-06-21 06:25:58Z $
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 /* */
48
49 /*
50 * osl multiple-precedence packet queue
51 * hi_prec is always >= the number of the highest non-empty precedence
52 */
53 void * BCMFASTPATH
pktq_penq(struct pktq * pq,int prec,void * p)54 pktq_penq(struct pktq *pq, int prec, void *p)
55 {
56 struct pktq_prec *q;
57
58 /* protect shared resource */
59 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
60 return NULL;
61
62 ASSERT(prec >= 0 && prec < pq->num_prec);
63 ASSERT(PKTLINK(p) == NULL); /* queueing chains not allowed */
64
65 ASSERT(!pktq_full(pq));
66 ASSERT(!pktq_pfull(pq, prec));
67
68 q = &pq->q[prec];
69
70 if (q->head)
71 PKTSETLINK(q->tail, p);
72 else
73 q->head = p;
74
75 q->tail = p;
76 q->len++;
77
78 pq->len++;
79
80 if (pq->hi_prec < prec)
81 pq->hi_prec = (uint8)prec;
82
83 /* protect shared resource */
84 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
85 return NULL;
86
87 return p;
88 }
89
90 void * BCMFASTPATH
pktq_penq_head(struct pktq * pq,int prec,void * p)91 pktq_penq_head(struct pktq *pq, int prec, void *p)
92 {
93 struct pktq_prec *q;
94
95 /* protect shared resource */
96 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
97 return NULL;
98
99 ASSERT(prec >= 0 && prec < pq->num_prec);
100 ASSERT(PKTLINK(p) == NULL); /* queueing chains not allowed */
101
102 ASSERT(!pktq_full(pq));
103 ASSERT(!pktq_pfull(pq, prec));
104
105 q = &pq->q[prec];
106
107 if (q->head == NULL)
108 q->tail = p;
109
110 PKTSETLINK(p, q->head);
111 q->head = p;
112 q->len++;
113
114 pq->len++;
115
116 if (pq->hi_prec < prec)
117 pq->hi_prec = (uint8)prec;
118
119 /* protect shared resource */
120 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
121 return NULL;
122
123 return p;
124 }
125
126 /*
127 * Append spktq 'list' to the tail of pktq 'pq'
128 */
129 void BCMFASTPATH
pktq_append(struct pktq * pq,int prec,struct spktq * list)130 pktq_append(struct pktq *pq, int prec, struct spktq *list)
131 {
132 struct pktq_prec *q;
133 struct pktq_prec *list_q;
134
135 /* protect shared resource */
136 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
137 return;
138
139 list_q = &list->q[0];
140
141 /* empty list check */
142 if (list_q->head == NULL)
143 goto done;
144
145 ASSERT(prec >= 0 && prec < pq->num_prec);
146 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
147
148 ASSERT(!pktq_full(pq));
149 ASSERT(!pktq_pfull(pq, prec));
150
151 q = &pq->q[prec];
152
153 if (q->head)
154 PKTSETLINK(q->tail, list_q->head);
155 else
156 q->head = list_q->head;
157
158 q->tail = list_q->tail;
159 q->len += list_q->len;
160 pq->len += list_q->len;
161
162 if (pq->hi_prec < prec)
163 pq->hi_prec = (uint8)prec;
164
165 list_q->head = NULL;
166 list_q->tail = NULL;
167 list_q->len = 0;
168 list->len = 0;
169
170 done:
171 /* protect shared resource */
172 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
173 return;
174 }
175
176 /*
177 * Prepend spktq 'list' to the head of pktq 'pq'
178 */
179 void BCMFASTPATH
pktq_prepend(struct pktq * pq,int prec,struct spktq * list)180 pktq_prepend(struct pktq *pq, int prec, struct spktq *list)
181 {
182 struct pktq_prec *q;
183 struct pktq_prec *list_q;
184
185 /* protect shared resource */
186 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
187 return;
188
189 list_q = &list->q[0];
190
191 /* empty list check */
192 if (list_q->head == NULL)
193 goto done;
194
195 ASSERT(prec >= 0 && prec < pq->num_prec);
196 ASSERT(PKTLINK(list_q->tail) == NULL); /* terminated list */
197
198 ASSERT(!pktq_full(pq));
199 ASSERT(!pktq_pfull(pq, prec));
200
201 q = &pq->q[prec];
202
203 /* set the tail packet of list to point at the former pq head */
204 PKTSETLINK(list_q->tail, q->head);
205 /* the new q head is the head of list */
206 q->head = list_q->head;
207
208 /* If the q tail was non-null, then it stays as is.
209 * If the q tail was null, it is now the tail of list
210 */
211 if (q->tail == NULL) {
212 q->tail = list_q->tail;
213 }
214
215 q->len += list_q->len;
216 pq->len += list_q->len;
217
218 if (pq->hi_prec < prec)
219 pq->hi_prec = (uint8)prec;
220
221 list_q->head = NULL;
222 list_q->tail = NULL;
223 list_q->len = 0;
224 list->len = 0;
225
226 done:
227 /* protect shared resource */
228 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
229 return;
230 }
231
232 void * BCMFASTPATH
pktq_pdeq(struct pktq * pq,int prec)233 pktq_pdeq(struct pktq *pq, int prec)
234 {
235 struct pktq_prec *q;
236 void *p;
237
238 /* protect shared resource */
239 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
240 return NULL;
241
242 ASSERT(prec >= 0 && prec < pq->num_prec);
243
244 q = &pq->q[prec];
245
246 if ((p = q->head) == NULL)
247 goto done;
248
249 if ((q->head = PKTLINK(p)) == NULL)
250 q->tail = NULL;
251
252 q->len--;
253
254 pq->len--;
255
256 PKTSETLINK(p, NULL);
257
258 done:
259 /* protect shared resource */
260 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
261 return NULL;
262
263 return p;
264 }
265
266 void * BCMFASTPATH
pktq_pdeq_prev(struct pktq * pq,int prec,void * prev_p)267 pktq_pdeq_prev(struct pktq *pq, int prec, void *prev_p)
268 {
269 struct pktq_prec *q;
270 void *p = NULL;
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 (prev_p == NULL)
281 goto done;
282
283 if ((p = PKTLINK(prev_p)) == NULL)
284 goto done;
285
286 q->len--;
287
288 pq->len--;
289
290 PKTSETLINK(prev_p, PKTLINK(p));
291 PKTSETLINK(p, NULL);
292
293 done:
294 /* protect shared resource */
295 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
296 return NULL;
297
298 return p;
299 }
300
301 void * BCMFASTPATH
pktq_pdeq_with_fn(struct pktq * pq,int prec,ifpkt_cb_t fn,int arg)302 pktq_pdeq_with_fn(struct pktq *pq, int prec, ifpkt_cb_t fn, int arg)
303 {
304 struct pktq_prec *q;
305 void *p, *prev = NULL;
306
307 /* protect shared resource */
308 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
309 return NULL;
310
311 ASSERT(prec >= 0 && prec < pq->num_prec);
312
313 q = &pq->q[prec];
314 p = q->head;
315
316 while (p) {
317 if (fn == NULL || (*fn)(p, arg)) {
318 break;
319 } else {
320 prev = p;
321 p = PKTLINK(p);
322 }
323 }
324 if (p == NULL)
325 goto done;
326
327 if (prev == NULL) {
328 if ((q->head = PKTLINK(p)) == NULL) {
329 q->tail = NULL;
330 }
331 } else {
332 PKTSETLINK(prev, PKTLINK(p));
333 if (q->tail == p) {
334 q->tail = prev;
335 }
336 }
337
338 q->len--;
339
340 pq->len--;
341
342 PKTSETLINK(p, NULL);
343
344 done:
345 /* protect shared resource */
346 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
347 return NULL;
348
349 return p;
350 }
351
352 void * BCMFASTPATH
pktq_pdeq_tail(struct pktq * pq,int prec)353 pktq_pdeq_tail(struct pktq *pq, int prec)
354 {
355 struct pktq_prec *q;
356 void *p, *prev;
357
358 /* protect shared resource */
359 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
360 return NULL;
361
362 ASSERT(prec >= 0 && prec < pq->num_prec);
363
364 q = &pq->q[prec];
365
366 if ((p = q->head) == NULL)
367 goto done;
368
369 for (prev = NULL; p != q->tail; p = PKTLINK(p))
370 prev = p;
371
372 if (prev)
373 PKTSETLINK(prev, NULL);
374 else
375 q->head = NULL;
376
377 q->tail = prev;
378 q->len--;
379
380 pq->len--;
381
382 done:
383 /* protect shared resource */
384 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
385 return NULL;
386
387 return p;
388 }
389
390 bool BCMFASTPATH
pktq_pdel(struct pktq * pq,void * pktbuf,int prec)391 pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
392 {
393 bool ret = FALSE;
394 struct pktq_prec *q;
395 void *p = NULL;
396
397 /* protect shared resource */
398 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
399 return FALSE;
400
401 ASSERT(prec >= 0 && prec < pq->num_prec);
402
403 /* Should this just assert pktbuf? */
404 if (!pktbuf)
405 goto done;
406
407 q = &pq->q[prec];
408
409 if (q->head == pktbuf) {
410 if ((q->head = PKTLINK(pktbuf)) == NULL)
411 q->tail = NULL;
412 } else {
413 for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
414 ;
415 if (p == NULL)
416 goto done;
417
418 PKTSETLINK(p, PKTLINK(pktbuf));
419 if (q->tail == pktbuf)
420 q->tail = p;
421 }
422
423 q->len--;
424 pq->len--;
425 PKTSETLINK(pktbuf, NULL);
426 ret = TRUE;
427
428 done:
429 /* protect shared resource */
430 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
431 return FALSE;
432
433 return ret;
434 }
435
436 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)437 _pktq_pfilter(struct pktq *pq, int prec, pktq_filter_t fltr, void* fltr_ctx,
438 defer_free_pkt_fn_t defer, void *defer_ctx)
439 {
440 struct pktq_prec wq;
441 struct pktq_prec *q;
442 void *p;
443
444 /* protect shared resource */
445 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
446 return;
447
448 /* move the prec queue aside to a work queue */
449 q = &pq->q[prec];
450
451 wq = *q;
452
453 q->head = NULL;
454 q->tail = NULL;
455 q->len = 0;
456
457 pq->len -= wq.len;
458
459 /* protect shared resource */
460 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
461 return;
462
463 /* start with the head of the work queue */
464 while ((p = wq.head) != NULL) {
465 /* unlink the current packet from the list */
466 wq.head = PKTLINK(p);
467 PKTSETLINK(p, NULL);
468 wq.len--;
469
470 /* call the filter function on current packet */
471 ASSERT(fltr != NULL);
472 switch ((*fltr)(fltr_ctx, p)) {
473 case PKT_FILTER_NOACTION:
474 /* put this packet back */
475 pktq_penq(pq, prec, p);
476 break;
477
478 case PKT_FILTER_DELETE:
479 /* delete this packet */
480 ASSERT(defer != NULL);
481 (*defer)(defer_ctx, p);
482 break;
483
484 case PKT_FILTER_REMOVE:
485 /* pkt already removed from list */
486 break;
487
488 default:
489 ASSERT(0);
490 break;
491 }
492 }
493
494 ASSERT(wq.len == 0);
495 }
496
497 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)498 pktq_pfilter(struct pktq *pq, int prec, pktq_filter_t fltr, void* fltr_ctx,
499 defer_free_pkt_fn_t defer, void *defer_ctx, flush_free_pkt_fn_t flush, void *flush_ctx)
500 {
501 _pktq_pfilter(pq, prec, fltr, fltr_ctx, defer, defer_ctx);
502
503 ASSERT(flush != NULL);
504 (*flush)(flush_ctx);
505 }
506
507 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)508 pktq_filter(struct pktq *pq, pktq_filter_t fltr, void* fltr_ctx,
509 defer_free_pkt_fn_t defer, void *defer_ctx, flush_free_pkt_fn_t flush, void *flush_ctx)
510 {
511 bool filter = FALSE;
512
513 /* protect shared resource */
514 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
515 return;
516
517 /* Optimize if pktq len = 0, just return.
518 * pktq len of 0 means pktq's prec q's are all empty.
519 */
520 if (pq->len > 0) {
521 filter = TRUE;
522 }
523
524 /* protect shared resource */
525 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
526 return;
527
528 if (filter) {
529 int prec;
530
531 PKTQ_PREC_ITER(pq, prec) {
532 _pktq_pfilter(pq, prec, fltr, fltr_ctx, defer, defer_ctx);
533 }
534
535 ASSERT(flush != NULL);
536 (*flush)(flush_ctx);
537 }
538 }
539
540 bool
pktq_init(struct pktq * pq,int num_prec,int max_len)541 pktq_init(struct pktq *pq, int num_prec, int max_len)
542 {
543 int prec;
544
545 if (HND_PKTQ_MUTEX_CREATE("pktq", &pq->mutex) != OSL_EXT_SUCCESS)
546 return FALSE;
547
548 ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
549
550 /* pq is variable size; only zero out what's requested */
551 bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
552
553 pq->num_prec = (uint16)num_prec;
554
555 pq->max = (uint16)max_len;
556
557 for (prec = 0; prec < num_prec; prec++)
558 pq->q[prec].max = pq->max;
559
560 return TRUE;
561 }
562
563 bool
pktq_deinit(struct pktq * pq)564 pktq_deinit(struct pktq *pq)
565 {
566 BCM_REFERENCE(pq);
567 if (HND_PKTQ_MUTEX_DELETE(&pq->mutex) != OSL_EXT_SUCCESS)
568 return FALSE;
569
570 return TRUE;
571 }
572
573 void
pktq_set_max_plen(struct pktq * pq,int prec,int max_len)574 pktq_set_max_plen(struct pktq *pq, int prec, int max_len)
575 {
576 ASSERT(prec >= 0 && prec < pq->num_prec);
577
578 /* protect shared resource */
579 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
580 return;
581
582 if (prec < pq->num_prec)
583 pq->q[prec].max = (uint16)max_len;
584
585 /* protect shared resource */
586 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
587 return;
588 }
589
590 void * BCMFASTPATH
pktq_deq(struct pktq * pq,int * prec_out)591 pktq_deq(struct pktq *pq, int *prec_out)
592 {
593 struct pktq_prec *q;
594 void *p = NULL;
595 int prec;
596
597 /* protect shared resource */
598 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
599 return NULL;
600
601 if (pq->len == 0)
602 goto done;
603
604 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
605 pq->hi_prec--;
606
607 q = &pq->q[prec];
608
609 if ((p = q->head) == NULL)
610 goto done;
611
612 if ((q->head = PKTLINK(p)) == NULL)
613 q->tail = NULL;
614
615 q->len--;
616
617 pq->len--;
618
619 if (prec_out)
620 *prec_out = prec;
621
622 PKTSETLINK(p, NULL);
623
624 done:
625 /* protect shared resource */
626 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
627 return NULL;
628
629 return p;
630 }
631
632 void * BCMFASTPATH
pktq_deq_tail(struct pktq * pq,int * prec_out)633 pktq_deq_tail(struct pktq *pq, int *prec_out)
634 {
635 struct pktq_prec *q;
636 void *p = NULL, *prev;
637 int prec;
638
639 /* protect shared resource */
640 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
641 return NULL;
642
643 if (pq->len == 0)
644 goto done;
645
646 for (prec = 0; prec < pq->hi_prec; prec++)
647 if (pq->q[prec].head)
648 break;
649
650 q = &pq->q[prec];
651
652 if ((p = q->head) == NULL)
653 goto done;
654
655 for (prev = NULL; p != q->tail; p = PKTLINK(p))
656 prev = p;
657
658 if (prev)
659 PKTSETLINK(prev, NULL);
660 else
661 q->head = NULL;
662
663 q->tail = prev;
664 q->len--;
665
666 pq->len--;
667
668 if (prec_out)
669 *prec_out = prec;
670
671 PKTSETLINK(p, NULL);
672
673 done:
674 /* protect shared resource */
675 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
676 return NULL;
677
678 return p;
679 }
680
681 void *
pktq_peek(struct pktq * pq,int * prec_out)682 pktq_peek(struct pktq *pq, int *prec_out)
683 {
684 int prec;
685 void *p = NULL;
686
687 /* protect shared resource */
688 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
689 return NULL;
690
691 if (pq->len == 0)
692 goto done;
693
694 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
695 pq->hi_prec--;
696
697 if (prec_out)
698 *prec_out = prec;
699
700 p = pq->q[prec].head;
701
702 done:
703 /* protect shared resource */
704 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
705 return NULL;
706
707 return p;
708 }
709
710 void *
pktq_peek_tail(struct pktq * pq,int * prec_out)711 pktq_peek_tail(struct pktq *pq, int *prec_out)
712 {
713 int prec;
714 void *p = NULL;
715
716 /* protect shared resource */
717 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
718 return NULL;
719
720 if (pq->len == 0)
721 goto done;
722
723 for (prec = 0; prec < pq->hi_prec; prec++)
724 if (pq->q[prec].head)
725 break;
726
727 if (prec_out)
728 *prec_out = prec;
729
730 p = pq->q[prec].tail;
731
732 done:
733 /* protect shared resource */
734 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
735 return NULL;
736
737 return p;
738 }
739
740 void
pktq_pflush(osl_t * osh,struct pktq * pq,int prec,bool dir)741 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir)
742 {
743 void *p;
744
745 /* no need for a mutex protection! */
746
747 /* start with the head of the list */
748 while ((p = pktq_pdeq(pq, prec)) != NULL) {
749 /* delete this packet */
750 PKTFREE(osh, p, dir);
751 }
752 }
753
754 void
pktq_flush(osl_t * osh,struct pktq * pq,bool dir)755 pktq_flush(osl_t *osh, struct pktq *pq, bool dir)
756 {
757 bool flush = FALSE;
758
759 /* protect shared resource */
760 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
761 return;
762
763 /* Optimize flush, if pktq len = 0, just return.
764 * pktq len of 0 means pktq's prec q's are all empty.
765 */
766 if (pq->len > 0) {
767 flush = TRUE;
768 }
769
770 /* protect shared resource */
771 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
772 return;
773
774 if (flush) {
775 int prec;
776
777 PKTQ_PREC_ITER(pq, prec) {
778 pktq_pflush(osh, pq, prec, dir);
779 }
780 }
781 }
782
783 /* Return sum of lengths of a specific set of precedences */
784 int
pktq_mlen(struct pktq * pq,uint prec_bmp)785 pktq_mlen(struct pktq *pq, uint prec_bmp)
786 {
787 int prec, len;
788
789 /* protect shared resource */
790 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
791 return 0;
792
793 len = 0;
794
795 for (prec = 0; prec <= pq->hi_prec; prec++)
796 if (prec_bmp & (1 << prec))
797 len += pq->q[prec].len;
798
799 /* protect shared resource */
800 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
801 return 0;
802
803 return len;
804 }
805
806 /* Priority peek from a specific set of precedences */
807 void * BCMFASTPATH
pktq_mpeek(struct pktq * pq,uint prec_bmp,int * prec_out)808 pktq_mpeek(struct pktq *pq, uint prec_bmp, int *prec_out)
809 {
810 struct pktq_prec *q;
811 void *p = NULL;
812 int prec;
813
814 /* protect shared resource */
815 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
816 return NULL;
817
818 if (pq->len == 0)
819 goto done;
820
821 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
822 pq->hi_prec--;
823
824 while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
825 if (prec-- == 0)
826 goto done;
827
828 q = &pq->q[prec];
829
830 if ((p = q->head) == NULL)
831 goto done;
832
833 if (prec_out)
834 *prec_out = prec;
835
836 done:
837 /* protect shared resource */
838 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
839 return NULL;
840
841 return p;
842 }
843 /* Priority dequeue from a specific set of precedences */
844 void * BCMFASTPATH
pktq_mdeq(struct pktq * pq,uint prec_bmp,int * prec_out)845 pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
846 {
847 struct pktq_prec *q;
848 void *p = NULL;
849 int prec;
850
851 /* protect shared resource */
852 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
853 return NULL;
854
855 if (pq->len == 0)
856 goto done;
857
858 while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
859 pq->hi_prec--;
860
861 while ((pq->q[prec].head == NULL) || ((prec_bmp & (1 << prec)) == 0))
862 if (prec-- == 0)
863 goto done;
864
865 q = &pq->q[prec];
866
867 if ((p = q->head) == NULL)
868 goto done;
869
870 if ((q->head = PKTLINK(p)) == NULL)
871 q->tail = NULL;
872
873 q->len--;
874
875 // terence 20150308: fix for non-null pointer of skb->prev sent from ndo_start_xmit
876 if (q->len == 0) {
877 q->head = NULL;
878 q->tail = NULL;
879 }
880
881 if (prec_out)
882 *prec_out = prec;
883
884 pq->len--;
885
886 PKTSETLINK(p, NULL);
887
888 done:
889 /* protect shared resource */
890 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
891 return NULL;
892
893 return p;
894 }
895
896 #ifdef HND_PKTQ_THREAD_SAFE
897 int
pktq_pavail(struct pktq * pq,int prec)898 pktq_pavail(struct pktq *pq, int prec)
899 {
900 int ret;
901
902 /* protect shared resource */
903 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
904 return 0;
905
906 ASSERT(prec >= 0 && prec < pq->num_prec);
907
908 ret = pq->q[prec].max - pq->q[prec].len;
909
910 /* protect shared resource */
911 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
912 return 0;
913
914 return ret;
915 }
916
917 bool
pktq_pfull(struct pktq * pq,int prec)918 pktq_pfull(struct pktq *pq, int prec)
919 {
920 bool ret;
921
922 /* protect shared resource */
923 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
924 return FALSE;
925
926 ASSERT(prec >= 0 && prec < pq->num_prec);
927
928 ret = pq->q[prec].len >= pq->q[prec].max;
929
930 /* protect shared resource */
931 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
932 return FALSE;
933
934 return ret;
935 }
936
937 int
pktq_avail(struct pktq * pq)938 pktq_avail(struct pktq *pq)
939 {
940 int ret;
941
942 /* protect shared resource */
943 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
944 return 0;
945
946 ret = pq->max - pq->len;
947
948 /* protect shared resource */
949 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
950 return 0;
951
952 return ret;
953 }
954
955 bool
pktq_full(struct pktq * pq)956 pktq_full(struct pktq *pq)
957 {
958 bool ret;
959
960 /* protect shared resource */
961 if (HND_PKTQ_MUTEX_ACQUIRE(&pq->mutex, OSL_EXT_TIME_FOREVER) != OSL_EXT_SUCCESS)
962 return FALSE;
963
964 ret = pq->len >= pq->max;
965
966 /* protect shared resource */
967 if (HND_PKTQ_MUTEX_RELEASE(&pq->mutex) != OSL_EXT_SUCCESS)
968 return FALSE;
969
970 return ret;
971 }
972 #endif /* HND_PKTQ_THREAD_SAFE */
973