drivers/net/wireless/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 <linux/module.h>
  19 #include <brcmu_utils.h>
  20
  21 MODULE_AUTHOR("Broadcom Corporation");
  22 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
  23 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
  24 MODULE_LICENSE("Dual BSD/GPL");
  25
  26 struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
  27 {
  28         struct sk_buff *skb;
  29
  30         skb = dev_alloc_skb(len);
  31         if (skb) {
  32                 skb_put(skb, len);
  33                 skb->priority = 0;
  34         }
  35
  36         return skb;
  37 }
  38 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
  39
  40 /* Free the driver packet. Free the tag if present */
  41 void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
  42 {
  43         struct sk_buff *nskb;
  44         int nest = 0;
  45
  46         /* perversion: we use skb->next to chain multi-skb packets */
  47         while (skb) {
  48                 nskb = skb->next;
  49                 skb->next = NULL;
  50
  51                 if (skb->destructor)
  52                         /* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
  53                          * destructor exists
  54                          */
  55                         dev_kfree_skb_any(skb);
  56                 else
  57                         /* can free immediately (even in_irq()) if destructor
  58                          * does not exist
  59                          */
  60                         dev_kfree_skb(skb);
  61
  62                 nest++;
  63                 skb = nskb;
  64         }
  65 }
  66 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
  67
  68
  69 /* copy a buffer into a pkt buffer chain */
  70 uint brcmu_pktfrombuf(struct sk_buff *p, uint offset, int len,
  71                 unsigned char *buf)
  72 {
  73         uint n, ret = 0;
  74
  75         /* skip 'offset' bytes */
  76         for (; p && offset; p = p->next) {
  77                 if (offset < (uint) (p->len))
  78                         break;
  79                 offset -= p->len;
  80         }
  81
  82         if (!p)
  83                 return 0;
  84
  85         /* copy the data */
  86         for (; p && len; p = p->next) {
  87                 n = min((uint) (p->len) - offset, (uint) len);
  88                 memcpy(p->data + offset, buf, n);
  89                 buf += n;
  90                 len -= n;
  91                 ret += n;
  92                 offset = 0;
  93         }
  94
  95         return ret;
  96 }
  97 EXPORT_SYMBOL(brcmu_pktfrombuf);
  98
  99 /* return total length of buffer chain */
 100 uint brcmu_pkttotlen(struct sk_buff *p)
 101 {
 102         uint total;
 103
 104         total = 0;
 105         for (; p; p = p->next)
 106                 total += p->len;
 107         return total;
 108 }
 109 EXPORT_SYMBOL(brcmu_pkttotlen);
 110
 111 /*
 112  * osl multiple-precedence packet queue
 113  * hi_prec is always >= the number of the highest non-empty precedence
 114  */
 115 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
 116                                       struct sk_buff *p)
 117 {
 118         struct pktq_prec *q;
 119
 120         if (pktq_full(pq) || pktq_pfull(pq, prec))
 121                 return NULL;
 122
 123         q = &pq->q[prec];
 124
 125         if (q->head)
 126                 q->tail->prev = p;
 127         else
 128                 q->head = p;
 129
 130         q->tail = p;
 131         q->len++;
 132
 133         pq->len++;
 134
 135         if (pq->hi_prec < prec)
 136                 pq->hi_prec = (u8) prec;
 137
 138         return p;
 139 }
 140 EXPORT_SYMBOL(brcmu_pktq_penq);
 141
 142 struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
 143                                            struct sk_buff *p)
 144 {
 145         struct pktq_prec *q;
 146
 147         if (pktq_full(pq) || pktq_pfull(pq, prec))
 148                 return NULL;
 149
 150         q = &pq->q[prec];
 151
 152         if (q->head == NULL)
 153                 q->tail = p;
 154
 155         p->prev = q->head;
 156         q->head = p;
 157         q->len++;
 158
 159         pq->len++;
 160
 161         if (pq->hi_prec < prec)
 162                 pq->hi_prec = (u8) prec;
 163
 164         return p;
 165 }
 166 EXPORT_SYMBOL(brcmu_pktq_penq_head);
 167
 168 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
 169 {
 170         struct pktq_prec *q;
 171         struct sk_buff *p;
 172
 173         q = &pq->q[prec];
 174
 175         p = q->head;
 176         if (p == NULL)
 177                 return NULL;
 178
 179         q->head = p->prev;
 180         if (q->head == NULL)
 181                 q->tail = NULL;
 182
 183         q->len--;
 184
 185         pq->len--;
 186
 187         p->prev = NULL;
 188
 189         return p;
 190 }
 191 EXPORT_SYMBOL(brcmu_pktq_pdeq);
 192
 193 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 194 {
 195         struct pktq_prec *q;
 196         struct sk_buff *p, *prev;
 197
 198         q = &pq->q[prec];
 199
 200         p = q->head;
 201         if (p == NULL)
 202                 return NULL;
 203
 204         for (prev = NULL; p != q->tail; p = p->prev)
 205                 prev = p;
 206
 207         if (prev)
 208                 prev->prev = NULL;
 209         else
 210                 q->head = NULL;
 211
 212         q->tail = prev;
 213         q->len--;
 214
 215         pq->len--;
 216
 217         return p;
 218 }
 219 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 220
 221 void
 222 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 223                   bool (*fn)(struct sk_buff *, void *), void *arg)
 224 {
 225         struct pktq_prec *q;
 226         struct sk_buff *p, *prev = NULL;
 227
 228         q = &pq->q[prec];
 229         p = q->head;
 230         while (p) {
 231                 if (fn == NULL || (*fn) (p, arg)) {
 232                         bool head = (p == q->head);
 233                         if (head)
 234                                 q->head = p->prev;
 235                         else
 236                                 prev->prev = p->prev;
 237                         p->prev = NULL;
 238                         brcmu_pkt_buf_free_skb(p);
 239                         q->len--;
 240                         pq->len--;
 241                         p = (head ? q->head : prev->prev);
 242                 } else {
 243                         prev = p;
 244                         p = p->prev;
 245                 }
 246         }
 247
 248         if (q->head == NULL)
 249                 q->tail = NULL;
 250 }
 251 EXPORT_SYMBOL(brcmu_pktq_pflush);
 252
 253 void brcmu_pktq_flush(struct pktq *pq, bool dir,
 254                       bool (*fn)(struct sk_buff *, void *), void *arg)
 255 {
 256         int prec;
 257         for (prec = 0; prec < pq->num_prec; prec++)
 258                 brcmu_pktq_pflush(pq, prec, dir, fn, arg);
 259 }
 260 EXPORT_SYMBOL(brcmu_pktq_flush);
 261
 262 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
 263 {
 264         int prec;
 265
 266         /* pq is variable size; only zero out what's requested */
 267         memset(pq, 0,
 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 EXPORT_SYMBOL(brcmu_pktq_init);
 278
 279 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
 280 {
 281         int prec;
 282
 283         if (pq->len == 0)
 284                 return NULL;
 285
 286         for (prec = 0; prec < pq->hi_prec; prec++)
 287                 if (pq->q[prec].head)
 288                         break;
 289
 290         if (prec_out)
 291                 *prec_out = prec;
 292
 293         return pq->q[prec].tail;
 294 }
 295 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 296
 297 /* Return sum of lengths of a specific set of precedences */
 298 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
 299 {
 300         int prec, len;
 301
 302         len = 0;
 303
 304         for (prec = 0; prec <= pq->hi_prec; prec++)
 305                 if (prec_bmp & (1 << prec))
 306                         len += pq->q[prec].len;
 307
 308         return len;
 309 }
 310 EXPORT_SYMBOL(brcmu_pktq_mlen);
 311
 312 /* Priority dequeue from a specific set of precedences */
 313 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 314                                       int *prec_out)
 315 {
 316         struct pktq_prec *q;
 317         struct sk_buff *p;
 318         int prec;
 319
 320         if (pq->len == 0)
 321                 return NULL;
 322
 323         while ((prec = pq->hi_prec) > 0 && pq->q[prec].head == NULL)
 324                 pq->hi_prec--;
 325
 326         while ((prec_bmp & (1 << prec)) == 0 || pq->q[prec].head == NULL)
 327                 if (prec-- == 0)
 328                         return NULL;
 329
 330         q = &pq->q[prec];
 331
 332         p = q->head;
 333         if (p == NULL)
 334                 return NULL;
 335
 336         q->head = p->prev;
 337         if (q->head == NULL)
 338                 q->tail = NULL;
 339
 340         q->len--;
 341
 342         if (prec_out)
 343                 *prec_out = prec;
 344
 345         pq->len--;
 346
 347         p->prev = NULL;
 348
 349         return p;
 350 }
 351 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 352
 353 #if defined(BCMDBG)
 354 /* pretty hex print a pkt buffer chain */
 355 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 356 {
 357         struct sk_buff *p;
 358
 359         if (msg && (msg[0] != '\0'))
 360                 printk(KERN_DEBUG "%s:\n", msg);
 361
 362         for (p = p0; p; p = p->next)
 363                 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
 364 }
 365 EXPORT_SYMBOL(brcmu_prpkt);
 366 #endif                          /* defined(BCMDBG) */
 367
 368 #if defined(BCMDBG)
 369 /*
 370  * print bytes formatted as hex to a string. return the resulting
 371  * string length
 372  */
 373 int brcmu_format_hex(char *str, const void *bytes, int len)
 374 {
 375         int i;
 376         char *p = str;
 377         const u8 *src = (const u8 *)bytes;
 378
 379         for (i = 0; i < len; i++) {
 380                 p += snprintf(p, 3, "%02X", *src);
 381                 src++;
 382         }
 383         return (int)(p - str);
 384 }
 385 EXPORT_SYMBOL(brcmu_format_hex);
 386 #endif                          /* defined(BCMDBG) */