package/broadcom-wl/src/kmod/bcmutils.c

   1 /*
   2  * Misc useful OS-independent routines.
   3  *
   4  * Copyright 2006, Broadcom Corporation
   5  * All Rights Reserved.
   6  *
   7  * THIS SOFTWARE IS OFFERED "AS IS", AND BROADCOM GRANTS NO WARRANTIES OF ANY
   8  * KIND, EXPRESS OR IMPLIED, BY STATUTE, COMMUNICATION OR OTHERWISE. BROADCOM
   9  * SPECIFICALLY DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS
  10  * FOR A SPECIFIC PURPOSE OR NONINFRINGEMENT CONCERNING THIS SOFTWARE.
  11  * $Id: bcmutils.c,v 1.1.1.12 2006/02/27 03:43:16 honor Exp $
  12  */
  13
  14 #include <typedefs.h>
  15 #include <bcmdefs.h>
  16 #include <stdarg.h>
  17 #include <osl.h>
  18 #include "linux_osl.h"
  19 #include "pktq.h"
  20 #include <bcmutils.h>
  21 #include <sbutils.h>
  22 #include <bcmnvram.h>
  23 #include <bcmendian.h>
  24 #include <bcmdevs.h>
  25 #include "bcmip.h"
  26
  27 #define ETHER_TYPE_8021Q       0x8100
  28 #define ETHER_TYPE_IP          0x0800
  29 #define VLAN_PRI_SHIFT             13
  30 #define VLAN_PRI_MASK               7
  31
  32
  33 struct  ether_header {
  34         uint8   ether_dhost[6];
  35         uint8   ether_shost[6];
  36         uint16  ether_type;
  37 } __attribute__((packed));
  38
  39
  40 struct ethervlan_header {
  41         uint8   ether_dhost[6];
  42         uint8   ether_shost[6];
  43         uint16  vlan_type;              /* 0x8100 */
  44         uint16  vlan_tag;               /* priority, cfi and vid */
  45         uint16  ether_type;
  46 };
  47
  48 /* copy a pkt buffer chain into a buffer */
  49 uint
  50 pktcopy(osl_t *osh, void *p, uint offset, int len, uchar *buf)
  51 {
  52         uint n, ret = 0;
  53
  54         if (len < 0)
  55                 len = 4096;     /* "infinite" */
  56
  57         /* skip 'offset' bytes */
  58         for (; p && offset; p = PKTNEXT(osh, p)) {
  59                 if (offset < (uint)PKTLEN(osh, p))
  60                         break;
  61                 offset -= PKTLEN(osh, p);
  62         }
  63
  64         if (!p)
  65                 return 0;
  66
  67         /* copy the data */
  68         for (; p && len; p = PKTNEXT(osh, p)) {
  69                 n = MIN((uint)PKTLEN(osh, p) - offset, (uint)len);
  70                 bcopy(PKTDATA(osh, p) + offset, buf, n);
  71                 buf += n;
  72                 len -= n;
  73                 ret += n;
  74                 offset = 0;
  75         }
  76
  77         return ret;
  78 }
  79
  80 /* return total length of buffer chain */
  81 uint
  82 pkttotlen(osl_t *osh, void *p)
  83 {
  84         uint total;
  85
  86         total = 0;
  87         for (; p; p = PKTNEXT(osh, p))
  88                 total += PKTLEN(osh, p);
  89         return (total);
  90 }
  91
  92 /* return the last buffer of chained pkt */
  93 void *
  94 pktlast(osl_t *osh, void *p)
  95 {
  96         for (; PKTNEXT(osh, p); p = PKTNEXT(osh, p))
  97                 ;
  98
  99         return (p);
 100 }
 101
 102
 103 /*
 104  * osl multiple-precedence packet queue
 105  * hi_prec is always >= the number of the highest non-empty queue
 106  */
 107 void *
 108 pktq_penq(struct pktq *pq, int prec, void *p)
 109 {
 110         struct pktq_prec *q;
 111
 112         ASSERT(prec >= 0 && prec < pq->num_prec);
 113         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
 114
 115         ASSERT(!pktq_full(pq));
 116         ASSERT(!pktq_pfull(pq, prec));
 117
 118         q = &pq->q[prec];
 119
 120         if (q->head)
 121                 PKTSETLINK(q->tail, p);
 122         else
 123                 q->head = p;
 124
 125         q->tail = p;
 126         q->len++;
 127
 128         pq->len++;
 129
 130         if (pq->hi_prec < prec)
 131                 pq->hi_prec = (uint8)prec;
 132
 133         return p;
 134 }
 135
 136 void *
 137 pktq_penq_head(struct pktq *pq, int prec, void *p)
 138 {
 139         struct pktq_prec *q;
 140
 141         ASSERT(prec >= 0 && prec < pq->num_prec);
 142         ASSERT(PKTLINK(p) == NULL);         /* queueing chains not allowed */
 143
 144         ASSERT(!pktq_full(pq));
 145         ASSERT(!pktq_pfull(pq, prec));
 146
 147         q = &pq->q[prec];
 148
 149         if (q->head == NULL)
 150                 q->tail = p;
 151
 152         PKTSETLINK(p, q->head);
 153         q->head = p;
 154         q->len++;
 155
 156         pq->len++;
 157
 158         if (pq->hi_prec < prec)
 159                 pq->hi_prec = (uint8)prec;
 160
 161         return p;
 162 }
 163
 164 void *
 165 pktq_pdeq(struct pktq *pq, int prec)
 166 {
 167         struct pktq_prec *q;
 168         void *p;
 169
 170         ASSERT(prec >= 0 && prec < pq->num_prec);
 171
 172         q = &pq->q[prec];
 173
 174         if ((p = q->head) == NULL)
 175                 return NULL;
 176
 177         if ((q->head = PKTLINK(p)) == NULL)
 178                 q->tail = NULL;
 179
 180         q->len--;
 181
 182         pq->len--;
 183
 184         PKTSETLINK(p, NULL);
 185
 186         return p;
 187 }
 188
 189 void *
 190 pktq_pdeq_tail(struct pktq *pq, int prec)
 191 {
 192         struct pktq_prec *q;
 193         void *p, *prev;
 194
 195         ASSERT(prec >= 0 && prec < pq->num_prec);
 196
 197         q = &pq->q[prec];
 198
 199         if ((p = q->head) == NULL)
 200                 return NULL;
 201
 202         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 203                 prev = p;
 204
 205         if (prev)
 206                 PKTSETLINK(prev, NULL);
 207         else
 208                 q->head = NULL;
 209
 210         q->tail = prev;
 211         q->len--;
 212
 213         pq->len--;
 214
 215         return p;
 216 }
 217
 218 void
 219 pktq_pflush(osl_t *osh, struct pktq *pq, int prec, bool dir)
 220 {
 221         struct pktq_prec *q;
 222         void *p;
 223
 224         q = &pq->q[prec];
 225         p = q->head;
 226         while (p) {
 227                 q->head = PKTLINK(p);
 228                 PKTSETLINK(p, NULL);
 229                 PKTFREE(osh, p, dir);
 230                 q->len--;
 231                 pq->len--;
 232                 p = q->head;
 233         }
 234         ASSERT(q->len == 0);
 235         q->tail = NULL;
 236 }
 237
 238 bool
 239 pktq_pdel(struct pktq *pq, void *pktbuf, int prec)
 240 {
 241         struct pktq_prec *q;
 242         void *p;
 243
 244         ASSERT(prec >= 0 && prec < pq->num_prec);
 245
 246         if (!pktbuf)
 247                 return FALSE;
 248
 249         q = &pq->q[prec];
 250
 251         if (q->head == pktbuf) {
 252                 if ((q->head = PKTLINK(pktbuf)) == NULL)
 253                         q->tail = NULL;
 254         } else {
 255                 for (p = q->head; p && PKTLINK(p) != pktbuf; p = PKTLINK(p))
 256                         ;
 257                 if (p == NULL)
 258                         return FALSE;
 259
 260                 PKTSETLINK(p, PKTLINK(pktbuf));
 261                 if (q->tail == pktbuf)
 262                         q->tail = p;
 263         }
 264
 265         q->len--;
 266         pq->len--;
 267         PKTSETLINK(pktbuf, NULL);
 268         return TRUE;
 269 }
 270
 271 void
 272 pktq_init(struct pktq *pq, int num_prec, int max_len)
 273 {
 274         int prec;
 275
 276         ASSERT(num_prec > 0 && num_prec <= PKTQ_MAX_PREC);
 277
 278         bzero(pq, sizeof(*pq));
 279
 280         pq->num_prec = (uint16)num_prec;
 281
 282         pq->max = (uint16)max_len;
 283
 284         for (prec = 0; prec < num_prec; prec++)
 285                 pq->q[prec].max = pq->max;
 286 }
 287
 288 void *
 289 pktq_deq(struct pktq *pq, int *prec_out)
 290 {
 291         struct pktq_prec *q;
 292         void *p;
 293         int prec;
 294
 295         if (pq->len == 0)
 296                 return NULL;
 297
 298         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 299                 pq->hi_prec--;
 300
 301         q = &pq->q[prec];
 302
 303         if ((p = q->head) == NULL)
 304                 return NULL;
 305
 306         if ((q->head = PKTLINK(p)) == NULL)
 307                 q->tail = NULL;
 308
 309         q->len--;
 310
 311         pq->len--;
 312
 313         if (prec_out)
 314                 *prec_out = prec;
 315
 316         PKTSETLINK(p, NULL);
 317
 318         return p;
 319 }
 320
 321 void *
 322 pktq_deq_tail(struct pktq *pq, int *prec_out)
 323 {
 324         struct pktq_prec *q;
 325         void *p, *prev;
 326         int prec;
 327
 328         if (pq->len == 0)
 329                 return NULL;
 330
 331         for (prec = 0; prec < pq->hi_prec; prec++)
 332                 if (pq->q[prec].head)
 333                         break;
 334
 335         q = &pq->q[prec];
 336
 337         if ((p = q->head) == NULL)
 338                 return NULL;
 339
 340         for (prev = NULL; p != q->tail; p = PKTLINK(p))
 341                 prev = p;
 342
 343         if (prev)
 344                 PKTSETLINK(prev, NULL);
 345         else
 346                 q->head = NULL;
 347
 348         q->tail = prev;
 349         q->len--;
 350
 351         pq->len--;
 352
 353         if (prec_out)
 354                 *prec_out = prec;
 355
 356         PKTSETLINK(p, NULL);
 357
 358         return p;
 359 }
 360
 361 void *
 362 pktq_peek(struct pktq *pq, int *prec_out)
 363 {
 364         int prec;
 365
 366         if (pq->len == 0)
 367                 return NULL;
 368
 369         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 370                 pq->hi_prec--;
 371
 372         if (prec_out)
 373                 *prec_out = prec;
 374
 375         return (pq->q[prec].head);
 376 }
 377
 378 void *
 379 pktq_peek_tail(struct pktq *pq, int *prec_out)
 380 {
 381         int prec;
 382
 383         if (pq->len == 0)
 384                 return NULL;
 385
 386         for (prec = 0; prec < pq->hi_prec; prec++)
 387                 if (pq->q[prec].head)
 388                         break;
 389
 390         if (prec_out)
 391                 *prec_out = prec;
 392
 393         return (pq->q[prec].tail);
 394 }
 395
 396 void
 397 pktq_flush(osl_t *osh, struct pktq *pq, bool dir)
 398 {
 399         int prec;
 400         for (prec = 0; prec < pq->num_prec; prec++)
 401                 pktq_pflush(osh, pq, prec, dir);
 402         ASSERT(pq->len == 0);
 403 }
 404
 405 /* Return sum of lengths of a specific set of precedences */
 406 int
 407 pktq_mlen(struct pktq *pq, uint prec_bmp)
 408 {
 409         int prec, len;
 410
 411         len = 0;
 412
 413         for (prec = 0; prec <= pq->hi_prec; prec++)
 414                 if (prec_bmp & (1 << prec))
 415                         len += pq->q[prec].len;
 416
 417         return len;
 418 }
 419
 420 /* Priority dequeue from a specific set of precedences */
 421 void *
 422 pktq_mdeq(struct pktq *pq, uint prec_bmp, int *prec_out)
 423 {
 424         struct pktq_prec *q;
 425         void *p;
 426         int prec;
 427
 428         if (pq->len == 0)
 429                 return NULL;
 430
 431         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 432                 pq->hi_prec--;
 433
 434         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 435                 if (prec-- == 0)
 436                         return NULL;
 437
 438         q = &pq->q[prec];
 439
 440         if ((p = q->head) == NULL)
 441                 return NULL;
 442
 443         if ((q->head = PKTLINK(p)) == NULL)
 444                 q->tail = NULL;
 445
 446         q->len--;
 447
 448         if (prec_out)
 449                 *prec_out = prec;
 450
 451         pq->len--;
 452
 453         PKTSETLINK(p, NULL);
 454
 455         return p;
 456 }
 457
 458 char*
 459 bcmstrcat(char *dest, const char *src)
 460 {
 461         strcpy(&dest[strlen(dest)], src);
 462         return (dest);
 463 }
 464
 465 char*
 466 bcm_ether_ntoa(struct ether_addr *ea, char *buf)
 467 {
 468         sprintf(buf, "%02x:%02x:%02x:%02x:%02x:%02x",
 469                 ea->octet[0]&0xff, ea->octet[1]&0xff, ea->octet[2]&0xff,
 470                 ea->octet[3]&0xff, ea->octet[4]&0xff, ea->octet[5]&0xff);
 471         return (buf);
 472 }
 473
 474 /* parse a xx:xx:xx:xx:xx:xx format ethernet address */
 475 int
 476 bcm_ether_atoe(char *p, struct ether_addr *ea)
 477 {
 478         int i = 0;
 479
 480         for (;;) {
 481                 ea->octet[i++] = (char) bcm_strtoul(p, &p, 16);
 482                 if (!*p++ || i == 6)
 483                         break;
 484         }
 485
 486         return (i == 6);
 487 }
 488
 489 /* Takes an Ethernet frame and sets out-of-bound PKTPRIO
 490  * Also updates the inplace vlan tag if requested
 491  */
 492 void
 493 pktsetprio(void *pkt, bool update_vtag)
 494 {
 495         struct ether_header *eh;
 496         struct ethervlan_header *evh;
 497         uint8 *pktdata;
 498         int priority = 0;
 499
 500         pktdata = (uint8 *) PKTDATA(NULL, pkt);
 501         ASSERT(ISALIGNED((uintptr)pktdata, sizeof(uint16)));
 502
 503         eh = (struct ether_header *) pktdata;
 504
 505         if (ntoh16(eh->ether_type) == ETHER_TYPE_8021Q) {
 506                 uint16 vlan_tag;
 507                 int vlan_prio, dscp_prio = 0;
 508
 509                 evh = (struct ethervlan_header *)eh;
 510
 511                 vlan_tag = ntoh16(evh->vlan_tag);
 512                 vlan_prio = (int) (vlan_tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
 513
 514                 if (ntoh16(evh->ether_type) == ETHER_TYPE_IP) {
 515                         uint8 *ip_body = pktdata + sizeof(struct ethervlan_header);
 516                         uint8 tos_tc = IP_TOS(ip_body);
 517                         dscp_prio = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
 518                 }
 519
 520                 /* DSCP priority gets precedence over 802.1P (vlan tag) */
 521                 priority = (dscp_prio != 0) ? dscp_prio : vlan_prio;
 522
 523                 /*
 524                  * If the DSCP priority is not the same as the VLAN priority,
 525                  * then overwrite the priority field in the vlan tag, with the
 526                  * DSCP priority value. This is required for Linux APs because
 527                  * the VLAN driver on Linux, overwrites the skb->priority field
 528                  * with the priority value in the vlan tag
 529                  */
 530                 if (update_vtag && (priority != vlan_prio)) {
 531                         vlan_tag &= ~(VLAN_PRI_MASK << VLAN_PRI_SHIFT);
 532                         vlan_tag |= (uint16)priority << VLAN_PRI_SHIFT;
 533                         evh->vlan_tag = hton16(vlan_tag);
 534                 }
 535         } else if (ntoh16(eh->ether_type) == ETHER_TYPE_IP) {
 536                 uint8 *ip_body = pktdata + sizeof(struct ether_header);
 537                 uint8 tos_tc = IP_TOS(ip_body);
 538                 priority = (int)(tos_tc >> IPV4_TOS_PREC_SHIFT);
 539         }
 540
 541         ASSERT(priority >= 0 && priority <= MAXPRIO);
 542         PKTSETPRIO(pkt, priority);
 543 }
 544
 545 static char bcm_undeferrstr[BCME_STRLEN];
 546
 547 static const char *bcmerrorstrtable[] = BCMERRSTRINGTABLE;
 548
 549 /* Convert the Error codes into related Error strings  */
 550 const char *
 551 bcmerrorstr(int bcmerror)
 552 {
 553         int abs_bcmerror;
 554
 555         abs_bcmerror = ABS(bcmerror);
 556
 557         /* check if someone added a bcmerror code but forgot to add errorstring */
 558         ASSERT(ABS(BCME_LAST) == (ARRAYSIZE(bcmerrorstrtable) - 1));
 559         if ((bcmerror > 0) || (abs_bcmerror > ABS(BCME_LAST))) {
 560                 sprintf(bcm_undeferrstr, "undefined Error %d", bcmerror);
 561                 return bcm_undeferrstr;
 562         }
 563
 564         ASSERT((strlen((char*)bcmerrorstrtable[abs_bcmerror])) < BCME_STRLEN);
 565
 566         return bcmerrorstrtable[abs_bcmerror];
 567 }
 568
 569
 570 int
 571 bcm_iovar_lencheck(const bcm_iovar_t *vi, void *arg, int len, bool set)
 572 {
 573         int bcmerror = 0;
 574
 575         /* length check on io buf */
 576         switch (vi->type) {
 577         case IOVT_BOOL:
 578         case IOVT_INT8:
 579         case IOVT_INT16:
 580         case IOVT_INT32:
 581         case IOVT_UINT8:
 582         case IOVT_UINT16:
 583         case IOVT_UINT32:
 584                 /* all integers are int32 sized args at the ioctl interface */
 585                 if (len < (int)sizeof(int)) {
 586                         bcmerror = BCME_BUFTOOSHORT;
 587                 }
 588                 break;
 589
 590         case IOVT_BUFFER:
 591                 /* buffer must meet minimum length requirement */
 592                 if (len < vi->minlen) {
 593                         bcmerror = BCME_BUFTOOSHORT;
 594                 }
 595                 break;
 596
 597         case IOVT_VOID:
 598                 if (!set) {
 599                         /* Cannot return nil... */
 600                         bcmerror = BCME_UNSUPPORTED;
 601                 } else if (len) {
 602                         /* Set is an action w/o parameters */
 603                         bcmerror = BCME_BUFTOOLONG;
 604                 }
 605                 break;
 606
 607         default:
 608                 /* unknown type for length check in iovar info */
 609                 ASSERT(0);
 610                 bcmerror = BCME_UNSUPPORTED;
 611         }
 612
 613         return bcmerror;
 614 }
 615
 616 #define CRC_INNER_LOOP(n, c, x) \
 617                     (c) = ((c) >> 8) ^ crc##n##_table[((c) ^ (x)) & 0xff]
 618
 619 static uint32 crc32_table[256] = {
 620     0x00000000, 0x77073096, 0xEE0E612C, 0x990951BA,
 621     0x076DC419, 0x706AF48F, 0xE963A535, 0x9E6495A3,
 622     0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E, 0x97D2D988,
 623     0x09B64C2B, 0x7EB17CBD, 0xE7B82D07, 0x90BF1D91,
 624     0x1DB71064, 0x6AB020F2, 0xF3B97148, 0x84BE41DE,
 625     0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551, 0x83D385C7,
 626     0x136C9856, 0x646BA8C0, 0xFD62F97A, 0x8A65C9EC,
 627     0x14015C4F, 0x63066CD9, 0xFA0F3D63, 0x8D080DF5,
 628     0x3B6E20C8, 0x4C69105E, 0xD56041E4, 0xA2677172,
 629     0x3C03E4D1, 0x4B04D447, 0xD20D85FD, 0xA50AB56B,
 630     0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6, 0xACBCF940,
 631     0x32D86CE3, 0x45DF5C75, 0xDCD60DCF, 0xABD13D59,
 632     0x26D930AC, 0x51DE003A, 0xC8D75180, 0xBFD06116,
 633     0x21B4F4B5, 0x56B3C423, 0xCFBA9599, 0xB8BDA50F,
 634     0x2802B89E, 0x5F058808, 0xC60CD9B2, 0xB10BE924,
 635     0x2F6F7C87, 0x58684C11, 0xC1611DAB, 0xB6662D3D,
 636     0x76DC4190, 0x01DB7106, 0x98D220BC, 0xEFD5102A,
 637     0x71B18589, 0x06B6B51F, 0x9FBFE4A5, 0xE8B8D433,
 638     0x7807C9A2, 0x0F00F934, 0x9609A88E, 0xE10E9818,
 639     0x7F6A0DBB, 0x086D3D2D, 0x91646C97, 0xE6635C01,
 640     0x6B6B51F4, 0x1C6C6162, 0x856530D8, 0xF262004E,
 641     0x6C0695ED, 0x1B01A57B, 0x8208F4C1, 0xF50FC457,
 642     0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA, 0xFCB9887C,
 643     0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3, 0xFBD44C65,
 644     0x4DB26158, 0x3AB551CE, 0xA3BC0074, 0xD4BB30E2,
 645     0x4ADFA541, 0x3DD895D7, 0xA4D1C46D, 0xD3D6F4FB,
 646     0x4369E96A, 0x346ED9FC, 0xAD678846, 0xDA60B8D0,
 647     0x44042D73, 0x33031DE5, 0xAA0A4C5F, 0xDD0D7CC9,
 648     0x5005713C, 0x270241AA, 0xBE0B1010, 0xC90C2086,
 649     0x5768B525, 0x206F85B3, 0xB966D409, 0xCE61E49F,
 650     0x5EDEF90E, 0x29D9C998, 0xB0D09822, 0xC7D7A8B4,
 651     0x59B33D17, 0x2EB40D81, 0xB7BD5C3B, 0xC0BA6CAD,
 652     0xEDB88320, 0x9ABFB3B6, 0x03B6E20C, 0x74B1D29A,
 653     0xEAD54739, 0x9DD277AF, 0x04DB2615, 0x73DC1683,
 654     0xE3630B12, 0x94643B84, 0x0D6D6A3E, 0x7A6A5AA8,
 655     0xE40ECF0B, 0x9309FF9D, 0x0A00AE27, 0x7D079EB1,
 656     0xF00F9344, 0x8708A3D2, 0x1E01F268, 0x6906C2FE,
 657     0xF762575D, 0x806567CB, 0x196C3671, 0x6E6B06E7,
 658     0xFED41B76, 0x89D32BE0, 0x10DA7A5A, 0x67DD4ACC,
 659     0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43, 0x60B08ED5,
 660     0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4, 0x4FDFF252,
 661     0xD1BB67F1, 0xA6BC5767, 0x3FB506DD, 0x48B2364B,
 662     0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6, 0x41047A60,
 663     0xDF60EFC3, 0xA867DF55, 0x316E8EEF, 0x4669BE79,
 664     0xCB61B38C, 0xBC66831A, 0x256FD2A0, 0x5268E236,
 665     0xCC0C7795, 0xBB0B4703, 0x220216B9, 0x5505262F,
 666     0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92, 0x5CB36A04,
 667     0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B, 0x5BDEAE1D,
 668     0x9B64C2B0, 0xEC63F226, 0x756AA39C, 0x026D930A,
 669     0x9C0906A9, 0xEB0E363F, 0x72076785, 0x05005713,
 670     0x95BF4A82, 0xE2B87A14, 0x7BB12BAE, 0x0CB61B38,
 671     0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7, 0x0BDBDF21,
 672     0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8, 0x1FDA836E,
 673     0x81BE16CD, 0xF6B9265B, 0x6FB077E1, 0x18B74777,
 674     0x88085AE6, 0xFF0F6A70, 0x66063BCA, 0x11010B5C,
 675     0x8F659EFF, 0xF862AE69, 0x616BFFD3, 0x166CCF45,
 676     0xA00AE278, 0xD70DD2EE, 0x4E048354, 0x3903B3C2,
 677     0xA7672661, 0xD06016F7, 0x4969474D, 0x3E6E77DB,
 678     0xAED16A4A, 0xD9D65ADC, 0x40DF0B66, 0x37D83BF0,
 679     0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F, 0x30B5FFE9,
 680     0xBDBDF21C, 0xCABAC28A, 0x53B39330, 0x24B4A3A6,
 681     0xBAD03605, 0xCDD70693, 0x54DE5729, 0x23D967BF,
 682     0xB3667A2E, 0xC4614AB8, 0x5D681B02, 0x2A6F2B94,
 683     0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B, 0x2D02EF8D
 684 };
 685
 686 uint32
 687 hndcrc32(
 688     uint8 *pdata,  /* pointer to array of data to process */
 689     uint   nbytes, /* number of input data bytes to process */
 690     uint32 crc     /* either CRC32_INIT_VALUE or previous return value */
 691 )
 692 {
 693         uint8 *pend;
 694 #ifdef __mips__
 695         uint8 tmp[4];
 696         ulong *tptr = (ulong *)tmp;
 697
 698         /* in case the beginning of the buffer isn't aligned */
 699         pend = (uint8 *)((uint)(pdata + 3) & 0xfffffffc);
 700         nbytes -= (pend - pdata);
 701         while (pdata < pend)
 702                 CRC_INNER_LOOP(32, crc, *pdata++);
 703
 704         /* handle bulk of data as 32-bit words */
 705         pend = pdata + (nbytes & 0xfffffffc);
 706         while (pdata < pend) {
 707                 *tptr = *(ulong *)pdata;
 708                 pdata += sizeof(ulong *);
 709                 CRC_INNER_LOOP(32, crc, tmp[0]);
 710                 CRC_INNER_LOOP(32, crc, tmp[1]);
 711                 CRC_INNER_LOOP(32, crc, tmp[2]);
 712                 CRC_INNER_LOOP(32, crc, tmp[3]);
 713         }
 714
 715         /* 1-3 bytes at end of buffer */
 716         pend = pdata + (nbytes & 0x03);
 717         while (pdata < pend)
 718                 CRC_INNER_LOOP(32, crc, *pdata++);
 719 #else
 720         pend = pdata + nbytes;
 721         while (pdata < pend)
 722                 CRC_INNER_LOOP(32, crc, *pdata++);
 723 #endif /* __mips__ */
 724
 725         return crc;
 726 }
 727
 728
 729 /*
 730  * Advance from the current 1-byte tag/1-byte length/variable-length value
 731  * triple, to the next, returning a pointer to the next.
 732  * If the current or next TLV is invalid (does not fit in given buffer length),
 733  * NULL is returned.
 734  * *buflen is not modified if the TLV elt parameter is invalid, or is decremented
 735  * by the TLV paramter's length if it is valid.
 736  */
 737 bcm_tlv_t *
 738 bcm_next_tlv(bcm_tlv_t *elt, int *buflen)
 739 {
 740         int len;
 741
 742         /* validate current elt */
 743         if (!bcm_valid_tlv(elt, *buflen))
 744                 return NULL;
 745
 746         /* advance to next elt */
 747         len = elt->len;
 748         elt = (bcm_tlv_t*)(elt->data + len);
 749         *buflen -= (2 + len);
 750
 751         /* validate next elt */
 752         if (!bcm_valid_tlv(elt, *buflen))
 753                 return NULL;
 754
 755         return elt;
 756 }
 757
 758 /*
 759  * Traverse a string of 1-byte tag/1-byte length/variable-length value
 760  * triples, returning a pointer to the substring whose first element
 761  * matches tag
 762  */
 763 bcm_tlv_t *
 764 bcm_parse_tlvs(void *buf, int buflen, uint key)
 765 {
 766         bcm_tlv_t *elt;
 767         int totlen;
 768
 769         elt = (bcm_tlv_t*)buf;
 770         totlen = buflen;
 771
 772         /* find tagged parameter */
 773         while (totlen >= 2) {
 774                 int len = elt->len;
 775
 776                 /* validate remaining totlen */
 777                 if ((elt->id == key) && (totlen >= (len + 2)))
 778                         return (elt);
 779
 780                 elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
 781                 totlen -= (len + 2);
 782         }
 783
 784         return NULL;
 785 }
 786
 787 /*
 788  * Traverse a string of 1-byte tag/1-byte length/variable-length value
 789  * triples, returning a pointer to the substring whose first element
 790  * matches tag.  Stop parsing when we see an element whose ID is greater
 791  * than the target key.
 792  */
 793 bcm_tlv_t *
 794 bcm_parse_ordered_tlvs(void *buf, int buflen, uint key)
 795 {
 796         bcm_tlv_t *elt;
 797         int totlen;
 798
 799         elt = (bcm_tlv_t*)buf;
 800         totlen = buflen;
 801
 802         /* find tagged parameter */
 803         while (totlen >= 2) {
 804                 uint id = elt->id;
 805                 int len = elt->len;
 806
 807                 /* Punt if we start seeing IDs > than target key */
 808                 if (id > key)
 809                         return (NULL);
 810
 811                 /* validate remaining totlen */
 812                 if ((id == key) && (totlen >= (len + 2)))
 813                         return (elt);
 814
 815                 elt = (bcm_tlv_t*)((uint8*)elt + (len + 2));
 816                 totlen -= (len + 2);
 817         }
 818         return NULL;
 819 }
 820
 821
 822 /* Initialization of bcmstrbuf structure */
 823 void
 824 bcm_binit(struct bcmstrbuf *b, char *buf, uint size)
 825 {
 826         b->origsize = b->size = size;
 827         b->origbuf = b->buf = buf;
 828 }
 829
 830 /* Buffer sprintf wrapper to guard against buffer overflow */
 831 int
 832 bcm_bprintf(struct bcmstrbuf *b, const char *fmt, ...)
 833 {
 834         va_list ap;
 835         int r;
 836
 837         va_start(ap, fmt);
 838         r = vsnprintf(b->buf, b->size, fmt, ap);
 839
 840         /* Non Ansi C99 compliant returns -1,
 841          * Ansi compliant return r >= b->size,
 842          * bcmstdlib returns 0, handle all
 843          */
 844         if ((r == -1) || (r >= (int)b->size) || (r == 0))
 845         {
 846                 b->size = 0;
 847         }
 848         else
 849         {
 850                 b->size -= r;
 851                 b->buf += r;
 852         }
 853
 854         va_end(ap);
 855
 856         return r;
 857 }
 858
 859 uint
 860 bcm_bitcount(uint8 *bitmap, uint length)
 861 {
 862         uint bitcount = 0, i;
 863         uint8 tmp;
 864         for (i = 0; i < length; i++) {
 865                 tmp = bitmap[i];
 866                 while (tmp) {
 867                         bitcount++;
 868                         tmp &= (tmp - 1);
 869                 }
 870         }
 871         return bitcount;
 872 }
 873