drivers/net/wireless/rockchip_wlan/rkwifi/bcmdhd/hnd_pktq.c

   1 /*
   2  * HND generic pktq operation primitives
   3  *
   4  * $Copyright Open Broadcom Corporation$
   5  *
   6  * $Id: $
   7  */
   8
   9 #include <typedefs.h>
  10 #include <osl.h>
  11 #include <bcmutils.h>
  12 #include <hnd_pktq.h>
  13
  14 /*
  15  * osl multiple-precedence packet queue
  16  * hi_prec is always >= the number of the highest non-empty precedence
  17  */
  18 void * BCMFASTPATH
  19 pktq_penq(struct pktq *pq, int prec, void *p)
  20 {
  21         struct pktq_prec *q;
  22
  23         ASSERT(prec >= 0 && prec < pq->num_prec);
  24         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
  25
  26         ASSERT(!pktq_full(pq));
  27         ASSERT(!pktq_pfull(pq, prec));
  28
  29         q = &pq->q[prec];
  30
  31         if (q->head)
  32                 PKTSETLINK(q->tail, p);
  33         else
  34                 q->head = p;
  35
  36         q->tail = p;
  37         q->len++;
  38
  39         pq->len++;
  40
  41         if (pq->hi_prec < prec)
  42                 pq->hi_prec = (uint8)prec;
  43
  44         return p;
  45 }
  46
  47 void * BCMFASTPATH
  48 pktq_penq_head(struct pktq *pq, int prec, void *p)
  49 {
  50         struct pktq_prec *q;
  51
  52         ASSERT(prec >= 0 && prec < pq->num_prec);
  53         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
  54
  55         ASSERT(!pktq_full(pq));
  56         ASSERT(!pktq_pfull(pq, prec));
  57
  58         q = &pq->q[prec];
  59
  60         if (q->head == NULL)
  61                 q->tail = p;
  62
  63         PKTSETLINK(p, q->head);
  64         q->head = p;
  65         q->len++;
  66
  67         pq->len++;
  68
  69         if (pq->hi_prec < prec)
  70                 pq->hi_prec = (uint8)prec;
  71
  72         return p;
  73 }
  74
  75 /*
  76  * Append spktq 'list' to the tail of pktq 'pq'
  77  */
  78 void BCMFASTPATH
  79 pktq_append(struct pktq *pq, int prec, struct spktq *list)
  80 {
  81         struct pktq_prec *q;
  82         struct pktq_prec *list_q;
  83
  84         list_q = &list->q[0];
  85
  86         /* empty list check */
  87         if (list_q->head == NULL)
  88                 return;
  89
  90         ASSERT(prec >= 0 && prec < pq->num_prec);
  91         ASSERT(PKTLINK(list_q->tail) == NULL);         /* terminated list */
  92
  93         ASSERT(!pktq_full(pq));
  94         ASSERT(!pktq_pfull(pq, prec));
  95
  96         q = &pq->q[prec];
  97
  98         if (q->head)
  99                 PKTSETLINK(q->tail, list_q->head);
 100         else
 101                 q->head = list_q->head;
 102
 103         q->tail = list_q->tail;
 104         q->len += list_q->len;
 105         pq->len += list_q->len;
 106
 107         if (pq->hi_prec < prec)
 108                 pq->hi_prec = (uint8)prec;
 109
 110         list_q->head = NULL;
 111         list_q->tail = NULL;
 112         list_q->len = 0;
 113         list->len = 0;
 114 }
 115
 116 /*
 117  * Prepend spktq 'list' to the head of pktq 'pq'
 118  */
 119 void BCMFASTPATH
 120 pktq_prepend(struct pktq *pq, int prec, struct spktq *list)
 121 {
 122         struct pktq_prec *q;
 123         struct pktq_prec *list_q;
 124
 125         list_q = &list->q[0];
 126
 127         /* empty list check */
 128         if (list_q->head == NULL)
 129                 return;
 130
 131         ASSERT(prec >= 0 && prec < pq->num_prec);
 132         ASSERT(PKTLINK(list_q->tail) == NULL);         /* terminated list */
 133
 134         ASSERT(!pktq_full(pq));
 135         ASSERT(!pktq_pfull(pq, prec));
 136
 137         q = &pq->q[prec];
 138
 139         /* set the tail packet of list to point at the former pq head */
 140         PKTSETLINK(list_q->tail, q->head);
 141         /* the new q head is the head of list */
 142         q->head = list_q->head;
 143
 144         /* If the q tail was non-null, then it stays as is.
 145          * If the q tail was null, it is now the tail of list
 146          */
 147         if (q->tail == NULL) {
 148                 q->tail = list_q->tail;
 149         }
 150
 151         q->len += list_q->len;
 152         pq->len += list_q->len;
 153
 154         if (pq->hi_prec < prec)
 155                 pq->hi_prec = (uint8)prec;
 156
 157         list_q->head = NULL;
 158         list_q->tail = NULL;
 159         list_q->len = 0;
 160         list->len = 0;
 161 }
 162
 163 void * BCMFASTPATH
 164 pktq_pdeq(struct pktq *pq, int prec)
 165 {
 166         struct pktq_prec *q;
 167         void *p;
 168
 169         ASSERT(prec >= 0 && prec < pq->num_prec);
 170
 171         q = &pq->q[prec];
 172
 173         if ((p = q->head) == NULL)
 174                 return NULL;
 175
 176         if ((q->head = PKTLINK(p)) == NULL)
 177                 q->tail = NULL;
 178
 179         q->len--;
 180
 181         pq->len--;
 182
 183         PKTSETLINK(p, NULL);
 184
 185         return p;
 186 }
 187
 188 void * BCMFASTPATH
 189 pktq_pdeq_prev(struct pktq *pq, int prec, void *prev_p)
 190 {
 191         struct pktq_prec *q;
 192         void *p;
 193
 194         ASSERT(prec >= 0 && prec < pq->num_prec);
 195
 196         q = &pq->q[prec];
 197
 198         if (prev_p == NULL)
 199                 return NULL;
 200
 201         if ((p = PKTLINK(prev_p)) == NULL)
 202                 return NULL;
 203
 204         q->len--;
 205
 206         pq->len--;
 207
 208         PKTSETLINK(prev_p, PKTLINK(p));
 209         PKTSETLINK(p, NULL);
 210
 211         return p;
 212 }
 213
 214 void * BCMFASTPATH
 215 pktq_pdeq_with_fn(struct pktq *pq, int prec, ifpkt_cb_t fn, int arg)
 216 {
 217         struct pktq_prec *q;
 218         void *p, *prev = NULL;
 219
 220         ASSERT(prec >= 0 && prec < pq->num_prec);
 221
 222         q = &pq->q[prec];
 223         p = q->head;
 224
 225         while (p) {
 226                 if (fn == NULL || (*fn)(p, arg)) {
 227                         break;
 228                 } else {
 229                         prev = p;
 230                         p = PKTLINK(p);
 231                 }
 232         }
 233         if (p == NULL)
 234                 return NULL;
 235
 236         if (prev == NULL) {
 237                 if ((q->head = PKTLINK(p)) == NULL) {
 238                         q->tail = NULL;
 239                 }
 240         } else {
 241                 PKTSETLINK(prev, PKTLINK(p));
 242                 if (q->tail == p) {
 243                         q->tail = prev;
 244                 }
 245         }
 246
 247         q->len--;
 248
 249         pq->len--;
 250
 251         PKTSETLINK(p, NULL);
 252
 253         return p;
 254 }
 255
 256 void * BCMFASTPATH
 257 pktq_pdeq_tail(struct pktq *pq, int prec)
 258 {
 259         struct pktq_prec *q;
 260         void *p, *prev;
 261
 262         ASSERT(prec >= 0 && prec < pq->num_prec);
 263
 264         q = &pq->q[prec];
 265
 266         if ((p = q->head) == NULL)
 267                 return NULL;
 268
 269         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 270                 prev = p;
 271
 272         if (prev)
 273                 PKTSETLINK(prev, NULL);
 274         else
 275                 q->head = NULL;
 276
 277         q->tail = prev;
 278         q->len--;
 279
 280         pq->len--;
 281
 282         return p;
 283 }
 284
 285 void
 286 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir, ifpkt_cb_t fn, int arg)
 287 {
 288         struct pktq_prec *q;
 289         void *p, *prev = NULL;
 290
 291         q = &pq->q[prec];
 292         p = q->head;
 293         while (p) {
 294                 if (fn == NULL || (*fn)(p, arg)) {
 295                         bool head = (p == q->head);
 296                         if (head)
 297                                 q->head = PKTLINK(p);
 298                         else
 299                                 PKTSETLINK(prev, PKTLINK(p));
 300                         PKTSETLINK(p, NULL);
 301                         PKTFREE(osh, p, dir);
 302                         q->len--;
 303                         pq->len--;
 304                         p = (head ? q->head : PKTLINK(prev));
 305                 } else {
 306                         prev = p;
 307                         p = PKTLINK(p);
 308                 }
 309         }
 310
 311         if (q->head == NULL) {
 312                 ASSERT(q->len == 0);
 313                 q->tail = NULL;
 314         }
 315 }
 316
 317 bool BCMFASTPATH
 318 pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
 319 {
 320         struct pktq_prec *q;
 321         void *p;
 322
 323         ASSERT(prec >= 0 && prec < pq->num_prec);
 324
 325         /* Should this just assert pktbuf? */
 326         if (!pktbuf)
 327                 return FALSE;
 328
 329         q = &pq->q[prec];
 330
 331         if (q->head == pktbuf) {
 332                 if ((q->head = PKTLINK(pktbuf)) == NULL)
 333                         q->tail = NULL;
 334         } else {
 335                 for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
 336                         ;
 337                 if (p == NULL)
 338                         return FALSE;
 339
 340                 PKTSETLINK(p, PKTLINK(pktbuf));
 341                 if (q->tail == pktbuf)
 342                         q->tail = p;
 343         }
 344
 345         q->len--;
 346         pq->len--;
 347         PKTSETLINK(pktbuf, NULL);
 348         return TRUE;
 349 }
 350
 351 void
 352 pktq_init(struct pktq *pq, int num_prec, int max_len)
 353 {
 354         int prec;
 355
 356         ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
 357
 358         /* pq is variable size; only zero out what's requested */
 359         bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 360
 361         pq->num_prec = (uint16)num_prec;
 362
 363         pq->max = (uint16)max_len;
 364
 365         for (prec = 0; prec < num_prec; prec++)
 366                 pq->q[prec].max = pq->max;
 367 }
 368
 369 void
 370 pktq_set_max_plen(struct pktq *pq, int prec, int max_len)
 371 {
 372         ASSERT(prec >= 0 && prec < pq->num_prec);
 373
 374         if (prec < pq->num_prec)
 375                 pq->q[prec].max = (uint16)max_len;
 376 }
 377
 378 void * BCMFASTPATH
 379 pktq_deq(struct pktq *pq, int *prec_out)
 380 {
 381         struct pktq_prec *q;
 382         void *p;
 383         int prec;
 384
 385         if (pq->len == 0)
 386                 return NULL;
 387
 388         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 389                 pq->hi_prec--;
 390
 391         q = &pq->q[prec];
 392
 393         if ((p = q->head) == NULL)
 394                 return NULL;
 395
 396         if ((q->head = PKTLINK(p)) == NULL)
 397                 q->tail = NULL;
 398
 399         q->len--;
 400
 401         pq->len--;
 402
 403         if (prec_out)
 404                 *prec_out = prec;
 405
 406         PKTSETLINK(p, NULL);
 407
 408         return p;
 409 }
 410
 411 void * BCMFASTPATH
 412 pktq_deq_tail(struct pktq *pq, int *prec_out)
 413 {
 414         struct pktq_prec *q;
 415         void *p, *prev;
 416         int prec;
 417
 418         if (pq->len == 0)
 419                 return NULL;
 420
 421         for (prec = 0; prec < pq->hi_prec; prec++)
 422                 if (pq->q[prec].head)
 423                         break;
 424
 425         q = &pq->q[prec];
 426
 427         if ((p = q->head) == NULL)
 428                 return NULL;
 429
 430         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 431                 prev = p;
 432
 433         if (prev)
 434                 PKTSETLINK(prev, NULL);
 435         else
 436                 q->head = NULL;
 437
 438         q->tail = prev;
 439         q->len--;
 440
 441         pq->len--;
 442
 443         if (prec_out)
 444                 *prec_out = prec;
 445
 446         PKTSETLINK(p, NULL);
 447
 448         return p;
 449 }
 450
 451 void *
 452 pktq_peek(struct pktq *pq, int *prec_out)
 453 {
 454         int prec;
 455
 456         if (pq->len == 0)
 457                 return NULL;
 458
 459         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 460                 pq->hi_prec--;
 461
 462         if (prec_out)
 463                 *prec_out = prec;
 464
 465         return (pq->q[prec].head);
 466 }
 467
 468 void *
 469 pktq_peek_tail(struct pktq *pq, int *prec_out)
 470 {
 471         int prec;
 472
 473         if (pq->len == 0)
 474                 return NULL;
 475
 476         for (prec = 0; prec < pq->hi_prec; prec++)
 477                 if (pq->q[prec].head)
 478                         break;
 479
 480         if (prec_out)
 481                 *prec_out = prec;
 482
 483         return (pq->q[prec].tail);
 484 }
 485
 486 void
 487 pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg)
 488 {
 489         int prec;
 490
 491         /* Optimize flush, if pktq len = 0, just return.
 492          * pktq len of 0 means pktq's prec q's are all empty.
 493          */
 494         if (pq->len == 0) {
 495                 return;
 496         }
 497
 498         for (prec = 0; prec < pq->num_prec; prec++)
 499                 pktq_pflush(osh, pq, prec, dir, fn, arg);
 500         if (fn == NULL)
 501                 ASSERT(pq->len == 0);
 502 }
 503
 504 /* Return sum of lengths of a specific set of precedences */
 505 int
 506 pktq_mlen(struct pktq *pq, uint prec_bmp)
 507 {
 508         int prec, len;
 509
 510         len = 0;
 511
 512         for (prec = 0; prec <= pq->hi_prec; prec++)
 513                 if (prec_bmp & (1 << prec))
 514                         len += pq->q[prec].len;
 515
 516         return len;
 517 }
 518
 519 /* Priority peek from a specific set of precedences */
 520 void * BCMFASTPATH
 521 pktq_mpeek(struct pktq *pq, uint prec_bmp, int *prec_out)
 522 {
 523         struct pktq_prec *q;
 524         void *p;
 525         int prec;
 526
 527         if (pq->len == 0)
 528         {
 529                 return NULL;
 530         }
 531         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 532                 pq->hi_prec--;
 533
 534         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 535                 if (prec-- == 0)
 536                         return NULL;
 537
 538         q = &pq->q[prec];
 539
 540         if ((p = q->head) == NULL)
 541                 return NULL;
 542
 543         if (prec_out)
 544                 *prec_out = prec;
 545
 546         return p;
 547 }
 548 /* Priority dequeue from a specific set of precedences */
 549 void * BCMFASTPATH
 550 pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
 551 {
 552         struct pktq_prec *q;
 553         void *p;
 554         int prec;
 555
 556         if (pq->len == 0)
 557                 return NULL;
 558
 559         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 560                 pq->hi_prec--;
 561
 562         while ((pq->q[prec].head == NULL) || ((prec_bmp & (1 << prec)) == 0))
 563                 if (prec-- == 0)
 564                         return NULL;
 565
 566         q = &pq->q[prec];
 567
 568         if ((p = q->head) == NULL)
 569                 return NULL;
 570
 571         if ((q->head = PKTLINK(p)) == NULL)
 572                 q->tail = NULL;
 573
 574         q->len--;
 575
 576         if (prec_out)
 577                 *prec_out = prec;
 578
 579         pq->len--;
 580
 581         PKTSETLINK(p, NULL);
 582
 583         return p;
 584 }