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 <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 #if defined(BCMDBG)
 368 /*
 369  * print bytes formatted as hex to a string. return the resulting
 370  * string length
 371  */
 372 int brcmu_format_hex(char *str, const void *bytes, int len)
 373 {
 374         int i;
 375         char *p = str;
 376         const u8 *src = (const u8 *)bytes;
 377
 378         for (i = 0; i < len; i++) {
 379                 p += snprintf(p, 3, "%02X", *src);
 380                 src++;
 381         }
 382         return (int)(p - str);
 383 }
 384 EXPORT_SYMBOL(brcmu_format_hex);
 385 #endif                          /* defined(BCMDBG) */