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