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