release/src-rt-6.x.4708/shared/bcmutils.c

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