drivers/net/wireless/bcmdhd/bcmutils.c

   1 /*
   2  * Driver O/S-independent utility routines
   3  *
   4  * Copyright (C) 1999-2015, 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  * $Id: bcmutils.c 551820 2015-04-24 08:43:23Z $
  24  */
  25
  26 #include <bcm_cfg.h>
  27 #include <typedefs.h>
  28 #include <bcmdefs.h>
  29 #include <stdarg.h>
  30 #ifdef BCMDRIVER
  31
  32 #include <osl.h>
  33 #include <bcmutils.h>
  34
  35 #else /* !BCMDRIVER */
  36
  37 #include <stdio.h>
  38 #include <string.h>
  39 #include <bcmutils.h>
  40
  41 #if defined(BCMEXTSUP)
  42 #include <bcm_osl.h>
  43 #endif
  44
  45 #ifndef ASSERT
  46 #define ASSERT(exp)
  47 #endif
  48
  49 #endif /* !BCMDRIVER */
  50
  51 #include <bcmendian.h>
  52 #include <bcmdevs.h>
  53 #include <proto/ethernet.h>
  54 #include <proto/vlan.h>
  55 #include <proto/bcmip.h>
  56 #include <proto/802.1d.h>
  57 #include <proto/802.11.h>
  58
  59
  60 void *_bcmutils_dummy_fn = NULL;
  61
  62
  63
  64 #ifdef BCMDRIVER
  65
  66
  67
  68 /* copy a pkt buffer chain into a buffer */
  69 uint
  70 pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf)
  71 {
  72         uint n, ret = 0;
  73
  74         if (len < 0)
  75                 len = 4096;     /* "infinite" */
  76
  77         /* skip 'offset' bytes */
  78         for (; p && offset; p = PKTNEXT(osh, p)) {
  79                 if (offset < (uint)PKTLEN(osh, p))
  80                         break;
  81                 offset -= PKTLEN(osh, p);
  82         }
  83
  84         if (!p)
  85                 return 0;
  86
  87         /* copy the data */
  88         for (; p && len; p = PKTNEXT(osh, p)) {
  89                 n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
  90                 bcopy(PKTDATA(osh, p) + offset, buf, n);
  91                 buf += n;
  92                 len -= n;
  93                 ret += n;
  94                 offset = 0;
  95         }
  96
  97         return ret;
  98 }
  99
 100 /* copy a buffer into a pkt buffer chain */
 101 uint
 102 pktfrombuf(osl_t *osh, void *p, uint offset, int len, uchar *buf)
 103 {
 104         uint n, ret = 0;
 105
 106
 107         /* skip 'offset' bytes */
 108         for (; p && offset; p = PKTNEXT(osh, p)) {
 109                 if (offset < (uint)PKTLEN(osh, p))
 110                         break;
 111                 offset -= PKTLEN(osh, p);
 112         }
 113
 114         if (!p)
 115                 return 0;
 116
 117         /* copy the data */
 118         for (; p && len; p = PKTNEXT(osh, p)) {
 119                 n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
 120                 bcopy(buf, PKTDATA(osh, p) + offset, n);
 121                 buf += n;
 122                 len -= n;
 123                 ret += n;
 124                 offset = 0;
 125         }
 126
 127         return ret;
 128 }
 129
 130
 131
 132 /* return total length of buffer chain */
 133 uint BCMFASTPATH
 134 pkttotlen(osl_t *osh, void *p)
 135 {
 136         uint total;
 137         int len;
 138
 139         total = 0;
 140         for (; p; p = PKTNEXT(osh, p)) {
 141                 len = PKTLEN(osh, p);
 142                 total += len;
 143 #ifdef BCMLFRAG
 144                 if (BCMLFRAG_ENAB()) {
 145                         if (PKTISFRAG(osh, p)) {
 146                                 total += PKTFRAGTOTLEN(osh, p);
 147                         }
 148                 }
 149 #endif
 150         }
 151
 152         return (total);
 153 }
 154
 155 /* return the last buffer of chained pkt */
 156 void *
 157 pktlast(osl_t *osh, void *p)
 158 {
 159         for (; PKTNEXT(osh, p); p = PKTNEXT(osh, p))
 160                 ;
 161
 162         return (p);
 163 }
 164
 165 /* count segments of a chained packet */
 166 uint BCMFASTPATH
 167 pktsegcnt(osl_t *osh, void *p)
 168 {
 169         uint cnt;
 170
 171         for (cnt = 0; p; p = PKTNEXT(osh, p)) {
 172                 cnt++;
 173 #ifdef BCMLFRAG
 174                 if (BCMLFRAG_ENAB()) {
 175                         if (PKTISFRAG(osh, p)) {
 176                                 cnt += PKTFRAGTOTNUM(osh, p);
 177                         }
 178                 }
 179 #endif
 180         }
 181
 182         return cnt;
 183 }
 184
 185
 186 /* count segments of a chained packet */
 187 uint BCMFASTPATH
 188 pktsegcnt_war(osl_t *osh, void *p)
 189 {
 190         uint cnt;
 191         uint8 *pktdata;
 192         uint len, remain, align64;
 193
 194         for (cnt = 0; p; p = PKTNEXT(osh, p)) {
 195                 cnt++;
 196                 len = PKTLEN(osh, p);
 197                 if (len > 128) {
 198                         pktdata = (uint8 *)PKTDATA(osh, p);     /* starting address of data */
 199                         /* Check for page boundary straddle (2048B) */
 200                         if (((uintptr)pktdata & ~0x7ff) != ((uintptr)(pktdata+len) & ~0x7ff))
 201                                 cnt++;
 202
 203                         align64 = (uint)((uintptr)pktdata & 0x3f);      /* aligned to 64B */
 204                         align64 = (64 - align64) & 0x3f;
 205                         len -= align64;         /* bytes from aligned 64B to end */
 206                         /* if aligned to 128B, check for MOD 128 between 1 to 4B */
 207                         remain = len % 128;
 208                         if (remain > 0 && remain <= 4)
 209                                 cnt++;          /* add extra seg */
 210                 }
 211         }
 212
 213         return cnt;
 214 }
 215
 216 uint8 * BCMFASTPATH
 217 pktdataoffset(osl_t *osh, void *p,  uint offset)
 218 {
 219         uint total = pkttotlen(osh, p);
 220         uint pkt_off = 0, len = 0;
 221         uint8 *pdata = (uint8 *) PKTDATA(osh, p);
 222
 223         if (offset > total)
 224                 return NULL;
 225
 226         for (; p; p = PKTNEXT(osh, p)) {
 227                 pdata = (uint8 *) PKTDATA(osh, p);
 228                 pkt_off = offset - len;
 229                 len += PKTLEN(osh, p);
 230                 if (len > offset)
 231                         break;
 232         }
 233         return (uint8*) (pdata+pkt_off);
 234 }
 235
 236
 237 /* given a offset in pdata, find the pkt seg hdr */
 238 void *
 239 pktoffset(osl_t *osh, void *p,  uint offset)
 240 {
 241         uint total = pkttotlen(osh, p);
 242         uint len = 0;
 243
 244         if (offset > total)
 245                 return NULL;
 246
 247         for (; p; p = PKTNEXT(osh, p)) {
 248                 len += PKTLEN(osh, p);
 249                 if (len > offset)
 250                         break;
 251         }
 252         return p;
 253 }
 254
 255 /*
 256  * osl multiple-precedence packet queue
 257  * hi_prec is always >= the number of the highest non-empty precedence
 258  */
 259 void * BCMFASTPATH
 260 pktq_penq(struct pktq *pq, int prec, void *p)
 261 {
 262         struct pktq_prec *q;
 263
 264         ASSERT(prec >= 0 && prec < pq->num_prec);
 265         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
 266
 267         ASSERT(!pktq_full(pq));
 268         ASSERT(!pktq_pfull(pq, prec));
 269
 270         q = &pq->q[prec];
 271
 272         if (q->head)
 273                 PKTSETLINK(q->tail, p);
 274         else
 275                 q->head = p;
 276
 277         q->tail = p;
 278         q->len++;
 279
 280         pq->len++;
 281
 282         if (pq->hi_prec < prec)
 283                 pq->hi_prec = (uint8)prec;
 284
 285         return p;
 286 }
 287
 288 void * BCMFASTPATH
 289 pktq_penq_head(struct pktq *pq, int prec, void *p)
 290 {
 291         struct pktq_prec *q;
 292
 293         ASSERT(prec >= 0 && prec < pq->num_prec);
 294         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
 295
 296         ASSERT(!pktq_full(pq));
 297         ASSERT(!pktq_pfull(pq, prec));
 298
 299         q = &pq->q[prec];
 300
 301         if (q->head == NULL)
 302                 q->tail = p;
 303
 304         PKTSETLINK(p, q->head);
 305         q->head = p;
 306         q->len++;
 307
 308         pq->len++;
 309
 310         if (pq->hi_prec < prec)
 311                 pq->hi_prec = (uint8)prec;
 312
 313         return p;
 314 }
 315
 316 void * BCMFASTPATH
 317 pktq_pdeq(struct pktq *pq, int prec)
 318 {
 319         struct pktq_prec *q;
 320         void *p;
 321
 322         ASSERT(prec >= 0 && prec < pq->num_prec);
 323
 324         q = &pq->q[prec];
 325
 326         if ((p = q->head) == NULL)
 327                 return NULL;
 328
 329         if ((q->head = PKTLINK(p)) == NULL)
 330                 q->tail = NULL;
 331
 332         q->len--;
 333
 334         pq->len--;
 335
 336         PKTSETLINK(p, NULL);
 337
 338         return p;
 339 }
 340
 341 void * BCMFASTPATH
 342 pktq_pdeq_prev(struct pktq *pq, int prec, void *prev_p)
 343 {
 344         struct pktq_prec *q;
 345         void *p;
 346
 347         ASSERT(prec >= 0 && prec < pq->num_prec);
 348
 349         q = &pq->q[prec];
 350
 351         if (prev_p == NULL)
 352                 return NULL;
 353
 354         if ((p = PKTLINK(prev_p)) == NULL)
 355                 return NULL;
 356
 357         q->len--;
 358
 359         pq->len--;
 360
 361         PKTSETLINK(prev_p, PKTLINK(p));
 362         PKTSETLINK(p, NULL);
 363
 364         return p;
 365 }
 366
 367 void * BCMFASTPATH
 368 pktq_pdeq_with_fn(struct pktq *pq, int prec, ifpkt_cb_t fn, int arg)
 369 {
 370         struct pktq_prec *q;
 371         void *p, *prev = NULL;
 372
 373         ASSERT(prec >= 0 && prec < pq->num_prec);
 374
 375         q = &pq->q[prec];
 376         p = q->head;
 377
 378         while (p) {
 379                 if (fn == NULL || (*fn)(p, arg)) {
 380                         break;
 381                 } else {
 382                         prev = p;
 383                         p = PKTLINK(p);
 384                 }
 385         }
 386         if (p == NULL)
 387                 return NULL;
 388
 389         if (prev == NULL) {
 390                 if ((q->head = PKTLINK(p)) == NULL) {
 391                         q->tail = NULL;
 392                 }
 393         } else {
 394                 PKTSETLINK(prev, PKTLINK(p));
 395                 if (q->tail == p) {
 396                         q->tail = prev;
 397                 }
 398         }
 399
 400         q->len--;
 401
 402         pq->len--;
 403
 404         PKTSETLINK(p, NULL);
 405
 406         return p;
 407 }
 408
 409 void * BCMFASTPATH
 410 pktq_pdeq_tail(struct pktq *pq, int prec)
 411 {
 412         struct pktq_prec *q;
 413         void *p, *prev;
 414
 415         ASSERT(prec >= 0 && prec < pq->num_prec);
 416
 417         q = &pq->q[prec];
 418
 419         if ((p = q->head) == NULL)
 420                 return NULL;
 421
 422         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 423                 prev = p;
 424
 425         if (prev)
 426                 PKTSETLINK(prev, NULL);
 427         else
 428                 q->head = NULL;
 429
 430         q->tail = prev;
 431         q->len--;
 432
 433         pq->len--;
 434
 435         return p;
 436 }
 437
 438 void
 439 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir, ifpkt_cb_t fn, int arg)
 440 {
 441         struct pktq_prec *q;
 442         void *p, *prev = NULL;
 443
 444         q = &pq->q[prec];
 445         p = q->head;
 446         while (p) {
 447                 if (fn == NULL || (*fn)(p, arg)) {
 448                         bool head = (p == q->head);
 449                         if (head)
 450                                 q->head = PKTLINK(p);
 451                         else
 452                                 PKTSETLINK(prev, PKTLINK(p));
 453                         PKTSETLINK(p, NULL);
 454                         PKTFREE(osh, p, dir);
 455                         q->len--;
 456                         pq->len--;
 457                         p = (head ? q->head : PKTLINK(prev));
 458                 } else {
 459                         prev = p;
 460                         p = PKTLINK(p);
 461                 }
 462         }
 463
 464         if (q->head == NULL) {
 465                 ASSERT(q->len == 0);
 466                 q->tail = NULL;
 467         }
 468 }
 469
 470 bool BCMFASTPATH
 471 pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
 472 {
 473         struct pktq_prec *q;
 474         void *p;
 475
 476         ASSERT(prec >= 0 && prec < pq->num_prec);
 477
 478         if (!pktbuf)
 479                 return FALSE;
 480
 481         q = &pq->q[prec];
 482
 483         if (q->head == pktbuf) {
 484                 if ((q->head = PKTLINK(pktbuf)) == NULL)
 485                         q->tail = NULL;
 486         } else {
 487                 for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
 488                         ;
 489                 if (p == NULL)
 490                         return FALSE;
 491
 492                 PKTSETLINK(p, PKTLINK(pktbuf));
 493                 if (q->tail == pktbuf)
 494                         q->tail = p;
 495         }
 496
 497         q->len--;
 498         pq->len--;
 499         PKTSETLINK(pktbuf, NULL);
 500         return TRUE;
 501 }
 502
 503 void
 504 pktq_init(struct pktq *pq, int num_prec, int max_len)
 505 {
 506         int prec;
 507
 508         ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
 509
 510         /* pq is variable size; only zero out what's requested */
 511         bzero(pq, OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 512
 513         pq->num_prec = (uint16)num_prec;
 514
 515         pq->max = (uint16)max_len;
 516
 517         for (prec = 0; prec < num_prec; prec++)
 518                 pq->q[prec].max = pq->max;
 519 }
 520
 521 void
 522 pktq_set_max_plen(struct pktq *pq, int prec, int max_len)
 523 {
 524         ASSERT(prec >= 0 && prec < pq->num_prec);
 525
 526         if (prec < pq->num_prec)
 527                 pq->q[prec].max = (uint16)max_len;
 528 }
 529
 530 void * BCMFASTPATH
 531 pktq_deq(struct pktq *pq, int *prec_out)
 532 {
 533         struct pktq_prec *q;
 534         void *p;
 535         int prec;
 536
 537         if (pq->len == 0)
 538                 return NULL;
 539
 540         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 541                 pq->hi_prec--;
 542
 543         q = &pq->q[prec];
 544
 545         if ((p = q->head) == NULL)
 546                 return NULL;
 547
 548         if ((q->head = PKTLINK(p)) == NULL)
 549                 q->tail = NULL;
 550
 551         q->len--;
 552
 553         pq->len--;
 554
 555         if (prec_out)
 556                 *prec_out = prec;
 557
 558         PKTSETLINK(p, NULL);
 559
 560         return p;
 561 }
 562
 563 void * BCMFASTPATH
 564 pktq_deq_tail(struct pktq *pq, int *prec_out)
 565 {
 566         struct pktq_prec *q;
 567         void *p, *prev;
 568         int prec;
 569
 570         if (pq->len == 0)
 571                 return NULL;
 572
 573         for (prec = 0; prec < pq->hi_prec; prec++)
 574                 if (pq->q[prec].head)
 575                         break;
 576
 577         q = &pq->q[prec];
 578
 579         if ((p = q->head) == NULL)
 580                 return NULL;
 581
 582         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 583                 prev = p;
 584
 585         if (prev)
 586                 PKTSETLINK(prev, NULL);
 587         else
 588                 q->head = NULL;
 589
 590         q->tail = prev;
 591         q->len--;
 592
 593         pq->len--;
 594
 595         if (prec_out)
 596                 *prec_out = prec;
 597
 598         PKTSETLINK(p, NULL);
 599
 600         return p;
 601 }
 602
 603 void *
 604 pktq_peek(struct pktq *pq, int *prec_out)
 605 {
 606         int prec;
 607
 608         if (pq->len == 0)
 609                 return NULL;
 610
 611         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 612                 pq->hi_prec--;
 613
 614         if (prec_out)
 615                 *prec_out = prec;
 616
 617         return (pq->q[prec].head);
 618 }
 619
 620 void *
 621 pktq_peek_tail(struct pktq *pq, int *prec_out)
 622 {
 623         int prec;
 624
 625         if (pq->len == 0)
 626                 return NULL;
 627
 628         for (prec = 0; prec < pq->hi_prec; prec++)
 629                 if (pq->q[prec].head)
 630                         break;
 631
 632         if (prec_out)
 633                 *prec_out = prec;
 634
 635         return (pq->q[prec].tail);
 636 }
 637
 638 void
 639 pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg)
 640 {
 641         int prec;
 642
 643         /* Optimize flush, if pktq len = 0, just return.
 644          * pktq len of 0 means pktq's prec q's are all empty.
 645          */
 646         if (pq->len == 0) {
 647                 return;
 648         }
 649
 650         for (prec = 0; prec < pq->num_prec; prec++)
 651                 pktq_pflush(osh, pq, prec, dir, fn, arg);
 652         if (fn == NULL)
 653                 ASSERT(pq->len == 0);
 654 }
 655
 656 /* Return sum of lengths of a specific set of precedences */
 657 int
 658 pktq_mlen(struct pktq *pq, uint prec_bmp)
 659 {
 660         int prec, len;
 661
 662         len = 0;
 663
 664         for (prec = 0; prec <= pq->hi_prec; prec++)
 665                 if (prec_bmp & (1 << prec))
 666                         len += pq->q[prec].len;
 667
 668         return len;
 669 }
 670
 671 /* Priority peek from a specific set of precedences */
 672 void * BCMFASTPATH
 673 pktq_mpeek(struct pktq *pq, uint prec_bmp, int *prec_out)
 674 {
 675         struct pktq_prec *q;
 676         void *p;
 677         int prec;
 678
 679         if (pq->len == 0)
 680         {
 681                 return NULL;
 682         }
 683         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 684                 pq->hi_prec--;
 685
 686         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 687                 if (prec-- == 0)
 688                         return NULL;
 689
 690         q = &pq->q[prec];
 691
 692         if ((p = q->head) == NULL)
 693                 return NULL;
 694
 695         if (prec_out)
 696                 *prec_out = prec;
 697
 698         return p;
 699 }
 700 /* Priority dequeue from a specific set of precedences */
 701 void * BCMFASTPATH
 702 pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
 703 {
 704         struct pktq_prec *q;
 705         void *p;
 706         int prec;
 707
 708         if (pq->len == 0)
 709                 return NULL;
 710
 711         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 712                 pq->hi_prec--;
 713
 714         while ((pq->q[prec].head == NULL) || ((prec_bmp & (1 << prec)) == 0))
 715                 if (prec-- == 0)
 716                         return NULL;
 717
 718         q = &pq->q[prec];
 719
 720         if ((p = q->head) == NULL)
 721                 return NULL;
 722
 723         if ((q->head = PKTLINK(p)) == NULL)
 724                 q->tail = NULL;
 725
 726         q->len--;
 727
 728         if (prec_out)
 729                 *prec_out = prec;
 730
 731         pq->len--;
 732
 733         PKTSETLINK(p, NULL);
 734
 735         return p;
 736 }
 737
 738 #endif /* BCMDRIVER */
 739
 740 #if !defined(BCMROMOFFLOAD_EXCLUDE_BCMUTILS_FUNCS)
 741 const unsigned char bcm_ctype[] = {
 742
 743         _BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,                        /* 0-7 */
 744         _BCM_C, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C|_BCM_S, _BCM_C,
 745         _BCM_C, /* 8-15 */
 746         _BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,                        /* 16-23 */
 747         _BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,_BCM_C,                        /* 24-31 */
 748         _BCM_S|_BCM_SP,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,                /* 32-39 */
 749         _BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,                        /* 40-47 */
 750         _BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,_BCM_D,                        /* 48-55 */
 751         _BCM_D,_BCM_D,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,                        /* 56-63 */
 752         _BCM_P, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X, _BCM_U|_BCM_X,
 753         _BCM_U|_BCM_X, _BCM_U, /* 64-71 */
 754         _BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,                        /* 72-79 */
 755         _BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,_BCM_U,                        /* 80-87 */
 756         _BCM_U,_BCM_U,_BCM_U,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_P,                        /* 88-95 */
 757         _BCM_P, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X, _BCM_L|_BCM_X,
 758         _BCM_L|_BCM_X, _BCM_L, /* 96-103 */
 759         _BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 104-111 */
 760         _BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L,_BCM_L, /* 112-119 */
 761         _BCM_L,_BCM_L,_BCM_L,_BCM_P,_BCM_P,_BCM_P,_BCM_P,_BCM_C, /* 120-127 */
 762         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,         /* 128-143 */
 763         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,         /* 144-159 */
 764         _BCM_S|_BCM_SP, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
 765         _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, /* 160-175 */
 766         _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P,
 767         _BCM_P, _BCM_P, _BCM_P, _BCM_P, _BCM_P, /* 176-191 */
 768         _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U,
 769         _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, /* 192-207 */
 770         _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_P, _BCM_U, _BCM_U, _BCM_U,
 771         _BCM_U, _BCM_U, _BCM_U, _BCM_U, _BCM_L, /* 208-223 */
 772         _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L,
 773         _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, /* 224-239 */
 774         _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_P, _BCM_L, _BCM_L, _BCM_L,
 775         _BCM_L, _BCM_L, _BCM_L, _BCM_L, _BCM_L /* 240-255 */
 776 };
 777
 778 ulong
 779 bcm_strtoul(const char *cp, char **endp, uint base)
 780 {
 781         ulong result, last_result = 0, value;
 782         bool minus;
 783
 784         minus = FALSE;
 785
 786         while (bcm_isspace(*cp))
 787                 cp++;
 788
 789         if (cp[0] == '+')
 790                 cp++;
 791         else if (cp[0] == '-') {
 792                 minus = TRUE;
 793                 cp++;
 794         }
 795
 796         if (base == 0) {
 797                 if (cp[0] == '0') {
 798                         if ((cp[1] == 'x') || (cp[1] == 'X')) {
 799                                 base = 16;
 800                                 cp = &cp[2];
 801                         } else {
 802                                 base = 8;
 803                                 cp = &cp[1];
 804                         }
 805                 } else
 806                         base = 10;
 807         } else if (base == 16 && (cp[0] == '0') && ((cp[1] == 'x') || (cp[1] == 'X'))) {
 808                 cp = &cp[2];
 809         }
 810
 811         result = 0;
 812
 813         while (bcm_isxdigit(*cp) &&
 814                (value = bcm_isdigit(*cp) ? *cp-'0' : bcm_toupper(*cp)-'A'+10) < base) {
 815                 result = result*base + value;
 816                 /* Detected overflow */
 817                 if (result < last_result && !minus)
 818                         return (ulong)-1;
 819                 last_result = result;
 820                 cp++;
 821         }
 822
 823         if (minus)
 824                 result = (ulong)(-(long)result);
 825
 826         if (endp)
 827                 *endp = DISCARD_QUAL(cp, char);
 828
 829         return (result);
 830 }
 831
 832 int
 833 bcm_atoi(const char *s)
 834 {
 835         return (int)bcm_strtoul(s, NULL, 10);
 836 }
 837
 838 /* return pointer to location of substring 'needle' in 'haystack' */
 839 char *
 840 bcmstrstr(const char *haystack, const char *needle)
 841 {
 842         int len, nlen;
 843         int i;
 844
 845         if ((haystack == NULL) || (needle == NULL))
 846                 return DISCARD_QUAL(haystack, char);
 847
 848         nlen = strlen(needle);
 849         len = strlen(haystack) - nlen + 1;
 850
 851         for (i = 0; i < len; i++)
 852                 if (memcmp(needle, &haystack[i], nlen) == 0)
 853                         return DISCARD_QUAL(&haystack[i], char);
 854         return (NULL);
 855 }
 856
 857 char *
 858 bcmstrcat(char *dest, const char *src)
 859 {
 860         char *p;
 861
 862         p = dest + strlen(dest);
 863
 864         while ((*p++ = *src++) != '\0')
 865                 ;
 866
 867         return (dest);
 868 }
 869
 870 char *
 871 bcmstrncat(char *dest, const char *src, uint size)
 872 {
 873         char *endp;
 874         char *p;
 875
 876         p = dest + strlen(dest);
 877         endp = p + size;
 878
 879         while (p != endp && (*p++ = *src++) != '\0')
 880                 ;
 881
 882         return (dest);
 883 }
 884
 885
 886 /****************************************************************************
 887 * Function:   bcmstrtok
 888 *
 889 * Purpose:
 890 *  Tokenizes a string. This function is conceptually similiar to ANSI C strtok(),
 891 *  but allows strToken() to be used by different strings or callers at the same
 892 *  time. Each call modifies '*string' by substituting a NULL character for the
 893 *  first delimiter that is encountered, and updates 'string' to point to the char
 894 *  after the delimiter. Leading delimiters are skipped.
 895 *
 896 * Parameters:
 897 *  string      (mod) Ptr to string ptr, updated by token.
 898 *  delimiters  (in)  Set of delimiter characters.
 899 *  tokdelim    (out) Character that delimits the returned token. (May
 900 *                    be set to NULL if token delimiter is not required).
 901 *
 902 * Returns:  Pointer to the next token found. NULL when no more tokens are found.
 903 *****************************************************************************
 904 */
 905 char *
 906 bcmstrtok(char **string, const char *delimiters, char *tokdelim)
 907 {
 908         unsigned char *str;
 909         unsigned long map[8];
 910         int count;
 911         char *nextoken;
 912
 913         if (tokdelim != NULL) {
 914                 /* Prime the token delimiter */
 915                 *tokdelim = '\0';
 916         }
 917
 918         /* Clear control map */
 919         for (count = 0; count < 8; count++) {
 920                 map[count] = 0;
 921         }
 922
 923         /* Set bits in delimiter table */
 924         do {
 925                 map[*delimiters >> 5] |= (1 << (*delimiters & 31));
 926         }
 927         while (*delimiters++);
 928
 929         str = (unsigned char*)*string;
 930
 931         /* Find beginning of token (skip over leading delimiters). Note that
 932          * there is no token iff this loop sets str to point to the terminal
 933          * null (*str == '\0')
 934          */
 935         while (((map[*str >> 5] & (1 << (*str & 31))) && *str) || (*str == ' ')) {
 936                 str++;
 937         }
 938
 939         nextoken = (char*)str;
 940
 941         /* Find the end of the token. If it is not the end of the string,
 942          * put a null there.
 943          */
 944         for (; *str; str++) {
 945                 if (map[*str >> 5] & (1 << (*str & 31))) {
 946                         if (tokdelim != NULL) {
 947                                 *tokdelim = *str;
 948                         }
 949
 950                         *str++ = '\0';
 951                         break;
 952                 }
 953         }
 954
 955         *string = (char*)str;
 956
 957         /* Determine if a token has been found. */
 958         if (nextoken == (char *) str) {
 959                 return NULL;
 960         }
 961         else {
 962                 return nextoken;
 963         }
 964 }
 965
 966
 967 #define xToLower(C) \
 968         ((C >= 'A' && C <= 'Z') ? (char)((int)C - (int)'A' + (int)'a') : C)
 969
 970
 971 /****************************************************************************
 972 * Function:   bcmstricmp
 973 *
 974 * Purpose:    Compare to strings case insensitively.
 975 *
 976 * Parameters: s1 (in) First string to compare.
 977 *             s2 (in) Second string to compare.
 978 *
 979 * Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
 980 *             t1 > t2, when ignoring case sensitivity.
 981 *****************************************************************************
 982 */
 983 int
 984 bcmstricmp(const char *s1, const char *s2)
 985 {
 986         char dc, sc;
 987
 988         while (*s2 && *s1) {
 989                 dc = xToLower(*s1);
 990                 sc = xToLower(*s2);
 991                 if (dc < sc) return -1;
 992                 if (dc > sc) return 1;
 993                 s1++;
 994                 s2++;
 995         }
 996
 997         if (*s1 && !*s2) return 1;
 998         if (!*s1 && *s2) return -1;
 999         return 0;
1000 }
1001
1002
1003 /****************************************************************************
1004 * Function:   bcmstrnicmp
1005 *
1006 * Purpose:    Compare to strings case insensitively, upto a max of 'cnt'
1007 *             characters.
1008 *
1009 * Parameters: s1  (in) First string to compare.
1010 *             s2  (in) Second string to compare.
1011 *             cnt (in) Max characters to compare.
1012 *
1013 * Returns:    Return 0 if the two strings are equal, -1 if t1 < t2 and 1 if
1014 *             t1 > t2, when ignoring case sensitivity.
1015 *****************************************************************************
1016 */
1017 int
1018 bcmstrnicmp(const char* s1, const char* s2, int cnt)
1019 {
1020         char dc, sc;
1021
1022         while (*s2 && *s1 && cnt) {
1023                 dc = xToLower(*s1);
1024                 sc = xToLower(*s2);
1025                 if (dc < sc) return -1;
1026                 if (dc > sc) return 1;
1027                 s1++;
1028                 s2++;
1029                 cnt--;
1030         }
1031
1032         if (!cnt) return 0;
1033         if (*s1 && !*s2) return 1;
1034         if (!*s1 && *s2) return -1;
1035         return 0;
1036 }
1037
1038 /* parse a xx:xx:xx:xx:xx:xx format ethernet address */
1039 int
1040 bcm_ether_atoe(const char *p, struct ether_addr *ea)
1041 {
1042         int i = 0;
1043         char *ep;
1044
1045         for (;;) {
1046                 ea->octet[i++] = (char) bcm_strtoul(p, &ep, 16);
1047                 p = ep;
1048                 if (!*p++ || i == 6)
1049                         break;
1050         }
1051
1052         return (i == 6);
1053 }
1054
1055 int
1056 bcm_atoipv4(const char *p, struct ipv4_addr *ip)
1057 {
1058
1059         int i = 0;
1060         char *c;
1061         for (;;) {
1062                 ip->addr[i++] = (uint8)bcm_strtoul(p, &c, 0);
1063                 if (*c++ != '.' || i == IPV4_ADDR_LEN)
1064                         break;
1065                 p = c;
1066         }
1067         return (i == IPV4_ADDR_LEN);
1068 }
1069 #endif  /* !BCMROMOFFLOAD_EXCLUDE_BCMUTILS_FUNCS */
1070
1071
1072 #if defined(CONFIG_USBRNDIS_RETAIL) || defined(NDIS_MINIPORT_DRIVER)
1073 /* registry routine buffer preparation utility functions:
1074  * parameter order is like strncpy, but returns count
1075  * of bytes copied. Minimum bytes copied is null char(1)/wchar(2)
1076  */
1077 ulong
1078 wchar2ascii(char *abuf, ushort *wbuf, ushort wbuflen, ulong abuflen)
1079 {
1080         ulong copyct = 1;
1081         ushort i;
1082
1083         if (abuflen == 0)
1084                 return 0;
1085
1086         /* wbuflen is in bytes */
1087         wbuflen /= sizeof(ushort);
1088
1089         for (i = 0; i < wbuflen; ++i) {
1090                 if (--abuflen == 0)
1091                         break;
1092                 *abuf++ = (char) *wbuf++;
1093                 ++copyct;
1094         }
1095         *abuf = '\0';
1096
1097         return copyct;
1098 }
1099 #endif /* CONFIG_USBRNDIS_RETAIL || NDIS_MINIPORT_DRIVER */
1100
1101 char *
1102 bcm_ether_ntoa(const struct ether_addr *ea, char *buf)
1103 {
1104         static const char hex[] =
1105           {
1106                   '0', '1', '2', '3', '4', '5', '6', '7',
1107                   '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'
1108           };
1109         const uint8 *octet = ea->octet;
1110         char *p = buf;
1111         int i;
1112
1113         for (i = 0; i < 6; i++, octet++) {
1114                 *p++ = hex[(*octet >> 4) & 0xf];
1115                 *p++ = hex[*octet & 0xf];
1116                 *p++ = ':';
1117         }
1118
1119         *(p-1) = '\0';
1120
1121         return (buf);
1122 }
1123
1124 char *
1125 bcm_ip_ntoa(struct ipv4_addr *ia, char *buf)
1126 {
1127         snprintf(buf, 16, "%d.%d.%d.%d",
1128                  ia->addr[0], ia->addr[1], ia->addr[2], ia->addr[3]);
1129         return (buf);
1130 }
1131
1132 char *
1133 bcm_ipv6_ntoa(void *ipv6, char *buf)
1134 {
1135         /* Implementing RFC 5952 Sections 4 + 5 */
1136         /* Not thoroughly tested */
1137         uint16 tmp[8];
1138         uint16 *a = &tmp[0];
1139         char *p = buf;
1140         int i, i_max = -1, cnt = 0, cnt_max = 1;
1141         uint8 *a4 = NULL;
1142         memcpy((uint8 *)&tmp[0], (uint8 *)ipv6, IPV6_ADDR_LEN);
1143
1144         for (i = 0; i < IPV6_ADDR_LEN/2; i++) {
1145                 if (a[i]) {
1146                         if (cnt > cnt_max) {
1147                                 cnt_max = cnt;
1148                                 i_max = i - cnt;
1149                         }
1150                         cnt = 0;
1151                 } else
1152                         cnt++;
1153         }
1154         if (cnt > cnt_max) {
1155                 cnt_max = cnt;
1156                 i_max = i - cnt;
1157         }
1158         if (i_max == 0 &&
1159                 /* IPv4-translated: ::ffff:0:a.b.c.d */
1160                 ((cnt_max == 4 && a[4] == 0xffff && a[5] == 0) ||
1161                 /* IPv4-mapped: ::ffff:a.b.c.d */
1162                 (cnt_max == 5 && a[5] == 0xffff)))
1163                 a4 = (uint8*) (a + 6);
1164
1165         for (i = 0; i < IPV6_ADDR_LEN/2; i++) {
1166                 if ((uint8*) (a + i) == a4) {
1167                         snprintf(p, 16, ":%u.%u.%u.%u", a4[0], a4[1], a4[2], a4[3]);
1168                         break;
1169                 } else if (i == i_max) {
1170                         *p++ = ':';
1171                         i += cnt_max - 1;
1172                         p[0] = ':';
1173                         p[1] = '\0';
1174                 } else {
1175                         if (i)
1176                                 *p++ = ':';
1177                         p += snprintf(p, 8, "%x", ntoh16(a[i]));
1178                 }
1179         }
1180
1181         return buf;
1182 }
1183 #ifdef BCMDRIVER
1184
1185 void
1186 bcm_mdelay(uint ms)
1187 {
1188         uint i;
1189
1190         for (i = 0; i < ms; i++) {
1191                 OSL_DELAY(1000);
1192         }
1193 }
1194
1195
1196
1197
1198
1199 #if defined(DHD_DEBUG)
1200 /* pretty hex print a pkt buffer chain */
1201 void
1202 prpkt(const char *msg, osl_t *osh, void *p0)
1203 {
1204         void *p;
1205
1206         if (msg && (msg[0] != '\0'))
1207                 printf("%s:\n", msg);
1208
1209         for (p = p0; p; p = PKTNEXT(osh, p))
1210                 prhex(NULL, PKTDATA(osh, p), PKTLEN(osh, p));
1211 }
1212 #endif
1213
1214 /* Takes an Ethernet frame and sets out-of-bound PKTPRIO.
1215  * Also updates the inplace vlan tag if requested.
1216  * For debugging, it returns an indication of what it did.
1217  */
1218 uint BCMFASTPATH
1219 pktsetprio(void *pkt, bool update_vtag)
1220 {
1221         struct ether_header *eh;
1222         struct ethervlan_header *evh;
1223         uint8 *pktdata;
1224         int priority = 0;
1225         int rc = 0;
1226
1227         pktdata = (uint8 *)PKTDATA(OSH_NULL, pkt);
1228         ASSERT(ISALIGNED((uintptr)pktdata, sizeof(uint16)));
1229
1230         eh = (struct ether_header *) pktdata;
1231
1232         if (eh->ether_type == hton16(ETHER_TYPE_8021Q)) {
1233                 uint16 vlan_tag;
1234                 int vlan_prio, dscp_prio = 0;
1235
1236                 evh = (struct ethervlan_header *)eh;
1237
1238                 vlan_tag = ntoh16(evh->vlan_tag);
1239                 vlan_prio = (int) (vlan_tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
1240
1241                 if (evh->ether_type == hton16(ETHER_TYPE_IP)) {
1242                         uint8 *ip_body = pktdata + sizeof(struct ethervlan_header);
1243                         uint8 tos_tc = IP_TOS46(ip_body);
1244                         dscp_prio = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
1245                 }
1246
1247                 /* DSCP priority gets precedence over 802.1P (vlan tag) */
1248                 if (dscp_prio != 0) {
1249                         priority = dscp_prio;
1250                         rc |= PKTPRIO_VDSCP;
1251                 } else {
1252                         priority = vlan_prio;
1253                         rc |= PKTPRIO_VLAN;
1254                 }
1255                 /*
1256                  * If the DSCP priority is not the same as the VLAN priority,
1257                  * then overwrite the priority field in the vlan tag, with the
1258                  * DSCP priority value. This is required for Linux APs because
1259                  * the VLAN driver on Linux, overwrites the skb->priority field
1260                  * with the priority value in the vlan tag
1261                  */
1262                 if (update_vtag && (priority != vlan_prio)) {
1263                         vlan_tag &= ~(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
1264                         vlan_tag |= (uint16)priority << VLAN_PRI_SHIFT;
1265                         evh->vlan_tag = hton16(vlan_tag);
1266                         rc |= PKTPRIO_UPD;
1267                 }
1268 #ifdef DHD_LOSSLESS_ROAMING
1269         } else if (eh->ether_type == hton16(ETHER_TYPE_802_1X)) {
1270                 priority = PRIO_8021D_NC;
1271                 rc = PKTPRIO_DSCP;
1272 #endif /* DHD_LOSSLESS_ROAMING */
1273         } else if (eh->ether_type == hton16(ETHER_TYPE_IP)) {
1274                 uint8 *ip_body = pktdata + sizeof(struct ether_header);
1275                 uint8 tos_tc = IP_TOS46(ip_body);
1276                 uint8 dscp = tos_tc >> IPV4_TOS_DSCP_SHIFT;
1277                 switch (dscp) {
1278                 case DSCP_EF:
1279                         priority = PRIO_8021D_VO;
1280                         break;
1281                 case DSCP_AF31:
1282                 case DSCP_AF32:
1283                 case DSCP_AF33:
1284                         priority = PRIO_8021D_CL;
1285                         break;
1286                 case DSCP_AF21:
1287                 case DSCP_AF22:
1288                 case DSCP_AF23:
1289                 case DSCP_AF11:
1290                 case DSCP_AF12:
1291                 case DSCP_AF13:
1292                         priority = PRIO_8021D_EE;
1293                         break;
1294                 default:
1295                         priority = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
1296                         break;
1297                 }
1298
1299                 rc |= PKTPRIO_DSCP;
1300         }
1301
1302         ASSERT(priority >= 0 && priority <= MAXPRIO);
1303         PKTSETPRIO(pkt, priority);
1304         return (rc | priority);
1305 }
1306
1307 /* lookup user priority for specified DSCP */
1308 static uint8
1309 dscp2up(uint8 *up_table, uint8 dscp)
1310 {
1311         uint8 up = 255;
1312
1313         /* lookup up from table if parameters valid */
1314         if (up_table != NULL && dscp < UP_TABLE_MAX) {
1315                 up = up_table[dscp];
1316         }
1317
1318         /* 255 is unused value so return up from dscp */
1319         if (up == 255) {
1320                 up = dscp >> (IPV4_TOS_PREC_SHIFT - IPV4_TOS_DSCP_SHIFT);
1321         }
1322
1323         return up;
1324 }
1325
1326 /* set user priority by QoS Map Set table (UP table), table size is UP_TABLE_MAX */
1327 uint BCMFASTPATH
1328 pktsetprio_qms(void *pkt, int8* up_table, bool update_vtag)
1329 {
1330         if (up_table) {
1331                 uint8 *pktdata;
1332                 uint pktlen;
1333                 uint8 dscp;
1334                 uint up = 0;
1335                 uint rc = 0;
1336
1337                 pktdata = (uint8 *)PKTDATA(OSH_NULL, pkt);
1338                 pktlen = PKTLEN(OSH_NULL, pkt);
1339
1340                 if (pktgetdscp(pktdata, pktlen, &dscp)) {
1341                         rc = PKTPRIO_DSCP;
1342                         up = dscp2up(up_table, dscp);
1343                         PKTSETPRIO(pkt, up);
1344                         printf("dscp=%d, up=%d\n", dscp, up);
1345                 }
1346
1347                 return (rc | up);
1348         }
1349         else {
1350                 return pktsetprio(pkt, update_vtag);
1351         }
1352 }
1353
1354 /* Returns TRUE and DSCP if IP header found, FALSE otherwise.
1355  */
1356 bool BCMFASTPATH
1357 pktgetdscp(uint8 *pktdata, uint pktlen, uint8 *dscp)
1358 {
1359         struct ether_header *eh;
1360         struct ethervlan_header *evh;
1361         uint8 *ip_body;
1362         bool rc = FALSE;
1363
1364         /* minimum length is ether header and IP header */
1365         if (pktlen < sizeof(struct ether_header) + IPV4_MIN_HEADER_LEN)
1366                 return FALSE;
1367
1368         eh = (struct ether_header *) pktdata;
1369
1370         if (eh->ether_type == HTON16(ETHER_TYPE_IP)) {
1371                 ip_body = pktdata + sizeof(struct ether_header);
1372                 *dscp = IP_DSCP46(ip_body);
1373                 rc = TRUE;
1374         }
1375         else if (eh->ether_type == HTON16(ETHER_TYPE_8021Q)) {
1376                 evh = (struct ethervlan_header *)eh;
1377
1378                 /* minimum length is ethervlan header and IP header */
1379                 if (pktlen >= sizeof(struct ethervlan_header) + IPV4_MIN_HEADER_LEN &&
1380                         evh->ether_type == HTON16(ETHER_TYPE_IP)) {
1381                         ip_body = pktdata + sizeof(struct ethervlan_header);
1382                         *dscp = IP_DSCP46(ip_body);
1383                         rc = TRUE;
1384                 }
1385         }
1386
1387         return rc;
1388 }
1389
1390
1391 static char bcm_undeferrstr[32];
1392 static const char *bcmerrorstrtable[] = BCMERRSTRINGTABLE;
1393
1394 /* Convert the error codes into related error strings  */
1395 const char *
1396 bcmerrorstr(int bcmerror)
1397 {
1398         /* check if someone added a bcmerror code but forgot to add errorstring */
1399         ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(bcmerrorstrtable) - 1));
1400
1401         if (bcmerror > 0 || bcmerror < BCME_LAST) {
1402                 snprintf(bcm_undeferrstr, sizeof(bcm_undeferrstr), "Undefined error %d", bcmerror);
1403                 return bcm_undeferrstr;
1404         }
1405
1406         ASSERT(strlen(bcmerrorstrtable[-bcmerror]) < BCME_STRLEN);
1407
1408         return bcmerrorstrtable[-bcmerror];
1409 }
1410
1411
1412
1413 /* iovar table lookup */
1414 const bcm_iovar_t*
1415 bcm_iovar_lookup(const bcm_iovar_t *table, const char *name)
1416 {
1417         const bcm_iovar_t *vi;
1418         const char *lookup_name;
1419
1420         /* skip any ':' delimited option prefixes */
1421         lookup_name = strrchr(name, ':');
1422         if (lookup_name != NULL)
1423                 lookup_name++;
1424         else
1425                 lookup_name = name;
1426
1427         ASSERT(table != NULL);
1428
1429         for (vi = table; vi->name; vi++) {
1430                 if (!strcmp(vi->name, lookup_name))
1431                         return vi;
1432         }
1433         /* ran to end of table */
1434
1435         return NULL; /* var name not found */
1436 }
1437
1438 int
1439 bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
1440 {
1441         int bcmerror = 0;
1442
1443         /* length check on io buf */
1444         switch (vi->type) {
1445         case IOVT_BOOL:
1446         case IOVT_INT8:
1447         case IOVT_INT16:
1448         case IOVT_INT32:
1449         case IOVT_UINT8:
1450         case IOVT_UINT16:
1451         case IOVT_UINT32:
1452                 /* all integers are int32 sized args at the ioctl interface */
1453                 if (len < (int)sizeof(int)) {
1454                         bcmerror = BCME_BUFTOOSHORT;
1455                 }
1456                 break;
1457
1458         case IOVT_BUFFER:
1459                 /* buffer must meet minimum length requirement */
1460                 if (len < vi->minlen) {
1461                         bcmerror = BCME_BUFTOOSHORT;
1462                 }
1463                 break;
1464
1465         case IOVT_VOID:
1466                 if (!set) {
1467                         /* Cannot return nil... */
1468                         bcmerror = BCME_UNSUPPORTED;
1469                 } else if (len) {
1470                         /* Set is an action w/o parameters */
1471                         bcmerror = BCME_BUFTOOLONG;
1472                 }
1473                 break;
1474
1475         default:
1476                 /* unknown type for length check in iovar info */
1477                 ASSERT(0);
1478                 bcmerror = BCME_UNSUPPORTED;
1479         }
1480
1481         return bcmerror;
1482 }
1483
1484 #endif  /* BCMDRIVER */
1485
1486
1487 uint8 *
1488 bcm_write_tlv(int type, const void *data, int datalen, uint8 *dst)
1489 {
1490         uint8 *new_dst = dst;
1491         bcm_tlv_t *dst_tlv = (bcm_tlv_t *)dst;
1492
1493         /* dst buffer should always be valid */
1494         ASSERT(dst);
1495
1496         /* data len must be within valid range */
1497         ASSERT((datalen >= 0) && (datalen <= BCM_TLV_MAX_DATA_SIZE));
1498
1499         /* source data buffer pointer should be valid, unless datalen is 0
1500          * meaning no data with this TLV
1501          */
1502         ASSERT((data != NULL) || (datalen == 0));
1503
1504         /* only do work if the inputs are valid
1505          * - must have a dst to write to AND
1506          * - datalen must be within range AND
1507          * - the source data pointer must be non-NULL if datalen is non-zero
1508          * (this last condition detects datalen > 0 with a NULL data pointer)
1509          */
1510         if ((dst != NULL) &&
1511             ((datalen >= 0) && (datalen <= BCM_TLV_MAX_DATA_SIZE)) &&
1512             ((data != NULL) || (datalen == 0))) {
1513
1514                 /* write type, len fields */
1515                 dst_tlv->id = (uint8)type;
1516                 dst_tlv->len = (uint8)datalen;
1517
1518                 /* if data is present, copy to the output buffer and update
1519                  * pointer to output buffer
1520                  */
1521                 if (datalen > 0) {
1522
1523                         memcpy(dst_tlv->data, data, datalen);
1524                 }
1525
1526                 /* update the output destination poitner to point past
1527                  * the TLV written
1528                  */
1529                 new_dst = dst + BCM_TLV_HDR_SIZE + datalen;
1530         }
1531
1532         return (new_dst);
1533 }
1534
1535 uint8 *
1536 bcm_write_tlv_safe(int type, const void *data, int datalen, uint8 *dst, int dst_maxlen)
1537 {
1538         uint8 *new_dst = dst;
1539
1540         if ((datalen >= 0) && (datalen <= BCM_TLV_MAX_DATA_SIZE)) {
1541
1542                 /* if len + tlv hdr len is more than destlen, don't do anything
1543                  * just return the buffer untouched
1544                  */
1545                 if ((int)(datalen + BCM_TLV_HDR_SIZE) <= dst_maxlen) {
1546
1547                         new_dst = bcm_write_tlv(type, data, datalen, dst);
1548                 }
1549         }
1550
1551         return (new_dst);
1552 }
1553
1554 uint8 *
1555 bcm_copy_tlv(const void *src, uint8 *dst)
1556 {
1557         uint8 *new_dst = dst;
1558         const bcm_tlv_t *src_tlv = (const bcm_tlv_t *)src;
1559         uint totlen;
1560
1561         ASSERT(dst && src);
1562         if (dst && src) {
1563
1564                 totlen = BCM_TLV_HDR_SIZE + src_tlv->len;
1565                 memcpy(dst, src_tlv, totlen);
1566                 new_dst = dst + totlen;
1567         }
1568
1569         return (new_dst);
1570 }
1571
1572
1573 uint8 *bcm_copy_tlv_safe(const void *src, uint8 *dst, int dst_maxlen)
1574 {
1575         uint8 *new_dst = dst;
1576         const bcm_tlv_t *src_tlv = (const bcm_tlv_t *)src;
1577
1578         ASSERT(src);
1579         if (src) {
1580                 if (bcm_valid_tlv(src_tlv, dst_maxlen)) {
1581                         new_dst = bcm_copy_tlv(src, dst);
1582                 }
1583         }
1584
1585         return (new_dst);
1586 }
1587
1588
1589 #if !defined(BCMROMOFFLOAD_EXCLUDE_BCMUTILS_FUNCS)
1590 /*******************************************************************************
1591  * crc8
1592  *
1593  * Computes a crc8 over the input data using the polynomial:
1594  *
1595  *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
1596  *
1597  * The caller provides the initial value (either CRC8_INIT_VALUE
1598  * or the previous returned value) to allow for processing of
1599  * discontiguous blocks of data.  When generating the CRC the
1600  * caller is responsible for complementing the final return value
1601  * and inserting it into the byte stream.  When checking, a final
1602  * return value of CRC8_GOOD_VALUE indicates a valid CRC.
1603  *
1604  * Reference: Dallas Semiconductor Application Note 27
1605  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
1606  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
1607  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
1608  *
1609  * ****************************************************************************
1610  */
1611
1612 static const uint8 crc8_table[256] = {
1613     0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
1614     0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
1615     0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
1616     0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
1617     0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
1618     0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
1619     0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
1620     0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
1621     0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
1622     0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
1623     0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
1624     0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
1625     0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
1626     0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
1627     0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
1628     0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
1629     0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
1630     0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
1631     0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
1632     0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
1633     0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
1634     0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
1635     0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
1636     0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
1637     0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
1638     0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
1639     0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
1640     0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
1641     0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
1642     0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
1643     0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
1644     0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
1645 };
1646
1647 #define CRC_INNER_LOOP(n, c, x) \
1648         (c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff]
1649
1650 uint8
1651 hndcrc8(
1652         uint8 *pdata,   /* pointer to array of data to process */
1653         uint  nbytes,   /* number of input data bytes to process */
1654         uint8 crc       /* either CRC8_INIT_VALUE or previous return value */
1655 )
1656 {
1657         /* hard code the crc loop instead of using CRC_INNER_LOOP macro
1658          * to avoid the undefined and unnecessary (uint8 >> 8) operation.
1659          */
1660         while (nbytes-- > 0)
1661                 crc = crc8_table[(crc ^ *pdata++) & 0xff];
1662
1663         return crc;
1664 }
1665
1666 /*******************************************************************************
1667  * crc16
1668  *
1669  * Computes a crc16 over the input data using the polynomial:
1670  *
1671  *       x^16 + x^12 +x^5 + 1
1672  *
1673  * The caller provides the initial value (either CRC16_INIT_VALUE
1674  * or the previous returned value) to allow for processing of
1675  * discontiguous blocks of data.  When generating the CRC the
1676  * caller is responsible for complementing the final return value
1677  * and inserting it into the byte stream.  When checking, a final
1678  * return value of CRC16_GOOD_VALUE indicates a valid CRC.
1679  *
1680  * Reference: Dallas Semiconductor Application Note 27
1681  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
1682  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
1683  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
1684  *
1685  * ****************************************************************************
1686  */
1687
1688 static const uint16 crc16_table[256] = {
1689     0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
1690     0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
1691     0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
1692     0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
1693     0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
1694     0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
1695     0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
1696     0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
1697     0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
1698     0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
1699     0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
1700     0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
1701     0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
1702     0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
1703     0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
1704     0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
1705     0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
1706     0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
1707     0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
1708     0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
1709     0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
1710     0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
1711     0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
1712     0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
1713     0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
1714     0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
1715     0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
1716     0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
1717     0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
1718     0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
1719     0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
1720     0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
1721 };
1722
1723 uint16
1724 hndcrc16(
1725     uint8 *pdata,  /* pointer to array of data to process */
1726     uint nbytes, /* number of input data bytes to process */
1727     uint16 crc     /* either CRC16_INIT_VALUE or previous return value */
1728 )
1729 {
1730         while (nbytes-- > 0)
1731                 CRC_INNER_LOOP(16, crc, *pdata++);
1732         return crc;
1733 }
1734
1735 static const uint32 crc32_table[256] = {
1736     0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
1737     0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
1738     0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
1739     0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
1740     0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
1741     0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
1742     0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
1743     0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
1744     0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
1745     0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
1746     0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
1747     0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
1748     0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
1749     0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
1750     0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
1751     0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
1752     0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
1753     0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
1754     0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
1755     0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
1756     0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
1757     0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
1758     0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
1759     0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
1760     0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
1761     0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
1762     0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
1763     0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
1764     0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
1765     0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
1766     0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
1767     0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
1768     0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
1769     0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
1770     0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
1771     0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
1772     0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
1773     0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
1774     0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
1775     0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
1776     0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
1777     0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
1778     0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
1779     0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
1780     0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
1781     0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
1782     0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
1783     0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
1784     0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
1785     0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
1786     0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
1787     0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
1788     0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
1789     0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
1790     0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
1791     0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
1792     0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
1793     0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
1794     0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
1795     0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
1796     0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
1797     0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
1798     0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
1799     0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
1800 };
1801
1802 /*
1803  * crc input is CRC32_INIT_VALUE for a fresh start, or previous return value if
1804  * accumulating over multiple pieces.
1805  */
1806 uint32
1807 hndcrc32(uint8 *pdata, uint nbytes, uint32 crc)
1808 {
1809         uint8 *pend;
1810         pend = pdata + nbytes;
1811         while (pdata < pend)
1812                 CRC_INNER_LOOP(32, crc, *pdata++);
1813
1814         return crc;
1815 }
1816
1817 #ifdef notdef
1818 #define CLEN    1499    /*  CRC Length */
1819 #define CBUFSIZ         (CLEN+4)
1820 #define CNBUFS          5 /* # of bufs */
1821
1822 void
1823 testcrc32(void)
1824 {
1825         uint j, k, l;
1826         uint8 *buf;
1827         uint len[CNBUFS];
1828         uint32 crcr;
1829         uint32 crc32tv[CNBUFS] =
1830                 {0xd2cb1faa, 0xd385c8fa, 0xf5b4f3f3, 0x55789e20, 0x00343110};
1831
1832         ASSERT((buf = MALLOC(CBUFSIZ*CNBUFS)) != NULL);
1833
1834         /* step through all possible alignments */
1835         for (l = 0; l <= 4; l++) {
1836                 for (j = 0; j < CNBUFS; j++) {
1837                         len[j] = CLEN;
1838                         for (k = 0; k < len[j]; k++)
1839                                 *(buf + j*CBUFSIZ + (k+l)) = (j+k) & 0xff;
1840                 }
1841
1842                 for (j = 0; j < CNBUFS; j++) {
1843                         crcr = crc32(buf + j*CBUFSIZ + l, len[j], CRC32_INIT_VALUE);
1844                         ASSERT(crcr == crc32tv[j]);
1845                 }
1846         }
1847
1848         MFREE(buf, CBUFSIZ*CNBUFS);
1849         return;
1850 }
1851 #endif /* notdef */
1852
1853 /*
1854  * Advance from the current 1-byte tag/1-byte length/variable-length value
1855  * triple, to the next, returning a pointer to the next.
1856  * If the current or next TLV is invalid (does not fit in given buffer length),
1857  * NULL is returned.
1858  * *buflen is not modified if the TLV elt parameter is invalid, or is decremented
1859  * by the TLV parameter's length if it is valid.
1860  */
1861 bcm_tlv_t *
1862 bcm_next_tlv(bcm_tlv_t *elt, int *buflen)
1863 {
1864         int len;
1865
1866         /* validate current elt */
1867         if (!bcm_valid_tlv(elt, *buflen)) {
1868                 return NULL;
1869         }
1870
1871         /* advance to next elt */
1872         len = elt->len;
1873         elt = (bcm_tlv_t*)(elt->data + len);
1874         *buflen -= (TLV_HDR_LEN + len);
1875
1876         /* validate next elt */
1877         if (!bcm_valid_tlv(elt, *buflen)) {
1878                 return NULL;
1879         }
1880
1881         return elt;
1882 }
1883
1884 /*
1885  * Traverse a string of 1-byte tag/1-byte length/variable-length value
1886  * triples, returning a pointer to the substring whose first element
1887  * matches tag
1888  */
1889 bcm_tlv_t *
1890 bcm_parse_tlvs(void *buf, int buflen, uint key)
1891 {
1892         bcm_tlv_t *elt;
1893         int totlen;
1894
1895         elt = (bcm_tlv_t*)buf;
1896         totlen = buflen;
1897
1898         /* find tagged parameter */
1899         while (totlen >= TLV_HDR_LEN) {
1900                 int len = elt->len;
1901
1902                 /* validate remaining totlen */
1903                 if ((elt->id == key) && (totlen >= (int)(len + TLV_HDR_LEN))) {
1904
1905                         return (elt);
1906                 }
1907
1908                 elt = (bcm_tlv_t*)((uint8*)elt + (len + TLV_HDR_LEN));
1909                 totlen -= (len + TLV_HDR_LEN);
1910         }
1911
1912         return NULL;
1913 }
1914
1915 /*
1916  * Traverse a string of 1-byte tag/1-byte length/variable-length value
1917  * triples, returning a pointer to the substring whose first element
1918  * matches tag.  Stop parsing when we see an element whose ID is greater
1919  * than the target key.
1920  */
1921 bcm_tlv_t *
1922 bcm_parse_ordered_tlvs(void *buf, int buflen, uint key)
1923 {
1924         bcm_tlv_t *elt;
1925         int totlen;
1926
1927         elt = (bcm_tlv_t*)buf;
1928         totlen = buflen;
1929
1930         /* find tagged parameter */
1931         while (totlen >= TLV_HDR_LEN) {
1932                 uint id = elt->id;
1933                 int len = elt->len;
1934
1935                 /* Punt if we start seeing IDs > than target key */
1936                 if (id > key) {
1937                         return (NULL);
1938                 }
1939
1940                 /* validate remaining totlen */
1941                 if ((id == key) && (totlen >= (int)(len + TLV_HDR_LEN))) {
1942                         return (elt);
1943                 }
1944
1945                 elt = (bcm_tlv_t*)((uint8*)elt + (len + TLV_HDR_LEN));
1946                 totlen -= (len + TLV_HDR_LEN);
1947         }
1948         return NULL;
1949 }
1950 #endif  /* !BCMROMOFFLOAD_EXCLUDE_BCMUTILS_FUNCS */
1951
1952 #if defined(WLMSG_PRHDRS) || defined(WLMSG_PRPKT) || defined(WLMSG_ASSOC) || \
1953         defined(DHD_DEBUG)
1954 int
1955 bcm_format_field(const bcm_bit_desc_ex_t *bd, uint32 flags, char* buf, int len)
1956 {
1957         int i, slen = 0;
1958         uint32 bit, mask;
1959         const char *name;
1960         mask = bd->mask;
1961         if (len < 2 || !buf)
1962                 return 0;
1963
1964         buf[0] = '\0';
1965
1966         for (i = 0;  (name = bd->bitfield[i].name) != NULL; i++) {
1967                 bit = bd->bitfield[i].bit;
1968                 if ((flags & mask) == bit) {
1969                         if (len > (int)strlen(name)) {
1970                                 slen = strlen(name);
1971                                 strncpy(buf, name, slen+1);
1972                         }
1973                         break;
1974                 }
1975         }
1976         return slen;
1977 }
1978
1979 int
1980 bcm_format_flags(const bcm_bit_desc_t *bd, uint32 flags, char* buf, int len)
1981 {
1982         int i;
1983         char* p = buf;
1984         char hexstr[16];
1985         int slen = 0, nlen = 0;
1986         uint32 bit;
1987         const char* name;
1988
1989         if (len < 2 || !buf)
1990                 return 0;
1991
1992         buf[0] = '\0';
1993
1994         for (i = 0; flags != 0; i++) {
1995                 bit = bd[i].bit;
1996                 name = bd[i].name;
1997                 if (bit == 0 && flags != 0) {
1998                         /* print any unnamed bits */
1999                         snprintf(hexstr, 16, "0x%X", flags);
2000                         name = hexstr;
2001                         flags = 0;      /* exit loop */
2002                 } else if ((flags & bit) == 0)
2003                         continue;
2004                 flags &= ~bit;
2005                 nlen = strlen(name);
2006                 slen += nlen;
2007                 /* count btwn flag space */
2008                 if (flags != 0)
2009                         slen += 1;
2010                 /* need NULL char as well */
2011                 if (len <= slen)
2012                         break;
2013                 /* copy NULL char but don't count it */
2014                 strncpy(p, name, nlen + 1);
2015                 p += nlen;
2016                 /* copy btwn flag space and NULL char */
2017                 if (flags != 0)
2018                         p += snprintf(p, 2, " ");
2019         }
2020
2021         /* indicate the str was too short */
2022         if (flags != 0) {
2023                 p += snprintf(p, 2, ">");
2024         }
2025
2026         return (int)(p - buf);
2027 }
2028
2029 /* print bytes formatted as hex to a string. return the resulting string length */
2030 int
2031 bcm_format_hex(char *str, const void *bytes, int len)
2032 {
2033         int i;
2034         char *p = str;
2035         const uint8 *src = (const uint8*)bytes;
2036
2037         for (i = 0; i < len; i++) {
2038                 p += snprintf(p, 3, "%02X", *src);
2039                 src++;
2040         }
2041         return (int)(p - str);
2042 }
2043 #endif
2044
2045 /* pretty hex print a contiguous buffer */
2046 void
2047 prhex(const char *msg, uchar *buf, uint nbytes)
2048 {
2049         char line[128], *p;
2050         int len = sizeof(line);
2051         int nchar;
2052         uint i;
2053
2054         if (msg && (msg[0] != '\0'))
2055                 printf("%s:\n", msg);
2056
2057         p = line;
2058         for (i = 0; i < nbytes; i++) {
2059                 if (i % 16 == 0) {
2060                         nchar = snprintf(p, len, "  %04d: ", i);        /* line prefix */
2061                         p += nchar;
2062                         len -= nchar;
2063                 }
2064                 if (len > 0) {
2065                         nchar = snprintf(p, len, "%02x ", buf[i]);
2066                         p += nchar;
2067                         len -= nchar;
2068                 }
2069
2070                 if (i % 16 == 15) {
2071                         printf("%s\n", line);           /* flush line */
2072                         p = line;
2073                         len = sizeof(line);
2074                 }
2075         }
2076
2077         /* flush last partial line */
2078         if (p != line)
2079                 printf("%s\n", line);
2080 }
2081
2082 static const char *crypto_algo_names[] = {
2083         "NONE",
2084         "WEP1",
2085         "TKIP",
2086         "WEP128",
2087         "AES_CCM",
2088         "AES_OCB_MSDU",
2089         "AES_OCB_MPDU",
2090 #ifdef BCMCCX
2091         "CKIP",
2092         "CKIP_MMH",
2093         "WEP_MMH",
2094         "NALG",
2095 #else
2096         "NALG",
2097         "UNDEF",
2098         "UNDEF",
2099         "UNDEF",
2100 #endif /* BCMCCX */
2101         "WAPI",
2102         "PMK",
2103         "BIP",
2104         "AES_GCM",
2105         "AES_CCM256",
2106         "AES_GCM256",
2107         "BIP_CMAC256",
2108         "BIP_GMAC",
2109         "BIP_GMAC256",
2110         "UNDEF"
2111 };
2112
2113 const char *
2114 bcm_crypto_algo_name(uint algo)
2115 {
2116         return (algo < ARRAYSIZE(crypto_algo_names)) ? crypto_algo_names[algo] : "ERR";
2117 }
2118
2119
2120 char *
2121 bcm_chipname(uint chipid, char *buf, uint len)
2122 {
2123         const char *fmt;
2124
2125         fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
2126         snprintf(buf, len, fmt, chipid);
2127         return buf;
2128 }
2129
2130 /* Produce a human-readable string for boardrev */
2131 char *
2132 bcm_brev_str(uint32 brev, char *buf)
2133 {
2134         if (brev < 0x100)
2135                 snprintf(buf, 8, "%d.%d", (brev & 0xf0) >> 4, brev & 0xf);
2136         else
2137                 snprintf(buf, 8, "%c%03x", ((brev & 0xf000) == 0x1000) ? 'P' : 'A', brev & 0xfff);
2138
2139         return (buf);
2140 }
2141
2142 #define BUFSIZE_TODUMP_ATONCE 512 /* Buffer size */
2143
2144 /* dump large strings to console */
2145 void
2146 printbig(char *buf)
2147 {
2148         uint len, max_len;
2149         char c;
2150
2151         len = strlen(buf);
2152
2153         max_len = BUFSIZE_TODUMP_ATONCE;
2154
2155         while (len > max_len) {
2156                 c = buf[max_len];
2157                 buf[max_len] = '\0';
2158                 printf("%s", buf);
2159                 buf[max_len] = c;
2160
2161                 buf += max_len;
2162                 len -= max_len;
2163         }
2164         /* print the remaining string */
2165         printf("%s\n", buf);
2166         return;
2167 }
2168
2169 /* routine to dump fields in a fileddesc structure */
2170 uint
2171 bcmdumpfields(bcmutl_rdreg_rtn read_rtn, void *arg0, uint arg1, struct fielddesc *fielddesc_array,
2172         char *buf, uint32 bufsize)
2173 {
2174         uint  filled_len;
2175         int len;
2176         struct fielddesc *cur_ptr;
2177
2178         filled_len = 0;
2179         cur_ptr = fielddesc_array;
2180
2181         while (bufsize > 1) {
2182                 if (cur_ptr->nameandfmt == NULL)
2183                         break;
2184                 len = snprintf(buf, bufsize, cur_ptr->nameandfmt,
2185                                read_rtn(arg0, arg1, cur_ptr->offset));
2186                 /* check for snprintf overflow or error */
2187                 if (len < 0 || (uint32)len >= bufsize)
2188                         len = bufsize - 1;
2189                 buf += len;
2190                 bufsize -= len;
2191                 filled_len += len;
2192                 cur_ptr++;
2193         }
2194         return filled_len;
2195 }
2196
2197 uint
2198 bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
2199 {
2200         uint len;
2201
2202         len = strlen(name) + 1;
2203
2204         if ((len + datalen) > buflen)
2205                 return 0;
2206
2207         strncpy(buf, name, buflen);
2208
2209         /* append data onto the end of the name string */
2210         if (data) {
2211                 memcpy(&buf[len], data, datalen);
2212                 len += datalen;
2213         }
2214
2215         return len;
2216 }
2217
2218 /* Quarter dBm units to mW
2219  * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
2220  * Table is offset so the last entry is largest mW value that fits in
2221  * a uint16.
2222  */
2223
2224 #define QDBM_OFFSET 153         /* Offset for first entry */
2225 #define QDBM_TABLE_LEN 40       /* Table size */
2226
2227 /* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
2228  * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
2229  */
2230 #define QDBM_TABLE_LOW_BOUND 6493 /* Low bound */
2231
2232 /* Largest mW value that will round down to the last table entry,
2233  * QDBM_OFFSET + QDBM_TABLE_LEN-1.
2234  * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) + mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
2235  */
2236 #define QDBM_TABLE_HIGH_BOUND 64938 /* High bound */
2237
2238 static const uint16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
2239 /* qdBm:        +0      +1      +2      +3      +4      +5      +6      +7 */
2240 /* 153: */      6683,   7079,   7499,   7943,   8414,   8913,   9441,   10000,
2241 /* 161: */      10593,  11220,  11885,  12589,  13335,  14125,  14962,  15849,
2242 /* 169: */      16788,  17783,  18836,  19953,  21135,  22387,  23714,  25119,
2243 /* 177: */      26607,  28184,  29854,  31623,  33497,  35481,  37584,  39811,
2244 /* 185: */      42170,  44668,  47315,  50119,  53088,  56234,  59566,  63096
2245 };
2246
2247 uint16
2248 bcm_qdbm_to_mw(uint8 qdbm)
2249 {
2250         uint factor = 1;
2251         int idx = qdbm - QDBM_OFFSET;
2252
2253         if (idx >= QDBM_TABLE_LEN) {
2254                 /* clamp to max uint16 mW value */
2255                 return 0xFFFF;
2256         }
2257
2258         /* scale the qdBm index up to the range of the table 0-40
2259          * where an offset of 40 qdBm equals a factor of 10 mW.
2260          */
2261         while (idx < 0) {
2262                 idx += 40;
2263                 factor *= 10;
2264         }
2265
2266         /* return the mW value scaled down to the correct factor of 10,
2267          * adding in factor/2 to get proper rounding.
2268          */
2269         return ((nqdBm_to_mW_map[idx] + factor/2) / factor);
2270 }
2271
2272 uint8
2273 bcm_mw_to_qdbm(uint16 mw)
2274 {
2275         uint8 qdbm;
2276         int offset;
2277         uint mw_uint = mw;
2278         uint boundary;
2279
2280         /* handle boundary case */
2281         if (mw_uint <= 1)
2282                 return 0;
2283
2284         offset = QDBM_OFFSET;
2285
2286         /* move mw into the range of the table */
2287         while (mw_uint < QDBM_TABLE_LOW_BOUND) {
2288                 mw_uint *= 10;
2289                 offset -= 40;
2290         }
2291
2292         for (qdbm = 0; qdbm < QDBM_TABLE_LEN-1; qdbm++) {
2293                 boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm+1] -
2294                                                     nqdBm_to_mW_map[qdbm])/2;
2295                 if (mw_uint < boundary) break;
2296         }
2297
2298         qdbm += (uint8)offset;
2299
2300         return (qdbm);
2301 }
2302
2303
2304 uint
2305 bcm_bitcount(uint8 *bitmap, uint length)
2306 {
2307         uint bitcount = 0, i;
2308         uint8 tmp;
2309         for (i = 0; i < length; i++) {
2310                 tmp = bitmap[i];
2311                 while (tmp) {
2312                         bitcount++;
2313                         tmp &= (tmp - 1);
2314                 }
2315         }
2316         return bitcount;
2317 }
2318
2319 #ifdef BCMDRIVER
2320
2321 /* Initialization of bcmstrbuf structure */
2322 void
2323 bcm_binit(struct bcmstrbuf *b, char *buf, uint size)
2324 {
2325         b->origsize = b->size = size;
2326         b->origbuf = b->buf = buf;
2327 }
2328
2329 /* Buffer sprintf wrapper to guard against buffer overflow */
2330 int
2331 bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...)
2332 {
2333         va_list ap;
2334         int r;
2335
2336         va_start(ap, fmt);
2337
2338         r = vsnprintf(b->buf, b->size, fmt, ap);
2339
2340         /* Non Ansi C99 compliant returns -1,
2341          * Ansi compliant return r >= b->size,
2342          * bcmstdlib returns 0, handle all
2343          */
2344         /* r == 0 is also the case when strlen(fmt) is zero.
2345          * typically the case when "" is passed as argument.
2346          */
2347         if ((r == -1) || (r >= (int)b->size)) {
2348                 b->size = 0;
2349         } else {
2350                 b->size -= r;
2351                 b->buf += r;
2352         }
2353
2354         va_end(ap);
2355
2356         return r;
2357 }
2358
2359 void
2360 bcm_bprhex(struct bcmstrbuf *b, const char *msg, bool newline, uint8 *buf, int len)
2361 {
2362         int i;
2363
2364         if (msg != NULL && msg[0] != '\0')
2365                 bcm_bprintf(b, "%s", msg);
2366         for (i = 0; i < len; i ++)
2367                 bcm_bprintf(b, "%02X", buf[i]);
2368         if (newline)
2369                 bcm_bprintf(b, "\n");
2370 }
2371
2372 void
2373 bcm_inc_bytes(uchar *num, int num_bytes, uint8 amount)
2374 {
2375         int i;
2376
2377         for (i = 0; i < num_bytes; i++) {
2378                 num[i] += amount;
2379                 if (num[i] >= amount)
2380                         break;
2381                 amount = 1;
2382         }
2383 }
2384
2385 int
2386 bcm_cmp_bytes(const uchar *arg1, const uchar *arg2, uint8 nbytes)
2387 {
2388         int i;
2389
2390         for (i = nbytes - 1; i >= 0; i--) {
2391                 if (arg1[i] != arg2[i])
2392                         return (arg1[i] - arg2[i]);
2393         }
2394         return 0;
2395 }
2396
2397 void
2398 bcm_print_bytes(const char *name, const uchar *data, int len)
2399 {
2400         int i;
2401         int per_line = 0;
2402
2403         printf("%s: %d \n", name ? name : "", len);
2404         for (i = 0; i < len; i++) {
2405                 printf("%02x ", *data++);
2406                 per_line++;
2407                 if (per_line == 16) {
2408                         per_line = 0;
2409                         printf("\n");
2410                 }
2411         }
2412         printf("\n");
2413 }
2414
2415 /* Look for vendor-specific IE with specified OUI and optional type */
2416 bcm_tlv_t *
2417 bcm_find_vendor_ie(void *tlvs, int tlvs_len, const char *voui, uint8 *type, int type_len)
2418 {
2419         bcm_tlv_t *ie;
2420         uint8 ie_len;
2421
2422         ie = (bcm_tlv_t*)tlvs;
2423
2424         /* make sure we are looking at a valid IE */
2425         if (ie == NULL || !bcm_valid_tlv(ie, tlvs_len)) {
2426                 return NULL;
2427         }
2428
2429         /* Walk through the IEs looking for an OUI match */
2430         do {
2431                 ie_len = ie->len;
2432                 if ((ie->id == DOT11_MNG_PROPR_ID) &&
2433                     (ie_len >= (DOT11_OUI_LEN + type_len)) &&
2434                     !bcmp(ie->data, voui, DOT11_OUI_LEN))
2435                 {
2436                         /* compare optional type */
2437                         if (type_len == 0 ||
2438                             !bcmp(&ie->data[DOT11_OUI_LEN], type, type_len)) {
2439                                 return (ie);            /* a match */
2440                         }
2441                 }
2442         } while ((ie = bcm_next_tlv(ie, &tlvs_len)) != NULL);
2443
2444         return NULL;
2445 }
2446
2447 #if defined(WLTINYDUMP) || defined(WLMSG_INFORM) || defined(WLMSG_ASSOC) || \
2448         defined(WLMSG_PRPKT) || defined(WLMSG_WSEC)
2449 #define SSID_FMT_BUF_LEN        ((4 * DOT11_MAX_SSID_LEN) + 1)
2450
2451 int
2452 bcm_format_ssid(char* buf, const uchar ssid[], uint ssid_len)
2453 {
2454         uint i, c;
2455         char *p = buf;
2456         char *endp = buf + SSID_FMT_BUF_LEN;
2457
2458         if (ssid_len > DOT11_MAX_SSID_LEN) ssid_len = DOT11_MAX_SSID_LEN;
2459
2460         for (i = 0; i < ssid_len; i++) {
2461                 c = (uint)ssid[i];
2462                 if (c == '\\') {
2463                         *p++ = '\\';
2464                         *p++ = '\\';
2465                 } else if (bcm_isprint((uchar)c)) {
2466                         *p++ = (char)c;
2467                 } else {
2468                         p += snprintf(p, (endp - p), "\\x%02X", c);
2469                 }
2470         }
2471         *p = '\0';
2472         ASSERT(p < endp);
2473
2474         return (int)(p - buf);
2475 }
2476 #endif
2477
2478 #endif /* BCMDRIVER */
2479
2480 /*
2481  * ProcessVars:Takes a buffer of "<var>=<value>\n" lines read from a file and ending in a NUL.
2482  * also accepts nvram files which are already in the format of <var1>=<value>\0\<var2>=<value2>\0
2483  * Removes carriage returns, empty lines, comment lines, and converts newlines to NULs.
2484  * Shortens buffer as needed and pads with NULs.  End of buffer is marked by two NULs.
2485 */
2486
2487 unsigned int
2488 process_nvram_vars(char *varbuf, unsigned int len)
2489 {
2490         char *dp;
2491         bool findNewline;
2492         int column;
2493         unsigned int buf_len, n;
2494         unsigned int pad = 0;
2495
2496         dp = varbuf;
2497
2498         findNewline = FALSE;
2499         column = 0;
2500
2501         for (n = 0; n < len; n++) {
2502                 if (varbuf[n] == '\r')
2503                         continue;
2504                 if (findNewline && varbuf[n] != '\n')
2505                         continue;
2506                 findNewline = FALSE;
2507                 if (varbuf[n] == '#') {
2508                         findNewline = TRUE;
2509                         continue;
2510                 }
2511                 if (varbuf[n] == '\n') {
2512                         if (column == 0)
2513                                 continue;
2514                         *dp++ = 0;
2515                         column = 0;
2516                         continue;
2517                 }
2518                 *dp++ = varbuf[n];
2519                 column++;
2520         }
2521         buf_len = (unsigned int)(dp - varbuf);
2522         if (buf_len % 4) {
2523                 pad = 4 - buf_len % 4;
2524                 if (pad && (buf_len + pad <= len)) {
2525                         buf_len += pad;
2526                 }
2527         }
2528
2529         while (dp < varbuf + n)
2530                 *dp++ = 0;
2531
2532         return buf_len;
2533 }
2534
2535 /* calculate a * b + c */
2536 void
2537 bcm_uint64_multiple_add(uint32* r_high, uint32* r_low, uint32 a, uint32 b, uint32 c)
2538 {
2539 #define FORMALIZE(var) {cc += (var & 0x80000000) ? 1 : 0; var &= 0x7fffffff;}
2540         uint32 r1, r0;
2541         uint32 a1, a0, b1, b0, t, cc = 0;
2542
2543         a1 = a >> 16;
2544         a0 = a & 0xffff;
2545         b1 = b >> 16;
2546         b0 = b & 0xffff;
2547
2548         r0 = a0 * b0;
2549         FORMALIZE(r0);
2550
2551         t = (a1 * b0) << 16;
2552         FORMALIZE(t);
2553
2554         r0 += t;
2555         FORMALIZE(r0);
2556
2557         t = (a0 * b1) << 16;
2558         FORMALIZE(t);
2559
2560         r0 += t;
2561         FORMALIZE(r0);
2562
2563         FORMALIZE(c);
2564
2565         r0 += c;
2566         FORMALIZE(r0);
2567
2568         r0 |= (cc % 2) ? 0x80000000 : 0;
2569         r1 = a1 * b1 + ((a1 * b0) >> 16) + ((b1 * a0) >> 16) + (cc / 2);
2570
2571         *r_high = r1;
2572         *r_low = r0;
2573 }
2574
2575 /* calculate a / b */
2576 void
2577 bcm_uint64_divide(uint32* r, uint32 a_high, uint32 a_low, uint32 b)
2578 {
2579         uint32 a1 = a_high, a0 = a_low, r0 = 0;
2580
2581         if (b < 2)
2582                 return;
2583
2584         while (a1 != 0) {
2585                 r0 += (0xffffffff / b) * a1;
2586                 bcm_uint64_multiple_add(&a1, &a0, ((0xffffffff % b) + 1) % b, a1, a0);
2587         }
2588
2589         r0 += a0 / b;
2590         *r = r0;
2591 }
2592
2593 #ifndef setbit /* As in the header file */
2594 #ifdef BCMUTILS_BIT_MACROS_USE_FUNCS
2595 /* Set bit in byte array. */
2596 void
2597 setbit(void *array, uint bit)
2598 {
2599         ((uint8 *)array)[bit / NBBY] |= 1 << (bit % NBBY);
2600 }
2601
2602 /* Clear bit in byte array. */
2603 void
2604 clrbit(void *array, uint bit)
2605 {
2606         ((uint8 *)array)[bit / NBBY] &= ~(1 << (bit % NBBY));
2607 }
2608
2609 /* Test if bit is set in byte array. */
2610 bool
2611 isset(const void *array, uint bit)
2612 {
2613         return (((const uint8 *)array)[bit / NBBY] & (1 << (bit % NBBY)));
2614 }
2615
2616 /* Test if bit is clear in byte array. */
2617 bool
2618 isclr(const void *array, uint bit)
2619 {
2620         return ((((const uint8 *)array)[bit / NBBY] & (1 << (bit % NBBY))) == 0);
2621 }
2622 #endif /* BCMUTILS_BIT_MACROS_USE_FUNCS */
2623 #endif /* setbit */
2624
2625 void
2626 bcm_bitprint32(const uint32 u32)
2627 {
2628         int i;
2629         for (i = NBITS(uint32) - 1; i >= 0; i--) {
2630                 isbitset(u32, i) ? printf("1") : printf("0");
2631                 if ((i % NBBY) == 0) printf(" ");
2632         }
2633         printf("\n");
2634 }
2635
2636 #ifdef BCMDRIVER
2637 /*
2638  * Hierarchical Multiword bitmap based small id allocator.
2639  *
2640  * Multilevel hierarchy bitmap. (maximum 2 levels)
2641  * First hierarchy uses a multiword bitmap to identify 32bit words in the
2642  * second hierarchy that have at least a single bit set. Each bit in a word of
2643  * the second hierarchy represents a unique ID that may be allocated.
2644  *
2645  * BCM_MWBMAP_ITEMS_MAX: Maximum number of IDs managed.
2646  * BCM_MWBMAP_BITS_WORD: Number of bits in a bitmap word word
2647  * BCM_MWBMAP_WORDS_MAX: Maximum number of bitmap words needed for free IDs.
2648  * BCM_MWBMAP_WDMAP_MAX: Maximum number of bitmap wordss identifying first non
2649  *                       non-zero bitmap word carrying at least one free ID.
2650  * BCM_MWBMAP_SHIFT_OP:  Used in MOD, DIV and MUL operations.
2651  * BCM_MWBMAP_INVALID_IDX: Value ~0U is treated as an invalid ID
2652  *
2653  * Design Notes:
2654  * BCM_MWBMAP_USE_CNTSETBITS trades CPU for memory. A runtime count of how many
2655  * bits are computed each time on allocation and deallocation, requiring 4
2656  * array indexed access and 3 arithmetic operations. When not defined, a runtime
2657  * count of set bits state is maintained. Upto 32 Bytes per 1024 IDs is needed.
2658  * In a 4K max ID allocator, up to 128Bytes are hence used per instantiation.
2659  * In a memory limited system e.g. dongle builds, a CPU for memory tradeoff may
2660  * be used by defining BCM_MWBMAP_USE_CNTSETBITS.
2661  *
2662  * Note: wd_bitmap[] is statically declared and is not ROM friendly ... array
2663  * size is fixed. No intention to support larger than 4K indice allocation. ID
2664  * allocators for ranges smaller than 4K will have a wastage of only 12Bytes
2665  * with savings in not having to use an indirect access, had it been dynamically
2666  * allocated.
2667  */
2668 #define BCM_MWBMAP_ITEMS_MAX    (4 * 1024)  /* May increase to 16K */
2669
2670 #define BCM_MWBMAP_BITS_WORD    (NBITS(uint32))
2671 #define BCM_MWBMAP_WORDS_MAX    (BCM_MWBMAP_ITEMS_MAX / BCM_MWBMAP_BITS_WORD)
2672 #define BCM_MWBMAP_WDMAP_MAX    (BCM_MWBMAP_WORDS_MAX / BCM_MWBMAP_BITS_WORD)
2673 #define BCM_MWBMAP_SHIFT_OP     (5)
2674 #define BCM_MWBMAP_MODOP(ix)    ((ix) & (BCM_MWBMAP_BITS_WORD - 1))
2675 #define BCM_MWBMAP_DIVOP(ix)    ((ix) >> BCM_MWBMAP_SHIFT_OP)
2676 #define BCM_MWBMAP_MULOP(ix)    ((ix) << BCM_MWBMAP_SHIFT_OP)
2677
2678 /* Redefine PTR() and/or HDL() conversion to invoke audit for debugging */
2679 #define BCM_MWBMAP_PTR(hdl)             ((struct bcm_mwbmap *)(hdl))
2680 #define BCM_MWBMAP_HDL(ptr)             ((void *)(ptr))
2681
2682 #if defined(BCM_MWBMAP_DEBUG)
2683 #define BCM_MWBMAP_AUDIT(mwb) \
2684         do { \
2685                 ASSERT((mwb != NULL) && \
2686                        (((struct bcm_mwbmap *)(mwb))->magic == (void *)(mwb))); \
2687                 bcm_mwbmap_audit(mwb); \
2688         } while (0)
2689 #define MWBMAP_ASSERT(exp)              ASSERT(exp)
2690 #define MWBMAP_DBG(x)           printf x
2691 #else   /* !BCM_MWBMAP_DEBUG */
2692 #define BCM_MWBMAP_AUDIT(mwb)   do {} while (0)
2693 #define MWBMAP_ASSERT(exp)              do {} while (0)
2694 #define MWBMAP_DBG(x)
2695 #endif  /* !BCM_MWBMAP_DEBUG */
2696
2697
2698 typedef struct bcm_mwbmap {     /* Hierarchical multiword bitmap allocator    */
2699         uint16 wmaps;               /* Total number of words in free wd bitmap    */
2700         uint16 imaps;               /* Total number of words in free id bitmap    */
2701         int16  ifree;               /* Count of free indices. Used only in audits */
2702         uint16 total;               /* Total indices managed by multiword bitmap  */
2703
2704         void * magic;               /* Audit handle parameter from user           */
2705
2706         uint32 wd_bitmap[BCM_MWBMAP_WDMAP_MAX]; /* 1st level bitmap of            */
2707 #if !defined(BCM_MWBMAP_USE_CNTSETBITS)
2708         int8   wd_count[BCM_MWBMAP_WORDS_MAX];  /* free id running count, 1st lvl */
2709 #endif /*  ! BCM_MWBMAP_USE_CNTSETBITS */
2710
2711         uint32 id_bitmap[0];        /* Second level bitmap                        */
2712 } bcm_mwbmap_t;
2713
2714 /* Incarnate a hierarchical multiword bitmap based small index allocator. */
2715 struct bcm_mwbmap *
2716 bcm_mwbmap_init(osl_t *osh, uint32 items_max)
2717 {
2718         struct bcm_mwbmap * mwbmap_p;
2719         uint32 wordix, size, words, extra;
2720
2721         /* Implementation Constraint: Uses 32bit word bitmap */
2722         MWBMAP_ASSERT(BCM_MWBMAP_BITS_WORD == 32U);
2723         MWBMAP_ASSERT(BCM_MWBMAP_SHIFT_OP == 5U);
2724         MWBMAP_ASSERT(ISPOWEROF2(BCM_MWBMAP_ITEMS_MAX));
2725         MWBMAP_ASSERT((BCM_MWBMAP_ITEMS_MAX % BCM_MWBMAP_BITS_WORD) == 0U);
2726
2727         ASSERT(items_max <= BCM_MWBMAP_ITEMS_MAX);
2728
2729         /* Determine the number of words needed in the multiword bitmap */
2730         extra = BCM_MWBMAP_MODOP(items_max);
2731         words = BCM_MWBMAP_DIVOP(items_max) + ((extra != 0U) ? 1U : 0U);
2732
2733         /* Allocate runtime state of multiword bitmap */
2734         /* Note: wd_count[] or wd_bitmap[] are not dynamically allocated */
2735         size = sizeof(bcm_mwbmap_t) + (sizeof(uint32) * words);
2736         mwbmap_p = (bcm_mwbmap_t *)MALLOC(osh, size);
2737         if (mwbmap_p == (bcm_mwbmap_t *)NULL) {
2738                 ASSERT(0);
2739                 goto error1;
2740         }
2741         memset(mwbmap_p, 0, size);
2742
2743         /* Initialize runtime multiword bitmap state */
2744         mwbmap_p->imaps = (uint16)words;
2745         mwbmap_p->ifree = (int16)items_max;
2746         mwbmap_p->total = (uint16)items_max;
2747
2748         /* Setup magic, for use in audit of handle */
2749         mwbmap_p->magic = BCM_MWBMAP_HDL(mwbmap_p);
2750
2751         /* Setup the second level bitmap of free indices */
2752         /* Mark all indices as available */
2753         for (wordix = 0U; wordix < mwbmap_p->imaps; wordix++) {
2754                 mwbmap_p->id_bitmap[wordix] = (uint32)(~0U);
2755 #if !defined(BCM_MWBMAP_USE_CNTSETBITS)
2756                 mwbmap_p->wd_count[wordix] = BCM_MWBMAP_BITS_WORD;
2757 #endif /*  ! BCM_MWBMAP_USE_CNTSETBITS */
2758         }
2759
2760         /* Ensure that extra indices are tagged as un-available */
2761         if (extra) { /* fixup the free ids in last bitmap and wd_count */
2762                 uint32 * bmap_p = &mwbmap_p->id_bitmap[mwbmap_p->imaps - 1];
2763                 *bmap_p ^= (uint32)(~0U << extra); /* fixup bitmap */
2764 #if !defined(BCM_MWBMAP_USE_CNTSETBITS)
2765                 mwbmap_p->wd_count[mwbmap_p->imaps - 1] = (int8)extra; /* fixup count */
2766 #endif /*  ! BCM_MWBMAP_USE_CNTSETBITS */
2767         }
2768
2769         /* Setup the first level bitmap hierarchy */
2770         extra = BCM_MWBMAP_MODOP(mwbmap_p->imaps);
2771         words = BCM_MWBMAP_DIVOP(mwbmap_p->imaps) + ((extra != 0U) ? 1U : 0U);
2772
2773         mwbmap_p->wmaps = (uint16)words;
2774
2775         for (wordix = 0U; wordix < mwbmap_p->wmaps; wordix++)
2776                 mwbmap_p->wd_bitmap[wordix] = (uint32)(~0U);
2777         if (extra) {
2778                 uint32 * bmap_p = &mwbmap_p->wd_bitmap[mwbmap_p->wmaps - 1];
2779                 *bmap_p ^= (uint32)(~0U << extra); /* fixup bitmap */
2780         }
2781
2782         return mwbmap_p;
2783
2784 error1:
2785         return BCM_MWBMAP_INVALID_HDL;
2786 }
2787
2788 /* Release resources used by multiword bitmap based small index allocator. */
2789 void
2790 bcm_mwbmap_fini(osl_t * osh, struct bcm_mwbmap * mwbmap_hdl)
2791 {
2792         bcm_mwbmap_t * mwbmap_p;
2793
2794         BCM_MWBMAP_AUDIT(mwbmap_hdl);
2795         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
2796
2797         MFREE(osh, mwbmap_p, sizeof(struct bcm_mwbmap)
2798                              + (sizeof(uint32) * mwbmap_p->imaps));
2799         return;
2800 }
2801
2802 /* Allocate a unique small index using a multiword bitmap index allocator.    */
2803 uint32
2804 bcm_mwbmap_alloc(struct bcm_mwbmap * mwbmap_hdl)
2805 {
2806         bcm_mwbmap_t * mwbmap_p;
2807         uint32 wordix, bitmap;
2808
2809         BCM_MWBMAP_AUDIT(mwbmap_hdl);
2810         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
2811
2812         /* Start with the first hierarchy */
2813         for (wordix = 0; wordix < mwbmap_p->wmaps; ++wordix) {
2814
2815                 bitmap = mwbmap_p->wd_bitmap[wordix]; /* get the word bitmap */
2816
2817                 if (bitmap != 0U) {
2818
2819                         uint32 count, bitix, *bitmap_p;
2820
2821                         bitmap_p = &mwbmap_p->wd_bitmap[wordix];
2822
2823                         /* clear all except trailing 1 */
2824                         bitmap   = (uint32)(((int)(bitmap)) & (-((int)(bitmap))));
2825                         MWBMAP_ASSERT(C_bcm_count_leading_zeros(bitmap) ==
2826                                       bcm_count_leading_zeros(bitmap));
2827                         bitix    = (BCM_MWBMAP_BITS_WORD - 1)
2828                                  - bcm_count_leading_zeros(bitmap); /* use asm clz */
2829                         wordix   = BCM_MWBMAP_MULOP(wordix) + bitix;
2830
2831                         /* Clear bit if wd count is 0, without conditional branch */
2832 #if defined(BCM_MWBMAP_USE_CNTSETBITS)
2833                         count = bcm_cntsetbits(mwbmap_p->id_bitmap[wordix]) - 1;
2834 #else  /* ! BCM_MWBMAP_USE_CNTSETBITS */
2835                         mwbmap_p->wd_count[wordix]--;
2836                         count = mwbmap_p->wd_count[wordix];
2837                         MWBMAP_ASSERT(count ==
2838                                       (bcm_cntsetbits(mwbmap_p->id_bitmap[wordix]) - 1));
2839 #endif /* ! BCM_MWBMAP_USE_CNTSETBITS */
2840                         MWBMAP_ASSERT(count >= 0);
2841
2842                         /* clear wd_bitmap bit if id_map count is 0 */
2843                         bitmap = (count == 0) << bitix;
2844
2845                         MWBMAP_DBG((
2846                             "Lvl1: bitix<%02u> wordix<%02u>: %08x ^ %08x = %08x wfree %d",
2847                             bitix, wordix, *bitmap_p, bitmap, (*bitmap_p) ^ bitmap, count));
2848
2849                         *bitmap_p ^= bitmap;
2850
2851                         /* Use bitix in the second hierarchy */
2852                         bitmap_p = &mwbmap_p->id_bitmap[wordix];
2853
2854                         bitmap = mwbmap_p->id_bitmap[wordix]; /* get the id bitmap */
2855                         MWBMAP_ASSERT(bitmap != 0U);
2856
2857                         /* clear all except trailing 1 */
2858                         bitmap   = (uint32)(((int)(bitmap)) & (-((int)(bitmap))));
2859                         MWBMAP_ASSERT(C_bcm_count_leading_zeros(bitmap) ==
2860                                       bcm_count_leading_zeros(bitmap));
2861                         bitix    = BCM_MWBMAP_MULOP(wordix)
2862                                  + (BCM_MWBMAP_BITS_WORD - 1)
2863                                  - bcm_count_leading_zeros(bitmap); /* use asm clz */
2864
2865                         mwbmap_p->ifree--; /* decrement system wide free count */
2866                         MWBMAP_ASSERT(mwbmap_p->ifree >= 0);
2867
2868                         MWBMAP_DBG((
2869                             "Lvl2: bitix<%02u> wordix<%02u>: %08x ^ %08x = %08x ifree %d",
2870                             bitix, wordix, *bitmap_p, bitmap, (*bitmap_p) ^ bitmap,
2871                             mwbmap_p->ifree));
2872
2873                         *bitmap_p ^= bitmap; /* mark as allocated = 1b0 */
2874
2875                         return bitix;
2876                 }
2877         }
2878
2879         ASSERT(mwbmap_p->ifree == 0);
2880
2881         return BCM_MWBMAP_INVALID_IDX;
2882 }
2883
2884 /* Force an index at a specified position to be in use */
2885 void
2886 bcm_mwbmap_force(struct bcm_mwbmap * mwbmap_hdl, uint32 bitix)
2887 {
2888         bcm_mwbmap_t * mwbmap_p;
2889         uint32 count, wordix, bitmap, *bitmap_p;
2890
2891         BCM_MWBMAP_AUDIT(mwbmap_hdl);
2892         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
2893
2894         ASSERT(bitix < mwbmap_p->total);
2895
2896         /* Start with second hierarchy */
2897         wordix   = BCM_MWBMAP_DIVOP(bitix);
2898         bitmap   = (uint32)(1U << BCM_MWBMAP_MODOP(bitix));
2899         bitmap_p = &mwbmap_p->id_bitmap[wordix];
2900
2901         ASSERT((*bitmap_p & bitmap) == bitmap);
2902
2903         mwbmap_p->ifree--; /* update free count */
2904         ASSERT(mwbmap_p->ifree >= 0);
2905
2906         MWBMAP_DBG(("Lvl2: bitix<%u> wordix<%u>: %08x ^ %08x = %08x ifree %d",
2907                    bitix, wordix, *bitmap_p, bitmap, (*bitmap_p) ^ bitmap,
2908                    mwbmap_p->ifree));
2909
2910         *bitmap_p ^= bitmap; /* mark as in use */
2911
2912         /* Update first hierarchy */
2913         bitix    = wordix;
2914
2915         wordix   = BCM_MWBMAP_DIVOP(bitix);
2916         bitmap_p = &mwbmap_p->wd_bitmap[wordix];
2917
2918 #if defined(BCM_MWBMAP_USE_CNTSETBITS)
2919         count = bcm_cntsetbits(mwbmap_p->id_bitmap[bitix]);
2920 #else  /* ! BCM_MWBMAP_USE_CNTSETBITS */
2921         mwbmap_p->wd_count[bitix]--;
2922         count = mwbmap_p->wd_count[bitix];
2923         MWBMAP_ASSERT(count == bcm_cntsetbits(mwbmap_p->id_bitmap[bitix]));
2924 #endif /* ! BCM_MWBMAP_USE_CNTSETBITS */
2925         MWBMAP_ASSERT(count >= 0);
2926
2927         bitmap   = (count == 0) << BCM_MWBMAP_MODOP(bitix);
2928
2929         MWBMAP_DBG(("Lvl1: bitix<%02lu> wordix<%02u>: %08x ^ %08x = %08x wfree %d",
2930                    BCM_MWBMAP_MODOP(bitix), wordix, *bitmap_p, bitmap,
2931                    (*bitmap_p) ^ bitmap, count));
2932
2933         *bitmap_p ^= bitmap; /* mark as in use */
2934
2935         return;
2936 }
2937
2938 /* Free a previously allocated index back into the multiword bitmap allocator */
2939 void
2940 bcm_mwbmap_free(struct bcm_mwbmap * mwbmap_hdl, uint32 bitix)
2941 {
2942         bcm_mwbmap_t * mwbmap_p;
2943         uint32 wordix, bitmap, *bitmap_p;
2944
2945         BCM_MWBMAP_AUDIT(mwbmap_hdl);
2946         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
2947
2948         ASSERT(bitix < mwbmap_p->total);
2949
2950         /* Start with second level hierarchy */
2951         wordix   = BCM_MWBMAP_DIVOP(bitix);
2952         bitmap   = (1U << BCM_MWBMAP_MODOP(bitix));
2953         bitmap_p = &mwbmap_p->id_bitmap[wordix];
2954
2955         ASSERT((*bitmap_p & bitmap) == 0U);     /* ASSERT not a double free */
2956
2957         mwbmap_p->ifree++; /* update free count */
2958         ASSERT(mwbmap_p->ifree <= mwbmap_p->total);
2959
2960         MWBMAP_DBG(("Lvl2: bitix<%02u> wordix<%02u>: %08x | %08x = %08x ifree %d",
2961                    bitix, wordix, *bitmap_p, bitmap, (*bitmap_p) | bitmap,
2962                    mwbmap_p->ifree));
2963
2964         *bitmap_p |= bitmap; /* mark as available */
2965
2966         /* Now update first level hierarchy */
2967
2968         bitix    = wordix;
2969
2970         wordix   = BCM_MWBMAP_DIVOP(bitix); /* first level's word index */
2971         bitmap   = (1U << BCM_MWBMAP_MODOP(bitix));
2972         bitmap_p = &mwbmap_p->wd_bitmap[wordix];
2973
2974 #if !defined(BCM_MWBMAP_USE_CNTSETBITS)
2975         mwbmap_p->wd_count[bitix]++;
2976 #endif
2977
2978 #if defined(BCM_MWBMAP_DEBUG)
2979         {
2980                 uint32 count;
2981 #if defined(BCM_MWBMAP_USE_CNTSETBITS)
2982                 count = bcm_cntsetbits(mwbmap_p->id_bitmap[bitix]);
2983 #else  /*  ! BCM_MWBMAP_USE_CNTSETBITS */
2984                 count = mwbmap_p->wd_count[bitix];
2985                 MWBMAP_ASSERT(count == bcm_cntsetbits(mwbmap_p->id_bitmap[bitix]));
2986 #endif /*  ! BCM_MWBMAP_USE_CNTSETBITS */
2987
2988                 MWBMAP_ASSERT(count <= BCM_MWBMAP_BITS_WORD);
2989
2990                 MWBMAP_DBG(("Lvl1: bitix<%02u> wordix<%02u>: %08x | %08x = %08x wfree %d",
2991                             bitix, wordix, *bitmap_p, bitmap, (*bitmap_p) | bitmap, count));
2992         }
2993 #endif /* BCM_MWBMAP_DEBUG */
2994
2995         *bitmap_p |= bitmap;
2996
2997         return;
2998 }
2999
3000 /* Fetch the toal number of free indices in the multiword bitmap allocator */
3001 uint32
3002 bcm_mwbmap_free_cnt(struct bcm_mwbmap * mwbmap_hdl)
3003 {
3004         bcm_mwbmap_t * mwbmap_p;
3005
3006         BCM_MWBMAP_AUDIT(mwbmap_hdl);
3007         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
3008
3009         ASSERT(mwbmap_p->ifree >= 0);
3010
3011         return mwbmap_p->ifree;
3012 }
3013
3014 /* Determine whether an index is inuse or free */
3015 bool
3016 bcm_mwbmap_isfree(struct bcm_mwbmap * mwbmap_hdl, uint32 bitix)
3017 {
3018         bcm_mwbmap_t * mwbmap_p;
3019         uint32 wordix, bitmap;
3020
3021         BCM_MWBMAP_AUDIT(mwbmap_hdl);
3022         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
3023
3024         ASSERT(bitix < mwbmap_p->total);
3025
3026         wordix   = BCM_MWBMAP_DIVOP(bitix);
3027         bitmap   = (1U << BCM_MWBMAP_MODOP(bitix));
3028
3029         return ((mwbmap_p->id_bitmap[wordix] & bitmap) != 0U);
3030 }
3031
3032 /* Debug dump a multiword bitmap allocator */
3033 void
3034 bcm_mwbmap_show(struct bcm_mwbmap * mwbmap_hdl)
3035 {
3036         uint32 ix, count;
3037         bcm_mwbmap_t * mwbmap_p;
3038
3039         BCM_MWBMAP_AUDIT(mwbmap_hdl);
3040         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
3041
3042         printf("mwbmap_p %p wmaps %u imaps %u ifree %d total %u\n", mwbmap_p,
3043                mwbmap_p->wmaps, mwbmap_p->imaps, mwbmap_p->ifree, mwbmap_p->total);
3044         for (ix = 0U; ix < mwbmap_p->wmaps; ix++) {
3045                 printf("\tWDMAP:%2u. 0x%08x\t", ix, mwbmap_p->wd_bitmap[ix]);
3046                 bcm_bitprint32(mwbmap_p->wd_bitmap[ix]);
3047                 printf("\n");
3048         }
3049         for (ix = 0U; ix < mwbmap_p->imaps; ix++) {
3050 #if defined(BCM_MWBMAP_USE_CNTSETBITS)
3051                 count = bcm_cntsetbits(mwbmap_p->id_bitmap[ix]);
3052 #else  /* ! BCM_MWBMAP_USE_CNTSETBITS */
3053                 count = mwbmap_p->wd_count[ix];
3054                 MWBMAP_ASSERT(count == bcm_cntsetbits(mwbmap_p->id_bitmap[ix]));
3055 #endif /* ! BCM_MWBMAP_USE_CNTSETBITS */
3056                 printf("\tIDMAP:%2u. 0x%08x %02u\t", ix, mwbmap_p->id_bitmap[ix], count);
3057                 bcm_bitprint32(mwbmap_p->id_bitmap[ix]);
3058                 printf("\n");
3059         }
3060
3061         return;
3062 }
3063
3064 /* Audit a hierarchical multiword bitmap */
3065 void
3066 bcm_mwbmap_audit(struct bcm_mwbmap * mwbmap_hdl)
3067 {
3068         bcm_mwbmap_t * mwbmap_p;
3069         uint32 count, free_cnt = 0U, wordix, idmap_ix, bitix, *bitmap_p;
3070
3071         mwbmap_p = BCM_MWBMAP_PTR(mwbmap_hdl);
3072
3073         for (wordix = 0U; wordix < mwbmap_p->wmaps; ++wordix) {
3074
3075                 bitmap_p = &mwbmap_p->wd_bitmap[wordix];
3076
3077                 for (bitix = 0U; bitix < BCM_MWBMAP_BITS_WORD; bitix++) {
3078                         if ((*bitmap_p) & (1 << bitix)) {
3079                                 idmap_ix = BCM_MWBMAP_MULOP(wordix) + bitix;
3080 #if defined(BCM_MWBMAP_USE_CNTSETBITS)
3081                                 count = bcm_cntsetbits(mwbmap_p->id_bitmap[idmap_ix]);
3082 #else  /* ! BCM_MWBMAP_USE_CNTSETBITS */
3083                                 count = mwbmap_p->wd_count[idmap_ix];
3084                                 ASSERT(count == bcm_cntsetbits(mwbmap_p->id_bitmap[idmap_ix]));
3085 #endif /* ! BCM_MWBMAP_USE_CNTSETBITS */
3086                                 ASSERT(count != 0U);
3087                                 free_cnt += count;
3088                         }
3089                 }
3090         }
3091
3092         ASSERT(free_cnt == mwbmap_p->ifree);
3093 }
3094 /* END : Multiword bitmap based 64bit to Unique 32bit Id allocator. */
3095
3096 #endif /* BCMDRIVER */
3097
3098 /* calculate a >> b; and returns only lower 32 bits */
3099 void
3100 bcm_uint64_right_shift(uint32* r, uint32 a_high, uint32 a_low, uint32 b)
3101 {
3102         uint32 a1 = a_high, a0 = a_low, r0 = 0;
3103
3104         if (b == 0) {
3105                 r0 = a_low;
3106                 *r = r0;
3107                 return;
3108         }
3109
3110         if (b < 32) {
3111                 a0 = a0 >> b;
3112                 a1 = a1 & ((1 << b) - 1);
3113                 a1 = a1 << (32 - b);
3114                 r0 = a0 | a1;
3115                 *r = r0;
3116                 return;
3117         } else {
3118                 r0 = a1 >> (b - 32);
3119                 *r = r0;
3120                 return;
3121         }
3122
3123 }
3124
3125 /* calculate a + b where a is a 64 bit number and b is a 32 bit number */
3126 void
3127 bcm_add_64(uint32* r_hi, uint32* r_lo, uint32 offset)
3128 {
3129         uint32 r1_lo = *r_lo;
3130         (*r_lo) += offset;
3131         if (*r_lo < r1_lo)
3132                 (*r_hi) ++;
3133 }
3134
3135 /* calculate a - b where a is a 64 bit number and b is a 32 bit number */
3136 void
3137 bcm_sub_64(uint32* r_hi, uint32* r_lo, uint32 offset)
3138 {
3139         uint32 r1_lo = *r_lo;
3140         (*r_lo) -= offset;
3141         if (*r_lo > r1_lo)
3142                 (*r_hi) --;
3143 }
3144
3145 #ifdef DEBUG_COUNTER
3146 #if (OSL_SYSUPTIME_SUPPORT == TRUE)
3147 void counter_printlog(counter_tbl_t *ctr_tbl)
3148 {
3149         uint32 now;
3150
3151         if (!ctr_tbl->enabled)
3152                 return;
3153
3154         now = OSL_SYSUPTIME();
3155
3156         if (now - ctr_tbl->prev_log_print > ctr_tbl->log_print_interval) {
3157                 uint8 i = 0;
3158                 printf("counter_print(%s %d):", ctr_tbl->name, now - ctr_tbl->prev_log_print);
3159
3160                 for (i = 0; i < ctr_tbl->needed_cnt; i++) {
3161                         printf(" %u", ctr_tbl->cnt[i]);
3162                 }
3163                 printf("\n");
3164
3165                 ctr_tbl->prev_log_print = now;
3166                 bzero(ctr_tbl->cnt, CNTR_TBL_MAX * sizeof(uint));
3167         }
3168 }
3169 #else
3170 /* OSL_SYSUPTIME is not supported so no way to get time */
3171 #define counter_printlog(a) do {} while (0)
3172 #endif /* OSL_SYSUPTIME_SUPPORT == TRUE */
3173 #endif /* DEBUG_COUNTER */