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
  20 #include <brcmu_utils.h>
  21
  22 MODULE_AUTHOR("Broadcom Corporation");
  23 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
  24 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
  25 MODULE_LICENSE("Dual BSD/GPL");
  26
  27 struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
  28 {
  29         struct sk_buff *skb;
  30
  31         skb = dev_alloc_skb(len);
  32         if (skb) {
  33                 skb_put(skb, len);
  34                 skb->priority = 0;
  35         }
  36
  37         return skb;
  38 }
  39 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
  40
  41 /* Free the driver packet. Free the tag if present */
  42 void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
  43 {
  44         WARN_ON(skb->next);
  45         if (skb->destructor)
  46                 /* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
  47                  * destructor exists
  48                  */
  49                 dev_kfree_skb_any(skb);
  50         else
  51                 /* can free immediately (even in_irq()) if destructor
  52                  * does not exist
  53                  */
  54                 dev_kfree_skb(skb);
  55 }
  56 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
  57
  58 /*
  59  * osl multiple-precedence packet queue
  60  * hi_prec is always >= the number of the highest non-empty precedence
  61  */
  62 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
  63                                       struct sk_buff *p)
  64 {
  65         struct sk_buff_head *q;
  66
  67         if (pktq_full(pq) || pktq_pfull(pq, prec))
  68                 return NULL;
  69
  70         q = &pq->q[prec].skblist;
  71         skb_queue_tail(q, p);
  72         pq->len++;
  73
  74         if (pq->hi_prec < prec)
  75                 pq->hi_prec = (u8) prec;
  76
  77         return p;
  78 }
  79 EXPORT_SYMBOL(brcmu_pktq_penq);
  80
  81 struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
  82                                            struct sk_buff *p)
  83 {
  84         struct sk_buff_head *q;
  85
  86         if (pktq_full(pq) || pktq_pfull(pq, prec))
  87                 return NULL;
  88
  89         q = &pq->q[prec].skblist;
  90         skb_queue_head(q, p);
  91         pq->len++;
  92
  93         if (pq->hi_prec < prec)
  94                 pq->hi_prec = (u8) prec;
  95
  96         return p;
  97 }
  98 EXPORT_SYMBOL(brcmu_pktq_penq_head);
  99
 100 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
 101 {
 102         struct sk_buff_head *q;
 103         struct sk_buff *p;
 104
 105         q = &pq->q[prec].skblist;
 106         p = skb_dequeue(q);
 107         if (p == NULL)
 108                 return NULL;
 109
 110         pq->len--;
 111         return p;
 112 }
 113 EXPORT_SYMBOL(brcmu_pktq_pdeq);
 114
 115 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 116 {
 117         struct sk_buff_head *q;
 118         struct sk_buff *p;
 119
 120         q = &pq->q[prec].skblist;
 121         p = skb_dequeue_tail(q);
 122         if (p == NULL)
 123                 return NULL;
 124
 125         pq->len--;
 126         return p;
 127 }
 128 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 129
 130 void
 131 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 132                   bool (*fn)(struct sk_buff *, void *), void *arg)
 133 {
 134         struct sk_buff_head *q;
 135         struct sk_buff *p, *next;
 136
 137         q = &pq->q[prec].skblist;
 138         skb_queue_walk_safe(q, p, next) {
 139                 if (fn == NULL || (*fn) (p, arg)) {
 140                         skb_unlink(p, q);
 141                         brcmu_pkt_buf_free_skb(p);
 142                         pq->len--;
 143                 }
 144         }
 145 }
 146 EXPORT_SYMBOL(brcmu_pktq_pflush);
 147
 148 void brcmu_pktq_flush(struct pktq *pq, bool dir,
 149                       bool (*fn)(struct sk_buff *, void *), void *arg)
 150 {
 151         int prec;
 152         for (prec = 0; prec < pq->num_prec; prec++)
 153                 brcmu_pktq_pflush(pq, prec, dir, fn, arg);
 154 }
 155 EXPORT_SYMBOL(brcmu_pktq_flush);
 156
 157 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
 158 {
 159         int prec;
 160
 161         /* pq is variable size; only zero out what's requested */
 162         memset(pq, 0,
 163               offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 164
 165         pq->num_prec = (u16) num_prec;
 166
 167         pq->max = (u16) max_len;
 168
 169         for (prec = 0; prec < num_prec; prec++) {
 170                 pq->q[prec].max = pq->max;
 171                 skb_queue_head_init(&pq->q[prec].skblist);
 172         }
 173 }
 174 EXPORT_SYMBOL(brcmu_pktq_init);
 175
 176 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
 177 {
 178         int prec;
 179
 180         if (pq->len == 0)
 181                 return NULL;
 182
 183         for (prec = 0; prec < pq->hi_prec; prec++)
 184                 if (!skb_queue_empty(&pq->q[prec].skblist))
 185                         break;
 186
 187         if (prec_out)
 188                 *prec_out = prec;
 189
 190         return skb_peek_tail(&pq->q[prec].skblist);
 191 }
 192 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 193
 194 /* Return sum of lengths of a specific set of precedences */
 195 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
 196 {
 197         int prec, len;
 198
 199         len = 0;
 200
 201         for (prec = 0; prec <= pq->hi_prec; prec++)
 202                 if (prec_bmp & (1 << prec))
 203                         len += pq->q[prec].skblist.qlen;
 204
 205         return len;
 206 }
 207 EXPORT_SYMBOL(brcmu_pktq_mlen);
 208
 209 /* Priority dequeue from a specific set of precedences */
 210 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 211                                       int *prec_out)
 212 {
 213         struct sk_buff_head *q;
 214         struct sk_buff *p;
 215         int prec;
 216
 217         if (pq->len == 0)
 218                 return NULL;
 219
 220         while ((prec = pq->hi_prec) > 0 &&
 221                skb_queue_empty(&pq->q[prec].skblist))
 222                 pq->hi_prec--;
 223
 224         while ((prec_bmp & (1 << prec)) == 0 ||
 225                skb_queue_empty(&pq->q[prec].skblist))
 226                 if (prec-- == 0)
 227                         return NULL;
 228
 229         q = &pq->q[prec].skblist;
 230         p = skb_dequeue(q);
 231         if (p == NULL)
 232                 return NULL;
 233
 234         pq->len--;
 235
 236         if (prec_out)
 237                 *prec_out = prec;
 238
 239         return p;
 240 }
 241 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 242
 243 #if defined(DEBUG)
 244 /* pretty hex print a pkt buffer chain */
 245 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 246 {
 247         struct sk_buff *p;
 248
 249         if (msg && (msg[0] != '\0'))
 250                 printk(KERN_DEBUG "%s:\n", msg);
 251
 252         for (p = p0; p; p = p->next)
 253                 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
 254 }
 255 EXPORT_SYMBOL(brcmu_prpkt);
 256
 257 void brcmu_dbg_hex_dump(const void *data, size_t size, const char *fmt, ...)
 258 {
 259         struct va_format vaf;
 260         va_list args;
 261
 262         va_start(args, fmt);
 263
 264         vaf.fmt = fmt;
 265         vaf.va = &args;
 266
 267         pr_debug("%pV", &vaf);
 268
 269         va_end(args);
 270
 271         print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size);
 272 }
 273 EXPORT_SYMBOL(brcmu_dbg_hex_dump);
 274 #endif                          /* defined(DEBUG) */