drivers/staging/brcm80211/brcmutil/utils.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 <linux/netdevice.h>
  18 #include <brcmu_utils.h>
  19
  20 MODULE_AUTHOR("Broadcom Corporation");
  21 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
  22 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
  23 MODULE_LICENSE("Dual BSD/GPL");
  24
  25 struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
  26 {
  27         struct sk_buff *skb;
  28
  29         skb = dev_alloc_skb(len);
  30         if (skb) {
  31                 skb_put(skb, len);
  32                 skb->priority = 0;
  33         }
  34
  35         return skb;
  36 }
  37 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
  38
  39 /* Free the driver packet. Free the tag if present */
  40 void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
  41 {
  42         struct sk_buff *nskb;
  43         int nest = 0;
  44
  45         /* perversion: we use skb->next to chain multi-skb packets */
  46         while (skb) {
  47                 nskb = skb->next;
  48                 skb->next = NULL;
  49
  50                 if (skb->destructor)
  51                         /* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
  52                          * destructor exists
  53                          */
  54                         dev_kfree_skb_any(skb);
  55                 else
  56                         /* can free immediately (even in_irq()) if destructor
  57                          * does not exist
  58                          */
  59                         dev_kfree_skb(skb);
  60
  61                 nest++;
  62                 skb = nskb;
  63         }
  64 }
  65 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
  66
  67
  68 /* copy a buffer into a pkt buffer chain */
  69 uint brcmu_pktfrombuf(struct sk_buff *p, uint offset, int len,
  70                 unsigned char *buf)
  71 {
  72         uint n, ret = 0;
  73
  74         /* skip 'offset' bytes */
  75         for (; p && offset; p = p->next) {
  76                 if (offset < (uint) (p->len))
  77                         break;
  78                 offset -= p->len;
  79         }
  80
  81         if (!p)
  82                 return 0;
  83
  84         /* copy the data */
  85         for (; p && len; p = p->next) {
  86                 n = min((uint) (p->len) - offset, (uint) len);
  87                 memcpy(p->data + offset, buf, n);
  88                 buf += n;
  89                 len -= n;
  90                 ret += n;
  91                 offset = 0;
  92         }
  93
  94         return ret;
  95 }
  96 EXPORT_SYMBOL(brcmu_pktfrombuf);
  97
  98 /* return total length of buffer chain */
  99 uint brcmu_pkttotlen(struct sk_buff *p)
 100 {
 101         uint total;
 102
 103         total = 0;
 104         for (; p; p = p->next)
 105                 total += p->len;
 106         return total;
 107 }
 108 EXPORT_SYMBOL(brcmu_pkttotlen);
 109
 110 /*
 111  * osl multiple-precedence packet queue
 112  * hi_prec is always >= the number of the highest non-empty precedence
 113  */
 114 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
 115                                       struct sk_buff *p)
 116 {
 117         struct pktq_prec *q;
 118
 119         if (pktq_full(pq) || pktq_pfull(pq, prec))
 120                 return NULL;
 121
 122         q = &pq->q[prec];
 123
 124         if (q->head)
 125                 q->tail->prev = p;
 126         else
 127                 q->head = p;
 128
 129         q->tail = p;
 130         q->len++;
 131
 132         pq->len++;
 133
 134         if (pq->hi_prec < prec)
 135                 pq->hi_prec = (u8) prec;
 136
 137         return p;
 138 }
 139 EXPORT_SYMBOL(brcmu_pktq_penq);
 140
 141 struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
 142                                            struct sk_buff *p)
 143 {
 144         struct pktq_prec *q;
 145
 146         if (pktq_full(pq) || pktq_pfull(pq, prec))
 147                 return NULL;
 148
 149         q = &pq->q[prec];
 150
 151         if (q->head == NULL)
 152                 q->tail = p;
 153
 154         p->prev = q->head;
 155         q->head = p;
 156         q->len++;
 157
 158         pq->len++;
 159
 160         if (pq->hi_prec < prec)
 161                 pq->hi_prec = (u8) prec;
 162
 163         return p;
 164 }
 165 EXPORT_SYMBOL(brcmu_pktq_penq_head);
 166
 167 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
 168 {
 169         struct pktq_prec *q;
 170         struct sk_buff *p;
 171
 172         q = &pq->q[prec];
 173
 174         p = q->head;
 175         if (p == NULL)
 176                 return NULL;
 177
 178         q->head = p->prev;
 179         if (q->head == NULL)
 180                 q->tail = NULL;
 181
 182         q->len--;
 183
 184         pq->len--;
 185
 186         p->prev = NULL;
 187
 188         return p;
 189 }
 190 EXPORT_SYMBOL(brcmu_pktq_pdeq);
 191
 192 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 193 {
 194         struct pktq_prec *q;
 195         struct sk_buff *p, *prev;
 196
 197         q = &pq->q[prec];
 198
 199         p = q->head;
 200         if (p == NULL)
 201                 return NULL;
 202
 203         for (prev = NULL; p != q->tail; p = p->prev)
 204                 prev = p;
 205
 206         if (prev)
 207                 prev->prev = NULL;
 208         else
 209                 q->head = NULL;
 210
 211         q->tail = prev;
 212         q->len--;
 213
 214         pq->len--;
 215
 216         return p;
 217 }
 218 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 219
 220 void
 221 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 222                   bool (*fn)(struct sk_buff *, void *), void *arg)
 223 {
 224         struct pktq_prec *q;
 225         struct sk_buff *p, *prev = NULL;
 226
 227         q = &pq->q[prec];
 228         p = q->head;
 229         while (p) {
 230                 if (fn == NULL || (*fn) (p, arg)) {
 231                         bool head = (p == q->head);
 232                         if (head)
 233                                 q->head = p->prev;
 234                         else
 235                                 prev->prev = p->prev;
 236                         p->prev = NULL;
 237                         brcmu_pkt_buf_free_skb(p);
 238                         q->len--;
 239                         pq->len--;
 240                         p = (head ? q->head : prev->prev);
 241                 } else {
 242                         prev = p;
 243                         p = p->prev;
 244                 }
 245         }
 246
 247         if (q->head == NULL)
 248                 q->tail = NULL;
 249 }
 250 EXPORT_SYMBOL(brcmu_pktq_pflush);
 251
 252 void brcmu_pktq_flush(struct pktq *pq, bool dir,
 253                       bool (*fn)(struct sk_buff *, void *), void *arg)
 254 {
 255         int prec;
 256         for (prec = 0; prec < pq->num_prec; prec++)
 257                 brcmu_pktq_pflush(pq, prec, dir, fn, arg);
 258 }
 259 EXPORT_SYMBOL(brcmu_pktq_flush);
 260
 261 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
 262 {
 263         int prec;
 264
 265         /* pq is variable size; only zero out what's requested */
 266         memset(pq, 0,
 267               offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 268
 269         pq->num_prec = (u16) num_prec;
 270
 271         pq->max = (u16) max_len;
 272
 273         for (prec = 0; prec < num_prec; prec++)
 274                 pq->q[prec].max = pq->max;
 275 }
 276 EXPORT_SYMBOL(brcmu_pktq_init);
 277
 278 struct sk_buff *brcmu_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 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 295
 296 /* Return sum of lengths of a specific set of precedences */
 297 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
 298 {
 299         int prec, len;
 300
 301         len = 0;
 302
 303         for (prec = 0; prec <= pq->hi_prec; prec++)
 304                 if (prec_bmp & (1 << prec))
 305                         len += pq->q[prec].len;
 306
 307         return len;
 308 }
 309 EXPORT_SYMBOL(brcmu_pktq_mlen);
 310
 311 /* Priority dequeue from a specific set of precedences */
 312 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 313                                       int *prec_out)
 314 {
 315         struct pktq_prec *q;
 316         struct sk_buff *p;
 317         int prec;
 318
 319         if (pq->len == 0)
 320                 return NULL;
 321
 322         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 323                 pq->hi_prec--;
 324
 325         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 326                 if (prec-- == 0)
 327                         return NULL;
 328
 329         q = &pq->q[prec];
 330
 331         p = q->head;
 332         if (p == NULL)
 333                 return NULL;
 334
 335         q->head = p->prev;
 336         if (q->head == NULL)
 337                 q->tail = NULL;
 338
 339         q->len--;
 340
 341         if (prec_out)
 342                 *prec_out = prec;
 343
 344         pq->len--;
 345
 346         p->prev = NULL;
 347
 348         return p;
 349 }
 350 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 351
 352 #if defined(BCMDBG)
 353 /* pretty hex print a pkt buffer chain */
 354 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 355 {
 356         struct sk_buff *p;
 357
 358         if (msg && (msg[0] != '\0'))
 359                 printk(KERN_DEBUG "%s:\n", msg);
 360
 361         for (p = p0; p; p = p->next)
 362                 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
 363 }
 364 EXPORT_SYMBOL(brcmu_prpkt);
 365 #endif                          /* defined(BCMDBG) */
 366
 367 /*
 368  * Traverse a string of 1-byte tag/1-byte length/variable-length value
 369  * triples, returning a pointer to the substring whose first element
 370  * matches tag
 371  */
 372 struct brcmu_tlv *brcmu_parse_tlvs(void *buf, int buflen, uint key)
 373 {
 374         struct brcmu_tlv *elt;
 375         int totlen;
 376
 377         elt = (struct brcmu_tlv *) buf;
 378         totlen = buflen;
 379
 380         /* find tagged parameter */
 381         while (totlen >= 2) {
 382                 int len = elt->len;
 383
 384                 /* validate remaining totlen */
 385                 if ((elt->id == key) && (totlen >= (len + 2)))
 386                         return elt;
 387
 388                 elt = (struct brcmu_tlv *) ((u8 *) elt + (len + 2));
 389                 totlen -= (len + 2);
 390         }
 391
 392         return NULL;
 393 }
 394 EXPORT_SYMBOL(brcmu_parse_tlvs);
 395
 396
 397 #if defined(BCMDBG)
 398 int
 399 brcmu_format_flags(const struct brcmu_bit_desc *bd, u32 flags, char *buf,
 400                    int len)
 401 {
 402         int i;
 403         char *p = buf;
 404         char hexstr[16];
 405         int slen = 0, nlen = 0;
 406         u32 bit;
 407         const char *name;
 408
 409         if (len < 2 || !buf)
 410                 return 0;
 411
 412         buf[0] = '\0';
 413
 414         for (i = 0; flags != 0; i++) {
 415                 bit = bd[i].bit;
 416                 name = bd[i].name;
 417                 if (bit == 0 && flags != 0) {
 418                         /* print any unnamed bits */
 419                         snprintf(hexstr, 16, "0x%X", flags);
 420                         name = hexstr;
 421                         flags = 0;      /* exit loop */
 422                 } else if ((flags & bit) == 0)
 423                         continue;
 424                 flags &= ~bit;
 425                 nlen = strlen(name);
 426                 slen += nlen;
 427                 /* count btwn flag space */
 428                 if (flags != 0)
 429                         slen += 1;
 430                 /* need NULL char as well */
 431                 if (len <= slen)
 432                         break;
 433                 /* copy NULL char but don't count it */
 434                 strncpy(p, name, nlen + 1);
 435                 p += nlen;
 436                 /* copy btwn flag space and NULL char */
 437                 if (flags != 0)
 438                         p += snprintf(p, 2, " ");
 439                 len -= slen;
 440         }
 441
 442         /* indicate the str was too short */
 443         if (flags != 0) {
 444                 if (len < 2)
 445                         p -= 2 - len;   /* overwrite last char */
 446                 p += snprintf(p, 2, ">");
 447         }
 448
 449         return (int)(p - buf);
 450 }
 451 EXPORT_SYMBOL(brcmu_format_flags);
 452
 453 /*
 454  * print bytes formatted as hex to a string. return the resulting
 455  * string length
 456  */
 457 int brcmu_format_hex(char *str, const void *bytes, int len)
 458 {
 459         int i;
 460         char *p = str;
 461         const u8 *src = (const u8 *)bytes;
 462
 463         for (i = 0; i < len; i++) {
 464                 p += snprintf(p, 3, "%02X", *src);
 465                 src++;
 466         }
 467         return (int)(p - str);
 468 }
 469 EXPORT_SYMBOL(brcmu_format_hex);
 470 #endif                          /* defined(BCMDBG) */
 471
 472 char *brcmu_chipname(uint chipid, char *buf, uint len)
 473 {
 474         const char *fmt;
 475
 476         fmt = ((chipid > 0xa000) || (chipid < 0x4000)) ? "%d" : "%x";
 477         snprintf(buf, len, fmt, chipid);
 478         return buf;
 479 }
 480 EXPORT_SYMBOL(brcmu_chipname);
 481
 482 uint brcmu_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
 483 {
 484         uint len;
 485
 486         len = strlen(name) + 1;
 487
 488         if ((len + datalen) > buflen)
 489                 return 0;
 490
 491         strncpy(buf, name, buflen);
 492
 493         /* append data onto the end of the name string */
 494         memcpy(&buf[len], data, datalen);
 495         len += datalen;
 496
 497         return len;
 498 }
 499 EXPORT_SYMBOL(brcmu_mkiovar);
 500
 501 /* Quarter dBm units to mW
 502  * Table starts at QDBM_OFFSET, so the first entry is mW for qdBm=153
 503  * Table is offset so the last entry is largest mW value that fits in
 504  * a u16.
 505  */
 506
 507 #define QDBM_OFFSET 153         /* Offset for first entry */
 508 #define QDBM_TABLE_LEN 40       /* Table size */
 509
 510 /* Smallest mW value that will round up to the first table entry, QDBM_OFFSET.
 511  * Value is ( mW(QDBM_OFFSET - 1) + mW(QDBM_OFFSET) ) / 2
 512  */
 513 #define QDBM_TABLE_LOW_BOUND 6493       /* Low bound */
 514
 515 /* Largest mW value that will round down to the last table entry,
 516  * QDBM_OFFSET + QDBM_TABLE_LEN-1.
 517  * Value is ( mW(QDBM_OFFSET + QDBM_TABLE_LEN - 1) +
 518  * mW(QDBM_OFFSET + QDBM_TABLE_LEN) ) / 2.
 519  */
 520 #define QDBM_TABLE_HIGH_BOUND 64938     /* High bound */
 521
 522 static const u16 nqdBm_to_mW_map[QDBM_TABLE_LEN] = {
 523 /* qdBm:        +0      +1      +2      +3      +4      +5      +6      +7 */
 524 /* 153: */ 6683, 7079, 7499, 7943, 8414, 8913, 9441, 10000,
 525 /* 161: */ 10593, 11220, 11885, 12589, 13335, 14125, 14962, 15849,
 526 /* 169: */ 16788, 17783, 18836, 19953, 21135, 22387, 23714, 25119,
 527 /* 177: */ 26607, 28184, 29854, 31623, 33497, 35481, 37584, 39811,
 528 /* 185: */ 42170, 44668, 47315, 50119, 53088, 56234, 59566, 63096
 529 };
 530
 531 u16 brcmu_qdbm_to_mw(u8 qdbm)
 532 {
 533         uint factor = 1;
 534         int idx = qdbm - QDBM_OFFSET;
 535
 536         if (idx >= QDBM_TABLE_LEN)
 537                 /* clamp to max u16 mW value */
 538                 return 0xFFFF;
 539
 540         /* scale the qdBm index up to the range of the table 0-40
 541          * where an offset of 40 qdBm equals a factor of 10 mW.
 542          */
 543         while (idx < 0) {
 544                 idx += 40;
 545                 factor *= 10;
 546         }
 547
 548         /* return the mW value scaled down to the correct factor of 10,
 549          * adding in factor/2 to get proper rounding.
 550          */
 551         return (nqdBm_to_mW_map[idx] + factor / 2) / factor;
 552 }
 553 EXPORT_SYMBOL(brcmu_qdbm_to_mw);
 554
 555 u8 brcmu_mw_to_qdbm(u16 mw)
 556 {
 557         u8 qdbm;
 558         int offset;
 559         uint mw_uint = mw;
 560         uint boundary;
 561
 562         /* handle boundary case */
 563         if (mw_uint <= 1)
 564                 return 0;
 565
 566         offset = QDBM_OFFSET;
 567
 568         /* move mw into the range of the table */
 569         while (mw_uint < QDBM_TABLE_LOW_BOUND) {
 570                 mw_uint *= 10;
 571                 offset -= 40;
 572         }
 573
 574         for (qdbm = 0; qdbm < QDBM_TABLE_LEN - 1; qdbm++) {
 575                 boundary = nqdBm_to_mW_map[qdbm] + (nqdBm_to_mW_map[qdbm + 1] -
 576                                                     nqdBm_to_mW_map[qdbm]) / 2;
 577                 if (mw_uint < boundary)
 578                         break;
 579         }
 580
 581         qdbm += (u8) offset;
 582
 583         return qdbm;
 584 }
 585 EXPORT_SYMBOL(brcmu_mw_to_qdbm);
 586
 587 uint brcmu_bitcount(u8 *bitmap, uint length)
 588 {
 589         uint bitcount = 0, i;
 590         u8 tmp;
 591         for (i = 0; i < length; i++) {
 592                 tmp = bitmap[i];
 593                 while (tmp) {
 594                         bitcount++;
 595                         tmp &= (tmp - 1);
 596                 }
 597         }
 598         return bitcount;
 599 }
 600 EXPORT_SYMBOL(brcmu_bitcount);