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         if (!skb)
  47                 return;
  48         WARN_ON(skb->next);
  49         if (skb->destructor)
  50                 /* cannot kfree_skb() on hard IRQ (net/core/skbuff.c) if
  51                  * destructor exists
  52                  */
  53                 dev_kfree_skb_any(skb);
  54         else
  55                 /* can free immediately (even in_irq()) if destructor
  56                  * does not exist
  57                  */
  58                 dev_kfree_skb(skb);
  59 }
  60 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
  61
  62 /*
  63  * osl multiple-precedence packet queue
  64  * hi_prec is always >= the number of the highest non-empty precedence
  65  */
  66 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
  67                                       struct sk_buff *p)
  68 {
  69         struct sk_buff_head *q;
  70
  71         if (pktq_full(pq) || pktq_pfull(pq, prec))
  72                 return NULL;
  73
  74         q = &pq->q[prec].skblist;
  75         skb_queue_tail(q, p);
  76         pq->len++;
  77
  78         if (pq->hi_prec < prec)
  79                 pq->hi_prec = (u8) prec;
  80
  81         return p;
  82 }
  83 EXPORT_SYMBOL(brcmu_pktq_penq);
  84
  85 struct sk_buff *brcmu_pktq_penq_head(struct pktq *pq, int prec,
  86                                            struct sk_buff *p)
  87 {
  88         struct sk_buff_head *q;
  89
  90         if (pktq_full(pq) || pktq_pfull(pq, prec))
  91                 return NULL;
  92
  93         q = &pq->q[prec].skblist;
  94         skb_queue_head(q, p);
  95         pq->len++;
  96
  97         if (pq->hi_prec < prec)
  98                 pq->hi_prec = (u8) prec;
  99
 100         return p;
 101 }
 102 EXPORT_SYMBOL(brcmu_pktq_penq_head);
 103
 104 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
 105 {
 106         struct sk_buff_head *q;
 107         struct sk_buff *p;
 108
 109         q = &pq->q[prec].skblist;
 110         p = skb_dequeue(q);
 111         if (p == NULL)
 112                 return NULL;
 113
 114         pq->len--;
 115         return p;
 116 }
 117 EXPORT_SYMBOL(brcmu_pktq_pdeq);
 118
 119 /*
 120  * precedence based dequeue with match function. Passing a NULL pointer
 121  * for the match function parameter is considered to be a wildcard so
 122  * any packet on the queue is returned. In that case it is no different
 123  * from brcmu_pktq_pdeq() above.
 124  */
 125 struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec,
 126                                       bool (*match_fn)(struct sk_buff *skb,
 127                                                        void *arg), void *arg)
 128 {
 129         struct sk_buff_head *q;
 130         struct sk_buff *p, *next;
 131
 132         q = &pq->q[prec].skblist;
 133         skb_queue_walk_safe(q, p, next) {
 134                 if (match_fn == NULL || match_fn(p, arg)) {
 135                         skb_unlink(p, q);
 136                         pq->len--;
 137                         return p;
 138                 }
 139         }
 140         return NULL;
 141 }
 142 EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
 143
 144 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 145 {
 146         struct sk_buff_head *q;
 147         struct sk_buff *p;
 148
 149         q = &pq->q[prec].skblist;
 150         p = skb_dequeue_tail(q);
 151         if (p == NULL)
 152                 return NULL;
 153
 154         pq->len--;
 155         return p;
 156 }
 157 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 158
 159 void
 160 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 161                   bool (*fn)(struct sk_buff *, void *), void *arg)
 162 {
 163         struct sk_buff_head *q;
 164         struct sk_buff *p, *next;
 165
 166         q = &pq->q[prec].skblist;
 167         skb_queue_walk_safe(q, p, next) {
 168                 if (fn == NULL || (*fn) (p, arg)) {
 169                         skb_unlink(p, q);
 170                         brcmu_pkt_buf_free_skb(p);
 171                         pq->len--;
 172                 }
 173         }
 174 }
 175 EXPORT_SYMBOL(brcmu_pktq_pflush);
 176
 177 void brcmu_pktq_flush(struct pktq *pq, bool dir,
 178                       bool (*fn)(struct sk_buff *, void *), void *arg)
 179 {
 180         int prec;
 181         for (prec = 0; prec < pq->num_prec; prec++)
 182                 brcmu_pktq_pflush(pq, prec, dir, fn, arg);
 183 }
 184 EXPORT_SYMBOL(brcmu_pktq_flush);
 185
 186 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
 187 {
 188         int prec;
 189
 190         /* pq is variable size; only zero out what's requested */
 191         memset(pq, 0,
 192               offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 193
 194         pq->num_prec = (u16) num_prec;
 195
 196         pq->max = (u16) max_len;
 197
 198         for (prec = 0; prec < num_prec; prec++) {
 199                 pq->q[prec].max = pq->max;
 200                 skb_queue_head_init(&pq->q[prec].skblist);
 201         }
 202 }
 203 EXPORT_SYMBOL(brcmu_pktq_init);
 204
 205 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
 206 {
 207         int prec;
 208
 209         if (pq->len == 0)
 210                 return NULL;
 211
 212         for (prec = 0; prec < pq->hi_prec; prec++)
 213                 if (!skb_queue_empty(&pq->q[prec].skblist))
 214                         break;
 215
 216         if (prec_out)
 217                 *prec_out = prec;
 218
 219         return skb_peek_tail(&pq->q[prec].skblist);
 220 }
 221 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 222
 223 /* Return sum of lengths of a specific set of precedences */
 224 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
 225 {
 226         int prec, len;
 227
 228         len = 0;
 229
 230         for (prec = 0; prec <= pq->hi_prec; prec++)
 231                 if (prec_bmp & (1 << prec))
 232                         len += pq->q[prec].skblist.qlen;
 233
 234         return len;
 235 }
 236 EXPORT_SYMBOL(brcmu_pktq_mlen);
 237
 238 /* Priority dequeue from a specific set of precedences */
 239 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 240                                       int *prec_out)
 241 {
 242         struct sk_buff_head *q;
 243         struct sk_buff *p;
 244         int prec;
 245
 246         if (pq->len == 0)
 247                 return NULL;
 248
 249         while ((prec = pq->hi_prec) > 0 &&
 250                skb_queue_empty(&pq->q[prec].skblist))
 251                 pq->hi_prec--;
 252
 253         while ((prec_bmp & (1 << prec)) == 0 ||
 254                skb_queue_empty(&pq->q[prec].skblist))
 255                 if (prec-- == 0)
 256                         return NULL;
 257
 258         q = &pq->q[prec].skblist;
 259         p = skb_dequeue(q);
 260         if (p == NULL)
 261                 return NULL;
 262
 263         pq->len--;
 264
 265         if (prec_out)
 266                 *prec_out = prec;
 267
 268         return p;
 269 }
 270 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 271
 272 #if defined(DEBUG)
 273 /* pretty hex print a pkt buffer chain */
 274 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 275 {
 276         struct sk_buff *p;
 277
 278         if (msg && (msg[0] != '\0'))
 279                 pr_debug("%s:\n", msg);
 280
 281         for (p = p0; p; p = p->next)
 282                 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
 283 }
 284 EXPORT_SYMBOL(brcmu_prpkt);
 285
 286 void brcmu_dbg_hex_dump(const void *data, size_t size, const char *fmt, ...)
 287 {
 288         struct va_format vaf;
 289         va_list args;
 290
 291         va_start(args, fmt);
 292
 293         vaf.fmt = fmt;
 294         vaf.va = &args;
 295
 296         pr_debug("%pV", &vaf);
 297
 298         va_end(args);
 299
 300         print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size);
 301 }
 302 EXPORT_SYMBOL(brcmu_dbg_hex_dump);
 303 #endif                          /* defined(DEBUG) */