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/802.1d.h>
  32 #include <proto/802.11.h>
  33
  34
  35 /* return total length of buffer chain */
  36 uint BCMFASTPATH pkttotlen(osl_t *osh, void *p)
  37 {
  38         uint total;
  39
  40         total = 0;
  41         for (; p; p = PKTNEXT(p))
  42                 total += PKTLEN(p);
  43         return total;
  44 }
  45
  46 /*
  47  * osl multiple-precedence packet queue
  48  * hi_prec is always >= the number of the highest non-empty precedence
  49  */
  50 void *BCMFASTPATH pktq_penq(struct pktq *pq, int prec, void *p)
  51 {
  52         struct pktq_prec *q;
  53
  54         ASSERT(prec >= 0 && prec < pq->num_prec);
  55         ASSERT(PKTLINK(p) == NULL);     /* queueing chains not allowed */
  56
  57         ASSERT(!pktq_full(pq));
  58         ASSERT(!pktq_pfull(pq, prec));
  59
  60         q = &pq->q[prec];
  61
  62         if (q->head)
  63                 PKTSETLINK(q->tail, p);
  64         else
  65                 q->head = p;
  66
  67         q->tail = p;
  68         q->len++;
  69
  70         pq->len++;
  71
  72         if (pq->hi_prec < prec)
  73                 pq->hi_prec = (u8) prec;
  74
  75         return p;
  76 }
  77
  78 void *BCMFASTPATH pktq_penq_head(struct pktq *pq, int prec, void *p)
  79 {
  80         struct pktq_prec *q;
  81
  82         ASSERT(prec >= 0 && prec < pq->num_prec);
  83         ASSERT(PKTLINK(p) == NULL);     /* queueing chains not allowed */
  84
  85         ASSERT(!pktq_full(pq));
  86         ASSERT(!pktq_pfull(pq, prec));
  87
  88         q = &pq->q[prec];
  89
  90         if (q->head == NULL)
  91                 q->tail = p;
  92
  93         PKTSETLINK(p, q->head);
  94         q->head = p;
  95         q->len++;
  96
  97         pq->len++;
  98
  99         if (pq->hi_prec < prec)
 100                 pq->hi_prec = (u8) prec;
 101
 102         return p;
 103 }
 104
 105 void *BCMFASTPATH pktq_pdeq(struct pktq *pq, int prec)
 106 {
 107         struct pktq_prec *q;
 108         void *p;
 109
 110         ASSERT(prec >= 0 && prec < pq->num_prec);
 111
 112         q = &pq->q[prec];
 113
 114         p = q->head;
 115         if (p == NULL)
 116                 return NULL;
 117
 118         q->head = PKTLINK(p);
 119         if (q->head == NULL)
 120                 q->tail = NULL;
 121
 122         q->len--;
 123
 124         pq->len--;
 125
 126         PKTSETLINK(p, NULL);
 127
 128         return p;
 129 }
 130
 131 void *BCMFASTPATH pktq_pdeq_tail(struct pktq *pq, int prec)
 132 {
 133         struct pktq_prec *q;
 134         void *p, *prev;
 135
 136         ASSERT(prec >= 0 && prec < pq->num_prec);
 137
 138         q = &pq->q[prec];
 139
 140         p = q->head;
 141         if (p == NULL)
 142                 return NULL;
 143
 144         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 145                 prev = p;
 146
 147         if (prev)
 148                 PKTSETLINK(prev, NULL);
 149         else
 150                 q->head = NULL;
 151
 152         q->tail = prev;
 153         q->len--;
 154
 155         pq->len--;
 156
 157         return p;
 158 }
 159
 160 void
 161 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir, ifpkt_cb_t fn,
 162             int arg)
 163 {
 164         struct pktq_prec *q;
 165         void *p, *prev = NULL;
 166
 167         q = &pq->q[prec];
 168         p = q->head;
 169         while (p) {
 170                 if (fn == NULL || (*fn) (p, arg)) {
 171                         bool head = (p == q->head);
 172                         if (head)
 173                                 q->head = PKTLINK(p);
 174                         else
 175                                 PKTSETLINK(prev, PKTLINK(p));
 176                         PKTSETLINK(p, NULL);
 177                         PKTFREE(osh, p, dir);
 178                         q->len--;
 179                         pq->len--;
 180                         p = (head ? q->head : PKTLINK(prev));
 181                 } else {
 182                         prev = p;
 183                         p = PKTLINK(p);
 184                 }
 185         }
 186
 187         if (q->head == NULL) {
 188                 ASSERT(q->len == 0);
 189                 q->tail = NULL;
 190         }
 191 }
 192
 193 void pktq_init(struct pktq *pq, int num_prec, int max_len)
 194 {
 195         int prec;
 196
 197         ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
 198
 199         /* pq is variable size; only zero out what's requested */
 200         bzero(pq,
 201               OFFSETOF(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 202
 203         pq->num_prec = (u16) num_prec;
 204
 205         pq->max = (u16) max_len;
 206
 207         for (prec = 0; prec < num_prec; prec++)
 208                 pq->q[prec].max = pq->max;
 209 }
 210
 211 void *pktq_peek_tail(struct pktq *pq, int *prec_out)
 212 {
 213         int prec;
 214
 215         if (pq->len == 0)
 216                 return NULL;
 217
 218         for (prec = 0; prec < pq->hi_prec; prec++)
 219                 if (pq->q[prec].head)
 220                         break;
 221
 222         if (prec_out)
 223                 *prec_out = prec;
 224
 225         return pq->q[prec].tail;
 226 }
 227
 228 void pktq_flush(osl_t *osh, struct pktq *pq, bool dir, ifpkt_cb_t fn, int arg)
 229 {
 230         int prec;
 231         for (prec = 0; prec < pq->num_prec; prec++)
 232                 pktq_pflush(osh, pq, prec, dir, fn, arg);
 233         if (fn == NULL)
 234                 ASSERT(pq->len == 0);
 235 }
 236
 237 /* Priority dequeue from a specific set of precedences */
 238 void *BCMFASTPATH pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
 239 {
 240         struct pktq_prec *q;
 241         void *p;
 242         int prec;
 243
 244         if (pq->len == 0)
 245                 return NULL;
 246
 247         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 248                 pq->hi_prec--;
 249
 250         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 251                 if (prec-- == 0)
 252                         return NULL;
 253
 254         q = &pq->q[prec];
 255
 256         p = q->head;
 257         if (p == NULL)
 258                 return NULL;
 259
 260         q->head = PKTLINK(p);
 261         if (q->head == NULL)
 262                 q->tail = NULL;
 263
 264         q->len--;
 265
 266         if (prec_out)
 267                 *prec_out = prec;
 268
 269         pq->len--;
 270
 271         PKTSETLINK(p, NULL);
 272
 273         return p;
 274 }
 275
 276 /* parse a xx:xx:xx:xx:xx:xx format ethernet address */
 277 int BCMROMFN(bcm_ether_atoe) (char *p, struct ether_addr *ea)
 278 {
 279         int i = 0;
 280
 281         for (;;) {
 282                 ea->octet[i++] = (char)simple_strtoul(p, &p, 16);
 283                 if (!*p++ || i == 6)
 284                         break;
 285         }
 286
 287         return i == 6;
 288 }
 289
 290 char *bcm_ether_ntoa(const struct ether_addr *ea, char *buf)
 291 {
 292         snprintf(buf, 18, "%pM", ea->octet);
 293         return buf;
 294 }
 295
 296 /*
 297  * Search the name=value vars for a specific one and return its value.
 298  * Returns NULL if not found.
 299  */
 300 char *getvar(char *vars, const char *name)
 301 {
 302         char *s;
 303         int len;
 304
 305         if (!name)
 306                 return NULL;
 307
 308         len = strlen(name);
 309         if (len == 0)
 310                 return NULL;
 311
 312         /* first look in vars[] */
 313         for (s = vars; s && *s;) {
 314                 if ((bcmp(s, name, len) == 0) && (s[len] == '='))
 315                         return &s[len + 1];
 316
 317                 while (*s++)
 318                         ;
 319         }
 320
 321         /* then query nvram */
 322         return nvram_get(name);
 323 }
 324
 325 /*
 326  * Search the vars for a specific one and return its value as
 327  * an integer. Returns 0 if not found.
 328  */
 329 int getintvar(char *vars, const char *name)
 330 {
 331         char *val;
 332
 333         val = getvar(vars, name);
 334         if (val == NULL)
 335                 return 0;
 336
 337         return simple_strtoul(val, NULL, 0);
 338 }
 339
 340 #if defined(BCMDBG)
 341 /* pretty hex print a pkt buffer chain */
 342 void prpkt(const char *msg, osl_t *osh, void *p0)
 343 {
 344         void *p;
 345
 346         if (msg && (msg[0] != '\0'))
 347                 printf("%s:\n", msg);
 348
 349         for (p = p0; p; p = PKTNEXT(p))
 350                 prhex(NULL, PKTDATA(p), PKTLEN(p));
 351 }
 352 #endif                          /* defined(BCMDBG) */
 353
 354 int bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
 355 {
 356         int bcmerror = 0;
 357
 358         /* length check on io buf */
 359         switch (vi->type) {
 360         case IOVT_BOOL:
 361         case IOVT_INT8:
 362         case IOVT_INT16:
 363         case IOVT_INT32:
 364         case IOVT_UINT8:
 365         case IOVT_UINT16:
 366         case IOVT_UINT32:
 367                 /* all integers are s32 sized args at the ioctl interface */
 368                 if (len < (int)sizeof(int)) {
 369                         bcmerror = BCME_BUFTOOSHORT;
 370                 }
 371                 break;
 372
 373         case IOVT_BUFFER:
 374                 /* buffer must meet minimum length requirement */
 375                 if (len < vi->minlen) {
 376                         bcmerror = BCME_BUFTOOSHORT;
 377                 }
 378                 break;
 379
 380         case IOVT_VOID:
 381                 if (!set) {
 382                         /* Cannot return nil... */
 383                         bcmerror = BCME_UNSUPPORTED;
 384                 } else if (len) {
 385                         /* Set is an action w/o parameters */
 386                         bcmerror = BCME_BUFTOOLONG;
 387                 }
 388                 break;
 389
 390         default:
 391                 /* unknown type for length check in iovar info */
 392                 ASSERT(0);
 393                 bcmerror = BCME_UNSUPPORTED;
 394         }
 395
 396         return bcmerror;
 397 }
 398
 399 /*******************************************************************************
 400  * crc8
 401  *
 402  * Computes a crc8 over the input data using the polynomial:
 403  *
 404  *       x^8 + x^7 +x^6 + x^4 + x^2 + 1
 405  *
 406  * The caller provides the initial value (either CRC8_INIT_VALUE
 407  * or the previous returned value) to allow for processing of
 408  * discontiguous blocks of data.  When generating the CRC the
 409  * caller is responsible for complementing the final return value
 410  * and inserting it into the byte stream.  When checking, a final
 411  * return value of CRC8_GOOD_VALUE indicates a valid CRC.
 412  *
 413  * Reference: Dallas Semiconductor Application Note 27
 414  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
 415  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
 416  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
 417  *
 418  * ****************************************************************************
 419  */
 420
 421 static const u8 crc8_table[256] = {
 422         0x00, 0xF7, 0xB9, 0x4E, 0x25, 0xD2, 0x9C, 0x6B,
 423         0x4A, 0xBD, 0xF3, 0x04, 0x6F, 0x98, 0xD6, 0x21,
 424         0x94, 0x63, 0x2D, 0xDA, 0xB1, 0x46, 0x08, 0xFF,
 425         0xDE, 0x29, 0x67, 0x90, 0xFB, 0x0C, 0x42, 0xB5,
 426         0x7F, 0x88, 0xC6, 0x31, 0x5A, 0xAD, 0xE3, 0x14,
 427         0x35, 0xC2, 0x8C, 0x7B, 0x10, 0xE7, 0xA9, 0x5E,
 428         0xEB, 0x1C, 0x52, 0xA5, 0xCE, 0x39, 0x77, 0x80,
 429         0xA1, 0x56, 0x18, 0xEF, 0x84, 0x73, 0x3D, 0xCA,
 430         0xFE, 0x09, 0x47, 0xB0, 0xDB, 0x2C, 0x62, 0x95,
 431         0xB4, 0x43, 0x0D, 0xFA, 0x91, 0x66, 0x28, 0xDF,
 432         0x6A, 0x9D, 0xD3, 0x24, 0x4F, 0xB8, 0xF6, 0x01,
 433         0x20, 0xD7, 0x99, 0x6E, 0x05, 0xF2, 0xBC, 0x4B,
 434         0x81, 0x76, 0x38, 0xCF, 0xA4, 0x53, 0x1D, 0xEA,
 435         0xCB, 0x3C, 0x72, 0x85, 0xEE, 0x19, 0x57, 0xA0,
 436         0x15, 0xE2, 0xAC, 0x5B, 0x30, 0xC7, 0x89, 0x7E,
 437         0x5F, 0xA8, 0xE6, 0x11, 0x7A, 0x8D, 0xC3, 0x34,
 438         0xAB, 0x5C, 0x12, 0xE5, 0x8E, 0x79, 0x37, 0xC0,
 439         0xE1, 0x16, 0x58, 0xAF, 0xC4, 0x33, 0x7D, 0x8A,
 440         0x3F, 0xC8, 0x86, 0x71, 0x1A, 0xED, 0xA3, 0x54,
 441         0x75, 0x82, 0xCC, 0x3B, 0x50, 0xA7, 0xE9, 0x1E,
 442         0xD4, 0x23, 0x6D, 0x9A, 0xF1, 0x06, 0x48, 0xBF,
 443         0x9E, 0x69, 0x27, 0xD0, 0xBB, 0x4C, 0x02, 0xF5,
 444         0x40, 0xB7, 0xF9, 0x0E, 0x65, 0x92, 0xDC, 0x2B,
 445         0x0A, 0xFD, 0xB3, 0x44, 0x2F, 0xD8, 0x96, 0x61,
 446         0x55, 0xA2, 0xEC, 0x1B, 0x70, 0x87, 0xC9, 0x3E,
 447         0x1F, 0xE8, 0xA6, 0x51, 0x3A, 0xCD, 0x83, 0x74,
 448         0xC1, 0x36, 0x78, 0x8F, 0xE4, 0x13, 0x5D, 0xAA,
 449         0x8B, 0x7C, 0x32, 0xC5, 0xAE, 0x59, 0x17, 0xE0,
 450         0x2A, 0xDD, 0x93, 0x64, 0x0F, 0xF8, 0xB6, 0x41,
 451         0x60, 0x97, 0xD9, 0x2E, 0x45, 0xB2, 0xFC, 0x0B,
 452         0xBE, 0x49, 0x07, 0xF0, 0x9B, 0x6C, 0x22, 0xD5,
 453         0xF4, 0x03, 0x4D, 0xBA, 0xD1, 0x26, 0x68, 0x9F
 454 };
 455
 456 #define CRC_INNER_LOOP(n, c, x) \
 457         ((c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff])
 458
 459 u8 BCMROMFN(hndcrc8) (u8 *pdata,        /* pointer to array of data to process */
 460                          uint nbytes,   /* number of input data bytes to process */
 461                          u8 crc /* either CRC8_INIT_VALUE or previous return value */
 462     ) {
 463         /* hard code the crc loop instead of using CRC_INNER_LOOP macro
 464          * to avoid the undefined and unnecessary (u8 >> 8) operation.
 465          */
 466         while (nbytes-- > 0)
 467                 crc = crc8_table[(crc ^ *pdata++) & 0xff];
 468
 469         return crc;
 470 }
 471
 472 /*******************************************************************************
 473  * crc16
 474  *
 475  * Computes a crc16 over the input data using the polynomial:
 476  *
 477  *       x^16 + x^12 +x^5 + 1
 478  *
 479  * The caller provides the initial value (either CRC16_INIT_VALUE
 480  * or the previous returned value) to allow for processing of
 481  * discontiguous blocks of data.  When generating the CRC the
 482  * caller is responsible for complementing the final return value
 483  * and inserting it into the byte stream.  When checking, a final
 484  * return value of CRC16_GOOD_VALUE indicates a valid CRC.
 485  *
 486  * Reference: Dallas Semiconductor Application Note 27
 487  *   Williams, Ross N., "A Painless Guide to CRC Error Detection Algorithms",
 488  *     ver 3, Aug 1993, ross@guest.adelaide.edu.au, Rocksoft Pty Ltd.,
 489  *     ftp://ftp.rocksoft.com/clients/rocksoft/papers/crc_v3.txt
 490  *
 491  * ****************************************************************************
 492  */
 493
 494 static const u16 crc16_table[256] = {
 495         0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
 496         0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
 497         0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
 498         0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
 499         0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
 500         0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
 501         0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
 502         0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
 503         0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
 504         0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
 505         0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
 506         0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
 507         0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
 508         0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
 509         0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
 510         0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
 511         0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
 512         0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
 513         0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
 514         0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
 515         0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
 516         0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
 517         0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
 518         0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
 519         0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
 520         0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
 521         0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
 522         0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
 523         0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
 524         0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
 525         0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
 526         0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
 527 };
 528
 529 u16 BCMROMFN(hndcrc16) (u8 *pdata,      /* pointer to array of data to process */
 530                            uint nbytes, /* number of input data bytes to process */
 531                            u16 crc      /* either CRC16_INIT_VALUE or previous return value */
 532     ) {
 533         while (nbytes-- > 0)
 534                 CRC_INNER_LOOP(16, crc, *pdata++);
 535         return crc;
 536 }
 537
 538 /*
 539  * Traverse a string of 1-byte tag/1-byte length/variable-length value
 540  * triples, returning a pointer to the substring whose first element
 541  * matches tag
 542  */
 543 bcm_tlv_t *BCMROMFN(bcm_parse_tlvs) (void *buf, int buflen, uint key)
 544 {
 545         bcm_tlv_t *elt;
 546         int totlen;
 547
 548         elt = (bcm_tlv_t *) buf;
 549         totlen = buflen;
 550
 551         /* find tagged parameter */
 552         while (totlen >= 2) {
 553                 int len = elt->len;
 554
 555                 /* validate remaining totlen */
 556                 if ((elt->id == key) && (totlen >= (len + 2)))
 557                         return elt;
 558
 559                 elt = (bcm_tlv_t *) ((u8 *) elt + (len + 2));
 560                 totlen -= (len + 2);
 561         }
 562
 563         return NULL;
 564 }
 565
 566
 567 #if defined(BCMDBG)
 568 int
 569 bcm_format_flags(const bcm_bit_desc_t *bd, u32 flags, char *buf, int len)
 570 {
 571         int i;
 572         char *p = buf;
 573         char hexstr[16];
 574         int slen = 0, nlen = 0;
 575         u32 bit;
 576         const char *name;
 577
 578         if (len < 2 || !buf)
 579                 return 0;
 580
 581         buf[0] = '\0';
 582
 583         for (i = 0; flags != 0; i++) {
 584                 bit = bd[i].bit;
 585                 name = bd[i].name;
 586                 if (bit == 0 && flags != 0) {
 587                         /* print any unnamed bits */
 588                         snprintf(hexstr, 16, "0x%X", flags);
 589                         name = hexstr;
 590                         flags = 0;      /* exit loop */
 591                 } else if ((flags & bit) == 0)
 592                         continue;
 593                 flags &= ~bit;
 594                 nlen = strlen(name);
 595                 slen += nlen;
 596                 /* count btwn flag space */
 597                 if (flags != 0)
 598                         slen += 1;
 599                 /* need NULL char as well */
 600                 if (len <= slen)
 601                         break;
 602                 /* copy NULL char but don't count it */
 603                 strncpy(p, name, nlen + 1);
 604                 p += nlen;
 605                 /* copy btwn flag space and NULL char */
 606                 if (flags != 0)
 607                         p += snprintf(p, 2, " ");
 608                 len -= slen;
 609         }
 610
 611         /* indicate the str was too short */
 612         if (flags != 0) {
 613                 if (len < 2)
 614                         p -= 2 - len;   /* overwrite last char */
 615                 p += snprintf(p, 2, ">");
 616         }
 617
 618         return (int)(p - buf);
 619 }
 620
 621 /* print bytes formatted as hex to a string. return the resulting string length */
 622 int bcm_format_hex(char *str, const void *bytes, int len)
 623 {
 624         int i;
 625         char *p = str;
 626         const u8 *src = (const u8 *)bytes;
 627
 628         for (i = 0; i < len; i++) {
 629                 p += snprintf(p, 3, "%02X", *src);
 630                 src++;
 631         }
 632         return (int)(p - str);
 633 }
 634 #endif                          /* defined(BCMDBG) */
 635
 636 /* pretty hex print a contiguous buffer */
 637 void prhex(const char *msg, unsigned char *buf, uint nbytes)
 638 {
 639         char line[128], *p;
 640         int len = sizeof(line);
 641         int nchar;
 642         uint i;
 643
 644         if (msg && (msg[0] != '\0'))
 645                 printf("%s:\n", msg);
 646
 647         p = line;
 648         for (i = 0; i < nbytes; i++) {
 649                 if (i % 16 == 0) {
 650                         nchar = snprintf(p, len, "  %04d: ", i);        /* line prefix */
 651                         p += nchar;
 652                         len -= nchar;
 653                 }
 654                 if (len > 0) {
 655                         nchar = snprintf(p, len, "%02x ", buf[i]);
 656                         p += nchar;
 657                         len -= nchar;
 658                 }
 659
 660                 if (i % 16 == 15) {
 661                         printf("%s\n", line);   /* flush line */
 662                         p = line;
 663                         len = sizeof(line);
 664                 }
 665         }
 666
 667         /* flush last partial line */
 668         if (p != line)
 669                 printf("%s\n", line);
 670 }
 671
 672 char *bcm_chipname(uint chipid, char *buf, uint len)
 673 {
 674         const char *fmt;
 675
 676         fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
 677         snprintf(buf, len, fmt, chipid);
 678         return buf;
 679 }
 680
 681 uint BCMROMFN(bcm_bitcount) (u8 *bitmap, uint length)
 682 {
 683         uint bitcount = 0, i;
 684         u8 tmp;
 685         for (i = 0; i < length; i++) {
 686                 tmp = bitmap[i];
 687                 while (tmp) {
 688                         bitcount++;
 689                         tmp &= (tmp - 1);
 690                 }
 691         }
 692         return bitcount;
 693 }