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

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