drivers/staging/brcm80211/util/bcmutils.c

   1 /*
   2  * Copyright (c) 2010 Broadcom Corporation
   3  *
   4  * Permission to use, copy, modify, and/or distribute this software for any
   5  * purpose with or without fee is hereby granted, provided that the above
   6  * copyright notice and this permission notice appear in all copies.
   7  *
   8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
   9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
  10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
  11  * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  13  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
  14  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  15  */
  16
  17 #include <typedefs.h>
  18 #include <bcmdefs.h>
  19 #include <stdarg.h>
  20 #include <osl.h>
  21 #include <linux/ctype.h>
  22 #include <linux/kernel.h>
  23 #include <linux/string.h>
  24 #include <linuxver.h>
  25 #include <bcmutils.h>
  26 #include <siutils.h>
  27 #include <bcmnvram.h>
  28 #include <bcmendian.h>
  29 #include <bcmdevs.h>
  30 #include <proto/ethernet.h>
  31 #include <proto/vlan.h>
  32 #include <proto/bcmip.h>
  33 #include <proto/802.1d.h>
  34 #include <proto/802.11.h>
  35
  36
  37 /* copy a buffer into a pkt buffer chain */
  38 uint pktfrombuf(osl_t *osh, void *p, uint offset, int len, unsigned char *buf)
  39 {
  40         uint n, ret = 0;
  41
  42         /* skip 'offset' bytes */
  43         for (; p && offset; p = PKTNEXT(p)) {
  44                 if (offset < (uint) PKTLEN(p))
  45                         break;
  46                 offset -= PKTLEN(p);
  47         }
  48
  49         if (!p)
  50                 return 0;
  51
  52         /* copy the data */
  53         for (; p && len; p = PKTNEXT(p)) {
  54                 n = min((uint) PKTLEN(p) - offset, (uint) len);
  55                 bcopy(buf, PKTDATA(p) + offset, n);
  56                 buf += n;
  57                 len -= n;
  58                 ret += n;
  59                 offset = 0;
  60         }
  61
  62         return ret;
  63 }
  64 /* return total length of buffer chain */
  65 uint BCMFASTPATH pkttotlen(osl_t *osh, void *p)
  66 {
  67         uint total;
  68
  69         total = 0;
  70         for (; p; p = PKTNEXT(p))
  71                 total += PKTLEN(p);
  72         return total;
  73 }
  74
  75 /*
  76  * osl multiple-precedence packet queue
  77  * hi_prec is always >= the number of the highest non-empty precedence
  78  */
  79 void *BCMFASTPATH pktq_penq(struct pktq *pq, int prec, void *p)
  80 {
  81         struct pktq_prec *q;
  82
  83         ASSERT(prec >= 0 && prec < pq->num_prec);
  84         ASSERT(PKTLINK(p) == NULL);     /* queueing chains not allowed */
  85
  86         ASSERT(!pktq_full(pq));
  87         ASSERT(!pktq_pfull(pq, prec));
  88
  89         q = &pq->q[prec];
  90
  91         if (q->head)
  92                 PKTSETLINK(q->tail, p);
  93         else
  94                 q->head = p;
  95
  96         q->tail = p;
  97         q->len++;
  98
  99         pq->len++;
 100
 101         if (pq->hi_prec < prec)
 102                 pq->hi_prec = (u8) prec;
 103
 104         return p;
 105 }
 106
 107 void *BCMFASTPATH pktq_penq_head(struct pktq *pq, int prec, void *p)
 108 {
 109         struct pktq_prec *q;
 110
 111         ASSERT(prec >= 0 && prec < pq->num_prec);
 112         ASSERT(PKTLINK(p) == NULL);     /* queueing chains not allowed */
 113
 114         ASSERT(!pktq_full(pq));
 115         ASSERT(!pktq_pfull(pq, prec));
 116
 117         q = &pq->q[prec];
 118
 119         if (q->head == NULL)
 120                 q->tail = p;
 121
 122         PKTSETLINK(p, q->head);
 123         q->head = p;
 124         q->len++;
 125
 126         pq->len++;
 127
 128         if (pq->hi_prec < prec)
 129                 pq->hi_prec = (u8) prec;
 130
 131         return p;
 132 }
 133
 134 void *BCMFASTPATH pktq_pdeq(struct pktq *pq, int prec)
 135 {
 136         struct pktq_prec *q;
 137         void *p;
 138
 139         ASSERT(prec >= 0 && prec < pq->num_prec);
 140
 141         q = &pq->q[prec];
 142
 143         p = q->head;
 144         if (p == NULL)
 145                 return NULL;
 146
 147         q->head = PKTLINK(p);
 148         if (q->head == NULL)
 149                 q->tail = NULL;
 150
 151         q->len--;
 152
 153         pq->len--;
 154
 155         PKTSETLINK(p, NULL);
 156
 157         return p;
 158 }
 159
 160 void *BCMFASTPATH pktq_pdeq_tail(struct pktq *pq, int prec)
 161 {
 162         struct pktq_prec *q;
 163         void *p, *prev;
 164
 165         ASSERT(prec >= 0 && prec < pq->num_prec);
 166
 167         q = &pq->q[prec];
 168
 169         p = q->head;
 170         if (p == NULL)
 171                 return NULL;
 172
 173         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 174                 prev = p;
 175
 176         if (prev)
 177                 PKTSETLINK(prev, NULL);
 178         else
 179                 q->head = NULL;
 180
 181         q->tail = prev;
 182         q->len--;
 183
 184         pq->len--;
 185
 186         return p;
 187 }
 188
 189 #ifdef BRCM_FULLMAC
 190 void pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir)
 191 {
 192         struct pktq_prec *q;
 193         void *p;
 194
 195         q = &pq->q[prec];
 196         p = q->head;
 197         while (p) {
 198                 q->head = PKTLINK(p);
 199                 PKTSETLINK(p, NULL);
 200                 PKTFREE(osh, p, dir);
 201                 q->len--;
 202                 pq->len--;
 203                 p = q->head;
 204         }
 205         ASSERT(q->len == 0);
 206         q->tail = NULL;
 207 }
 208
 209 void pktq_flush(osl_t *osh, struct pktq *pq, bool dir)
 210 {
 211         int prec;
 212         for (prec = 0; prec < pq->num_prec; prec++)
 213                 pktq_pflush(osh, pq, prec, dir);
 214         ASSERT(pq->len == 0);
 215 }
 216 #else /* !BRCM_FULLMAC */
 217 void
 218 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir, ifpkt_cb_t fn,
 219             int arg)
 220 {
 221         struct pktq_prec *q;
 222         void *p, *prev = NULL;
 223
 224         q = &pq->q[prec];
 225         p = q->head;
 226         while (p) {
 227                 if (fn == NULL || (*fn) (p, arg)) {
 228                         bool head = (p == q->head);
 229                         if (head)
 230                                 q->head = PKTLINK(p);
 231                         else
 232                                 PKTSETLINK(prev, PKTLINK(p));
 233                         PKTSETLINK(p, NULL);
 234                         PKTFREE(osh, p, dir);
 235                         q->len--;
 236                         pq->len--;
 237                         p = (head ? q->head : PKTLINK(prev));
 238                 } else {
 239                         prev = p;
 240                         p = PKTLINK(p);
 241                 }
 242         }
 243
 244         if (q->head == NULL) {
 245                 ASSERT(q->len == 0);
 246                 q->tail = NULL;
 247         }
 248 }
 249
 250 void pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg)
 251 {
 252         int prec;
 253         for (prec = 0; prec < pq->num_prec; prec++)
 254                 pktq_pflush(osh, pq, prec, dir, fn, arg);
 255         if (fn == NULL)
 256                 ASSERT(pq->len == 0);
 257 }
 258 #endif /* BRCM_FULLMAC */
 259
 260 void pktq_init(struct pktq *pq, int num_prec, int max_len)
 261 {
 262         int prec;
 263
 264         ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
 265
 266         /* pq is variable size; only zero out what's requested */
 267         bzero(pq,
 268               offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 269
 270         pq->num_prec = (u16) num_prec;
 271
 272         pq->max = (u16) max_len;
 273
 274         for (prec = 0; prec < num_prec; prec++)
 275                 pq->q[prec].max = pq->max;
 276 }
 277
 278 void *pktq_peek_tail(struct pktq *pq, int *prec_out)
 279 {
 280         int prec;
 281
 282         if (pq->len == 0)
 283                 return NULL;
 284
 285         for (prec = 0; prec < pq->hi_prec; prec++)
 286                 if (pq->q[prec].head)
 287                         break;
 288
 289         if (prec_out)
 290                 *prec_out = prec;
 291
 292         return pq->q[prec].tail;
 293 }
 294
 295 /* Return sum of lengths of a specific set of precedences */
 296 int pktq_mlen(struct pktq *pq, uint prec_bmp)
 297 {
 298         int prec, len;
 299
 300         len = 0;
 301
 302         for (prec = 0; prec <= pq->hi_prec; prec++)
 303                 if (prec_bmp & (1 << prec))
 304                         len += pq->q[prec].len;
 305
 306         return len;
 307 }
 308 /* Priority dequeue from a specific set of precedences */
 309 void *BCMFASTPATH pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
 310 {
 311         struct pktq_prec *q;
 312         void *p;
 313         int prec;
 314
 315         if (pq->len == 0)
 316                 return NULL;
 317
 318         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 319                 pq->hi_prec--;
 320
 321         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 322                 if (prec-- == 0)
 323                         return NULL;
 324
 325         q = &pq->q[prec];
 326
 327         p = q->head;
 328         if (p == NULL)
 329                 return NULL;
 330
 331         q->head = PKTLINK(p);
 332         if (q->head == NULL)
 333                 q->tail = NULL;
 334
 335         q->len--;
 336
 337         if (prec_out)
 338                 *prec_out = prec;
 339
 340         pq->len--;
 341
 342         PKTSETLINK(p, NULL);
 343
 344         return p;
 345 }
 346
 347 /* parse a xx:xx:xx:xx:xx:xx format ethernet address */
 348 int bcm_ether_atoe(char *p, struct ether_addr *ea)
 349 {
 350         int i = 0;
 351
 352         for (;;) {
 353                 ea->octet[i++] = (char)simple_strtoul(p, &p, 16);
 354                 if (!*p++ || i == 6)
 355                         break;
 356         }
 357
 358         return i == 6;
 359 }
 360
 361 char *bcm_ether_ntoa(const struct ether_addr *ea, char *buf)
 362 {
 363         snprintf(buf, 18, "%pM", ea->octet);
 364         return buf;
 365 }
 366
 367 /*
 368  * Search the name=value vars for a specific one and return its value.
 369  * Returns NULL if not found.
 370  */
 371 char *getvar(char *vars, const char *name)
 372 {
 373         char *s;
 374         int len;
 375
 376         if (!name)
 377                 return NULL;
 378
 379         len = strlen(name);
 380         if (len == 0)
 381                 return NULL;
 382
 383         /* first look in vars[] */
 384         for (s = vars; s && *s;) {
 385                 if ((bcmp(s, name, len) == 0) && (s[len] == '='))
 386                         return &s[len + 1];
 387
 388                 while (*s++)
 389                         ;
 390         }
 391 #ifdef BRCM_FULLMAC
 392         return NULL;
 393 #else
 394         /* then query nvram */
 395         return nvram_get(name);
 396 #endif
 397 }
 398
 399 /*
 400  * Search the vars for a specific one and return its value as
 401  * an integer. Returns 0 if not found.
 402  */
 403 int getintvar(char *vars, const char *name)
 404 {
 405         char *val;
 406
 407         val = getvar(vars, name);
 408         if (val == NULL)
 409                 return 0;
 410
 411         return simple_strtoul(val, NULL, 0);
 412 }
 413
 414 #if defined(BCMDBG)
 415 /* pretty hex print a pkt buffer chain */
 416 void prpkt(const char *msg, osl_t *osh, void *p0)
 417 {
 418         void *p;
 419
 420         if (msg && (msg[0] != '\0'))
 421                 printf("%s:\n", msg);
 422
 423         for (p = p0; p; p = PKTNEXT(p))
 424                 prhex(NULL, PKTDATA(p), PKTLEN(p));
 425 }
 426 #endif                          /* defined(BCMDBG) */
 427
 428 /* Takes an Ethernet frame and sets out-of-bound PKTPRIO.
 429  * Also updates the inplace vlan tag if requested.
 430  * For debugging, it returns an indication of what it did.
 431  */
 432 uint pktsetprio(void *pkt, bool update_vtag)
 433 {
 434         struct ether_header *eh;
 435         struct ethervlan_header *evh;
 436         u8 *pktdata;
 437         int priority = 0;
 438         int rc = 0;
 439
 440         pktdata = (u8 *) PKTDATA(pkt);
 441         ASSERT(IS_ALIGNED((uintptr) pktdata, sizeof(u16)));
 442
 443         eh = (struct ether_header *)pktdata;
 444
 445         if (ntoh16(eh->ether_type) == ETHER_TYPE_8021Q) {
 446                 u16 vlan_tag;
 447                 int vlan_prio, dscp_prio = 0;
 448
 449                 evh = (struct ethervlan_header *)eh;
 450
 451                 vlan_tag = ntoh16(evh->vlan_tag);
 452                 vlan_prio = (int)(vlan_tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
 453
 454                 if (ntoh16(evh->ether_type) == ETHER_TYPE_IP) {
 455                         u8 *ip_body =
 456                             pktdata + sizeof(struct ethervlan_header);
 457                         u8 tos_tc = IP_TOS(ip_body);
 458                         dscp_prio = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
 459                 }
 460
 461                 /* DSCP priority gets precedence over 802.1P (vlan tag) */
 462                 if (dscp_prio != 0) {
 463                         priority = dscp_prio;
 464                         rc |= PKTPRIO_VDSCP;
 465                 } else {
 466                         priority = vlan_prio;
 467                         rc |= PKTPRIO_VLAN;
 468                 }
 469                 /*
 470                  * If the DSCP priority is not the same as the VLAN priority,
 471                  * then overwrite the priority field in the vlan tag, with the
 472                  * DSCP priority value. This is required for Linux APs because
 473                  * the VLAN driver on Linux, overwrites the skb->priority field
 474                  * with the priority value in the vlan tag
 475                  */
 476                 if (update_vtag && (priority != vlan_prio)) {
 477                         vlan_tag &= ~(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
 478                         vlan_tag |= (u16) priority << VLAN_PRI_SHIFT;
 479                         evh->vlan_tag = hton16(vlan_tag);
 480                         rc |= PKTPRIO_UPD;
 481                 }
 482         } else if (ntoh16(eh->ether_type) == ETHER_TYPE_IP) {
 483                 u8 *ip_body = pktdata + sizeof(struct ether_header);
 484                 u8 tos_tc = IP_TOS(ip_body);
 485                 priority = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
 486                 rc |= PKTPRIO_DSCP;
 487         }
 488
 489         ASSERT(priority >= 0 && priority <= MAXPRIO);
 490         PKTSETPRIO(pkt, priority);
 491         return rc | priority;
 492 }
 493
 494 static char bcm_undeferrstr[BCME_STRLEN];
 495
 496 static const char *bcmerrorstrtable[] = BCMERRSTRINGTABLE;
 497
 498 /* Convert the error codes into related error strings  */
 499 const char *bcmerrorstr(int bcmerror)
 500 {
 501         /* check if someone added a bcmerror code but
 502                  forgot to add errorstring */
 503         ASSERT(ABS(BCME_LAST) == (ARRAY_SIZE(bcmerrorstrtable) - 1));
 504
 505         if (bcmerror > 0 || bcmerror < BCME_LAST) {
 506                 snprintf(bcm_undeferrstr, BCME_STRLEN, "Undefined error %d",
 507                          bcmerror);
 508                 return bcm_undeferrstr;
 509         }
 510
 511         ASSERT(strlen(bcmerrorstrtable[-bcmerror]) < BCME_STRLEN);
 512
 513         return bcmerrorstrtable[-bcmerror];
 514 }
 515
 516 /* iovar table lookup */
 517 const bcm_iovar_t *bcm_iovar_lookup(const bcm_iovar_t *table, const char *name)
 518 {
 519         const bcm_iovar_t *vi;
 520         const char *lookup_name;
 521
 522         /* skip any ':' delimited option prefixes */
 523         lookup_name = strrchr(name, ':');
 524         if (lookup_name != NULL)
 525                 lookup_name++;
 526         else
 527                 lookup_name = name;
 528
 529         ASSERT(table != NULL);
 530
 531         for (vi = table; vi->name; vi++) {
 532                 if (!strcmp(vi->name, lookup_name))
 533                         return vi;
 534         }
 535         /* ran to end of table */
 536
 537         return NULL;            /* var name not found */
 538 }
 539
 540 int bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
 541 {
 542         int bcmerror = 0;
 543
 544         /* length check on io buf */
 545         switch (vi->type) {
 546         case IOVT_BOOL:
 547         case IOVT_INT8:
 548         case IOVT_INT16:
 549         case IOVT_INT32:
 550         case IOVT_UINT8:
 551         case IOVT_UINT16:
 552         case IOVT_UINT32:
 553                 /* all integers are s32 sized args at the ioctl interface */
 554                 if (len < (int)sizeof(int)) {
 555                         bcmerror = BCME_BUFTOOSHORT;
 556                 }
 557                 break;
 558
 559         case IOVT_BUFFER:
 560                 /* buffer must meet minimum length requirement */
 561                 if (len < vi->minlen) {
 562                         bcmerror = BCME_BUFTOOSHORT;
 563                 }
 564                 break;
 565
 566         case IOVT_VOID:
 567                 if (!set) {
 568                         /* Cannot return nil... */
 569                         bcmerror = BCME_UNSUPPORTED;
 570                 } else if (len) {
 571                         /* Set is an action w/o parameters */
 572                         bcmerror = BCME_BUFTOOLONG;
 573                 }
 574                 break;
 575
 576         default:
 577                 /* unknown type for length check in iovar info */
 578                 ASSERT(0);
 579                 bcmerror = BCME_UNSUPPORTED;
 580         }
 581
 582         return bcmerror;
 583 }
 584
 585 /*******************************************************************************
 586  * crc8
 587  *
 588  * Computes a crc8 over the input data using the polynomial:
 589  *
 590  *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
 591  *
 592  * The caller provides the initial value (either CRC8_INIT_VALUE
 593  * or the previous returned value) to allow for processing of
 594  * discontiguous blocks of data.  When generating the CRC the
 595  * caller is responsible for complementing the final return value
 596  * and inserting it into the byte stream.  When checking, a final
 597  * return value of CRC8_GOOD_VALUE indicates a valid CRC.
 598  *
 599  * Reference: Dallas Semiconductor Application Note 27
 600  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
 601  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
 602  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
 603  *
 604  * ****************************************************************************
 605  */
 606
 607 static const u8 crc8_table[256] = {
 608         0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
 609         0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
 610         0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
 611         0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
 612         0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
 613         0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
 614         0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
 615         0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
 616         0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
 617         0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
 618         0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
 619         0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
 620         0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
 621         0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
 622         0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
 623         0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
 624         0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
 625         0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
 626         0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
 627         0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
 628         0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
 629         0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
 630         0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
 631         0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
 632         0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
 633         0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
 634         0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
 635         0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
 636         0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
 637         0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
 638         0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
 639         0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
 640 };
 641
 642 #define CRC_INNER_LOOP(n, c, x) \
 643         ((c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff])
 644
 645 u8 hndcrc8(u8 *pdata,   /* pointer to array of data to process */
 646                          uint nbytes,   /* number of input data bytes to process */
 647                          u8 crc /* either CRC8_INIT_VALUE or previous return value */
 648     ) {
 649         /* hard code the crc loop instead of using CRC_INNER_LOOP macro
 650          * to avoid the undefined and unnecessary (u8 >> 8) operation.
 651          */
 652         while (nbytes-- > 0)
 653                 crc = crc8_table[(crc ^ *pdata++) & 0xff];
 654
 655         return crc;
 656 }
 657
 658 /*******************************************************************************
 659  * crc16
 660  *
 661  * Computes a crc16 over the input data using the polynomial:
 662  *
 663  *       x^16 + x^12 +x^5 + 1
 664  *
 665  * The caller provides the initial value (either CRC16_INIT_VALUE
 666  * or the previous returned value) to allow for processing of
 667  * discontiguous blocks of data.  When generating the CRC the
 668  * caller is responsible for complementing the final return value
 669  * and inserting it into the byte stream.  When checking, a final
 670  * return value of CRC16_GOOD_VALUE indicates a valid CRC.
 671  *
 672  * Reference: Dallas Semiconductor Application Note 27
 673  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
 674  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
 675  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
 676  *
 677  * ****************************************************************************
 678  */
 679
 680 static const u16 crc16_table[256] = {
 681         0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
 682         0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
 683         0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
 684         0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
 685         0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
 686         0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
 687         0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
 688         0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
 689         0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
 690         0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
 691         0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
 692         0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
 693         0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
 694         0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
 695         0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
 696         0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
 697         0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
 698         0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
 699         0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
 700         0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
 701         0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
 702         0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
 703         0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
 704         0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
 705         0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
 706         0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
 707         0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
 708         0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
 709         0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
 710         0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
 711         0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
 712         0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
 713 };
 714
 715 u16 hndcrc16(u8 *pdata, /* pointer to array of data to process */
 716         uint nbytes,    /* number of input data bytes to process */
 717         u16 crc /* either CRC16_INIT_VALUE or previous return value */
 718     ) {
 719         while (nbytes-- > 0)
 720                 CRC_INNER_LOOP(16, crc, *pdata++);
 721         return crc;
 722 }
 723
 724 static const u32 crc32_table[256] = {
 725         0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
 726         0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
 727         0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
 728         0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
 729         0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
 730         0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
 731         0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
 732         0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
 733         0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
 734         0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
 735         0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
 736         0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
 737         0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
 738         0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
 739         0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
 740         0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
 741         0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
 742         0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
 743         0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
 744         0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
 745         0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
 746         0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
 747         0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
 748         0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
 749         0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
 750         0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
 751         0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
 752         0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
 753         0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
 754         0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
 755         0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
 756         0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
 757         0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
 758         0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
 759         0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
 760         0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
 761         0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
 762         0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
 763         0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
 764         0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
 765         0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
 766         0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
 767         0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
 768         0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
 769         0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
 770         0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
 771         0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
 772         0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
 773         0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
 774         0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
 775         0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
 776         0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
 777         0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
 778         0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
 779         0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
 780         0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
 781         0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
 782         0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
 783         0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
 784         0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
 785         0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
 786         0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
 787         0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
 788         0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
 789 };
 790
 791 u32 hndcrc32(u8 *pdata, /* pointer to array of data to process */
 792                 uint nbytes,    /* number of input data bytes to process */
 793                 u32 crc /* either CRC32_INIT_VALUE or previous
 794                                          return value */
 795 )
 796 {
 797         u8 *pend;
 798 #ifdef __mips__
 799         u8 tmp[4];
 800         unsigned long *tptr = (unsigned long *) tmp;
 801
 802         /* in case the beginning of the buffer isn't aligned */
 803         pend = (u8 *) ((uint) (pdata + 3) & 0xfffffffc);
 804         nbytes -= (pend - pdata);
 805         while (pdata < pend)
 806                 CRC_INNER_LOOP(32, crc, *pdata++);
 807
 808         /* handle bulk of data as 32-bit words */
 809         pend = pdata + (nbytes & 0xfffffffc);
 810         while (pdata < pend) {
 811                 *tptr = *(unsigned long *) pdata;
 812                 pdata += sizeof(unsigned long *);
 813                 CRC_INNER_LOOP(32, crc, tmp[0]);
 814                 CRC_INNER_LOOP(32, crc, tmp[1]);
 815                 CRC_INNER_LOOP(32, crc, tmp[2]);
 816                 CRC_INNER_LOOP(32, crc, tmp[3]);
 817         }
 818
 819         /* 1-3 bytes at end of buffer */
 820         pend = pdata + (nbytes & 0x03);
 821         while (pdata < pend)
 822                 CRC_INNER_LOOP(32, crc, *pdata++);
 823 #else
 824         pend = pdata + nbytes;
 825         while (pdata < pend)
 826                 CRC_INNER_LOOP(32, crc, *pdata++);
 827 #endif                          /* __mips__ */
 828
 829         return crc;
 830 }
 831 /*
 832  * Traverse a string of 1-byte tag/1-byte length/variable-length value
 833  * triples, returning a pointer to the substring whose first element
 834  * matches tag
 835  */
 836 bcm_tlv_t *bcm_parse_tlvs(void *buf, int buflen, uint key)
 837 {
 838         bcm_tlv_t *elt;
 839         int totlen;
 840
 841         elt = (bcm_tlv_t *) buf;
 842         totlen = buflen;
 843
 844         /* find tagged parameter */
 845         while (totlen >= 2) {
 846                 int len = elt->len;
 847
 848                 /* validate remaining totlen */
 849                 if ((elt->id == key) && (totlen >= (len + 2)))
 850                         return elt;
 851
 852                 elt = (bcm_tlv_t *) ((u8 *) elt + (len + 2));
 853                 totlen -= (len + 2);
 854         }
 855
 856         return NULL;
 857 }
 858
 859
 860 #if defined(BCMDBG)
 861 int
 862 bcm_format_flags(const bcm_bit_desc_t *bd, u32 flags, char *buf, int len)
 863 {
 864         int i;
 865         char *p = buf;
 866         char hexstr[16];
 867         int slen = 0, nlen = 0;
 868         u32 bit;
 869         const char *name;
 870
 871         if (len < 2 || !buf)
 872                 return 0;
 873
 874         buf[0] = '\0';
 875
 876         for (i = 0; flags != 0; i++) {
 877                 bit = bd[i].bit;
 878                 name = bd[i].name;
 879                 if (bit == 0 && flags != 0) {
 880                         /* print any unnamed bits */
 881                         snprintf(hexstr, 16, "0x%X", flags);
 882                         name = hexstr;
 883                         flags = 0;      /* exit loop */
 884                 } else if ((flags & bit) == 0)
 885                         continue;
 886                 flags &= ~bit;
 887                 nlen = strlen(name);
 888                 slen += nlen;
 889                 /* count btwn flag space */
 890                 if (flags != 0)
 891                         slen += 1;
 892                 /* need NULL char as well */
 893                 if (len <= slen)
 894                         break;
 895                 /* copy NULL char but don't count it */
 896                 strncpy(p, name, nlen + 1);
 897                 p += nlen;
 898                 /* copy btwn flag space and NULL char */
 899                 if (flags != 0)
 900                         p += snprintf(p, 2, " ");
 901                 len -= slen;
 902         }
 903
 904         /* indicate the str was too short */
 905         if (flags != 0) {
 906                 if (len < 2)
 907                         p -= 2 - len;   /* overwrite last char */
 908                 p += snprintf(p, 2, ">");
 909         }
 910
 911         return (int)(p - buf);
 912 }
 913
 914 /* print bytes formatted as hex to a string. return the resulting string length */
 915 int bcm_format_hex(char *str, const void *bytes, int len)
 916 {
 917         int i;
 918         char *p = str;
 919         const u8 *src = (const u8 *)bytes;
 920
 921         for (i = 0; i < len; i++) {
 922                 p += snprintf(p, 3, "%02X", *src);
 923                 src++;
 924         }
 925         return (int)(p - str);
 926 }
 927 #endif                          /* defined(BCMDBG) */
 928
 929 /* pretty hex print a contiguous buffer */
 930 void prhex(const char *msg, unsigned char *buf, uint nbytes)
 931 {
 932         char line[128], *p;
 933         int len = sizeof(line);
 934         int nchar;
 935         uint i;
 936
 937         if (msg && (msg[0] != '\0'))
 938                 printf("%s:\n", msg);
 939
 940         p = line;
 941         for (i = 0; i < nbytes; i++) {
 942                 if (i % 16 == 0) {
 943                         nchar = snprintf(p, len, "  %04d: ", i);        /* line prefix */
 944                         p += nchar;
 945                         len -= nchar;
 946                 }
 947                 if (len > 0) {
 948                         nchar = snprintf(p, len, "%02x ", buf[i]);
 949                         p += nchar;
 950                         len -= nchar;
 951                 }
 952
 953                 if (i % 16 == 15) {
 954                         printf("%s\n", line);   /* flush line */
 955                         p = line;
 956                         len = sizeof(line);
 957                 }
 958         }
 959
 960         /* flush last partial line */
 961         if (p != line)
 962                 printf("%s\n", line);
 963 }
 964
 965 char *bcm_chipname(uint chipid, char *buf, uint len)
 966 {
 967         const char *fmt;
 968
 969         fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
 970         snprintf(buf, len, fmt, chipid);
 971         return buf;
 972 }
 973
 974 uint bcm_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
 975 {
 976         uint len;
 977
 978         len = strlen(name) + 1;
 979
 980         if ((len + datalen) > buflen)
 981                 return 0;
 982
 983         strncpy(buf, name, buflen);
 984
 985         /* append data onto the end of the name string */
 986         memcpy(&buf[len], data, datalen);
 987         len += datalen;
 988
 989         return len;
 990 }
 991
 992 /* Quarter dBm units to mW
 993  * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
 994  * Table is offset so the last entry is largest mW value that fits in
 995  * a u16.
 996  */
 997
 998 #define QDBM_OFFSET 153         /* Offset for first entry */
 999 #define QDBM_TABLE_LEN 40       /* Table size */
1000
1001 /* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
1002  * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
1003  */
1004 #define QDBM_TABLE_LOW_BOUND 6493       /* Low bound */
1005
1006 /* Largest mW value that will round down to the last table entry,
1007  * QDBM_OFFSET + QDBM_TABLE_LEN-1.
1008  * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) +
1009  * mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
1010  */
1011 #define QDBM_TABLE_HIGH_BOUND 64938     /* High bound */
1012
1013 static const u16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
1014 /* qdBm:        +0      +1      +2      +3      +4      +5      +6      +7 */
1015 /* 153: */ 6683, 7079, 7499, 7943, 8414, 8913, 9441, 10000,
1016 /* 161: */ 10593, 11220, 11885, 12589, 13335, 14125, 14962, 15849,
1017 /* 169: */ 16788, 17783, 18836, 19953, 21135, 22387, 23714, 25119,
1018 /* 177: */ 26607, 28184, 29854, 31623, 33497, 35481, 37584, 39811,
1019 /* 185: */ 42170, 44668, 47315, 50119, 53088, 56234, 59566, 63096
1020 };
1021
1022 u16 bcm_qdbm_to_mw(u8 qdbm)
1023 {
1024         uint factor = 1;
1025         int idx = qdbm - QDBM_OFFSET;
1026
1027         if (idx >= QDBM_TABLE_LEN) {
1028                 /* clamp to max u16 mW value */
1029                 return 0xFFFF;
1030         }
1031
1032         /* scale the qdBm index up to the range of the table 0-40
1033          * where an offset of 40 qdBm equals a factor of 10 mW.
1034          */
1035         while (idx < 0) {
1036                 idx += 40;
1037                 factor *= 10;
1038         }
1039
1040         /* return the mW value scaled down to the correct factor of 10,
1041          * adding in factor/2 to get proper rounding.
1042          */
1043         return (nqdBm_to_mW_map[idx] + factor / 2) / factor;
1044 }
1045 u8 bcm_mw_to_qdbm(u16 mw)
1046 {
1047         u8 qdbm;
1048         int offset;
1049         uint mw_uint = mw;
1050         uint boundary;
1051
1052         /* handle boundary case */
1053         if (mw_uint <= 1)
1054                 return 0;
1055
1056         offset = QDBM_OFFSET;
1057
1058         /* move mw into the range of the table */
1059         while (mw_uint < QDBM_TABLE_LOW_BOUND) {
1060                 mw_uint *= 10;
1061                 offset -= 40;
1062         }
1063
1064         for (qdbm = 0; qdbm < QDBM_TABLE_LEN - 1; qdbm++) {
1065                 boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm + 1] -
1066                                                     nqdBm_to_mW_map[qdbm]) / 2;
1067                 if (mw_uint < boundary)
1068                         break;
1069         }
1070
1071         qdbm += (u8) offset;
1072
1073         return qdbm;
1074 }
1075 uint bcm_bitcount(u8 *bitmap, uint length)
1076 {
1077         uint bitcount = 0, i;
1078         u8 tmp;
1079         for (i = 0; i < length; i++) {
1080                 tmp = bitmap[i];
1081                 while (tmp) {
1082                         bitcount++;
1083                         tmp &= (tmp - 1);
1084                 }
1085         }
1086         return bitcount;
1087 }
1088 /* Initialization of bcmstrbuf structure */
1089 void bcm_binit(struct bcmstrbuf *b, char *buf, uint size)
1090 {
1091         b->origsize = b->size = size;
1092         b->origbuf = b->buf = buf;
1093 }
1094
1095 /* Buffer sprintf wrapper to guard against buffer overflow */
1096 int bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...)
1097 {
1098         va_list ap;
1099         int r;
1100
1101         va_start(ap, fmt);
1102         r = vsnprintf(b->buf, b->size, fmt, ap);
1103
1104         /* Non Ansi C99 compliant returns -1,
1105          * Ansi compliant return r >= b->size,
1106          * bcmstdlib returns 0, handle all
1107          */
1108         if ((r == -1) || (r >= (int)b->size) || (r == 0)) {
1109                 b->size = 0;
1110         } else {
1111                 b->size -= r;
1112                 b->buf += r;
1113         }
1114
1115         va_end(ap);
1116
1117         return r;
1118 }
1119
1120 /****************************************************************************
1121 * Function:   bcmstrtok
1122 *
1123 * Purpose:
1124 *  Tokenizes a string. This function is conceptually similiar to ANSI C strtok(),
1125 *  but allows strToken() to be used by different strings or callers at the same
1126 *  time. Each call modifies '*string' by substituting a NULL character for the
1127 *  first delimiter that is encountered, and updates 'string' to point to the char
1128 *  after the delimiter. Leading delimiters are skipped.
1129 *
1130 * Parameters:
1131 *  string      (mod) Ptr to string ptr, updated by token.
1132 *  delimiters  (in)  Set of delimiter characters.
1133 *  tokdelim    (out) Character that delimits the returned token. (May
1134 *                    be set to NULL if token delimiter is not required).
1135 *
1136 * Returns:  Pointer to the next token found. NULL when no more tokens are found.
1137 *****************************************************************************
1138 */
1139 char *bcmstrtok(char **string, const char *delimiters, char *tokdelim)
1140 {
1141         unsigned char *str;
1142         unsigned long map[8];
1143         int count;
1144         char *nextoken;
1145
1146         if (tokdelim != NULL) {
1147                 /* Prime the token delimiter */
1148                 *tokdelim = '\0';
1149         }
1150
1151         /* Clear control map */
1152         for (count = 0; count < 8; count++) {
1153                 map[count] = 0;
1154         }
1155
1156         /* Set bits in delimiter table */
1157         do {
1158                 map[*delimiters >> 5] |= (1 << (*delimiters & 31));
1159         } while (*delimiters++);
1160
1161         str = (unsigned char *)*string;
1162
1163         /* Find beginning of token (skip over leading delimiters). Note that
1164          * there is no token iff this loop sets str to point to the terminal
1165          * null (*str == '\0')
1166          */
1167         while (((map[*str >> 5] & (1 << (*str & 31))) && *str) || (*str == ' ')) {
1168                 str++;
1169         }
1170
1171         nextoken = (char *)str;
1172
1173         /* Find the end of the token. If it is not the end of the string,
1174          * put a null there.
1175          */
1176         for (; *str; str++) {
1177                 if (map[*str >> 5] & (1 << (*str & 31))) {
1178                         if (tokdelim != NULL) {
1179                                 *tokdelim = *str;
1180                         }
1181
1182                         *str++ = '\0';
1183                         break;
1184                 }
1185         }
1186
1187         *string = (char *)str;
1188
1189         /* Determine if a token has been found. */
1190         if (nextoken == (char *)str) {
1191                 return NULL;
1192         } else {
1193                 return nextoken;
1194         }
1195 }