drivers/net/wireless/broadcom/brcm80211/brcmutil/utils.c

   1 // SPDX-License-Identifier: ISC
   2 /*
   3  * Copyright (c) 2010 Broadcom Corporation
   4  */
   5
   6 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   7
   8 #include <linux/netdevice.h>
   9 #include <linux/module.h>
  10
  11 #include <brcmu_utils.h>
  12
  13 MODULE_AUTHOR("Broadcom Corporation");
  14 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
  15 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
  16 MODULE_LICENSE("Dual BSD/GPL");
  17
  18 struct sk_buff *brcmu_pkt_buf_get_skb(uint len)
  19 {
  20         struct sk_buff *skb;
  21
  22         skb = dev_alloc_skb(len);
  23         if (skb) {
  24                 skb_put(skb, len);
  25                 skb->priority = 0;
  26         }
  27
  28         return skb;
  29 }
  30 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
  31
  32 /* Free the driver packet. Free the tag if present */
  33 void brcmu_pkt_buf_free_skb(struct sk_buff *skb)
  34 {
  35         if (!skb)
  36                 return;
  37
  38         WARN_ON(skb->next);
  39         dev_kfree_skb_any(skb);
  40 }
  41 EXPORT_SYMBOL(brcmu_pkt_buf_free_skb);
  42
  43 /*
  44  * osl multiple-precedence packet queue
  45  * hi_prec is always >= the number of the highest non-empty precedence
  46  */
  47 struct sk_buff *brcmu_pktq_penq(struct pktq *pq, int prec,
  48                                       struct sk_buff *p)
  49 {
  50         struct sk_buff_head *q;
  51
  52         if (pktq_full(pq) || pktq_pfull(pq, prec))
  53                 return NULL;
  54
  55         q = &pq->q[prec].skblist;
  56         skb_queue_tail(q, p);
  57         pq->len++;
  58
  59         if (pq->hi_prec < prec)
  60                 pq->hi_prec = (u8) prec;
  61
  62         return p;
  63 }
  64 EXPORT_SYMBOL(brcmu_pktq_penq);
  65
  66 struct sk_buff *brcmu_pktq_penq_head(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_head(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_head);
  84
  85 struct sk_buff *brcmu_pktq_pdeq(struct pktq *pq, int prec)
  86 {
  87         struct sk_buff_head *q;
  88         struct sk_buff *p;
  89
  90         q = &pq->q[prec].skblist;
  91         p = skb_dequeue(q);
  92         if (p == NULL)
  93                 return NULL;
  94
  95         pq->len--;
  96         return p;
  97 }
  98 EXPORT_SYMBOL(brcmu_pktq_pdeq);
  99
 100 /*
 101  * precedence based dequeue with match function. Passing a NULL pointer
 102  * for the match function parameter is considered to be a wildcard so
 103  * any packet on the queue is returned. In that case it is no different
 104  * from brcmu_pktq_pdeq() above.
 105  */
 106 struct sk_buff *brcmu_pktq_pdeq_match(struct pktq *pq, int prec,
 107                                       bool (*match_fn)(struct sk_buff *skb,
 108                                                        void *arg), void *arg)
 109 {
 110         struct sk_buff_head *q;
 111         struct sk_buff *p, *next;
 112
 113         q = &pq->q[prec].skblist;
 114         skb_queue_walk_safe(q, p, next) {
 115                 if (match_fn == NULL || match_fn(p, arg)) {
 116                         skb_unlink(p, q);
 117                         pq->len--;
 118                         return p;
 119                 }
 120         }
 121         return NULL;
 122 }
 123 EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
 124
 125 struct sk_buff *brcmu_pktq_pdeq_tail(struct pktq *pq, int prec)
 126 {
 127         struct sk_buff_head *q;
 128         struct sk_buff *p;
 129
 130         q = &pq->q[prec].skblist;
 131         p = skb_dequeue_tail(q);
 132         if (p == NULL)
 133                 return NULL;
 134
 135         pq->len--;
 136         return p;
 137 }
 138 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 139
 140 void
 141 brcmu_pktq_pflush(struct pktq *pq, int prec, bool dir,
 142                   bool (*fn)(struct sk_buff *, void *), void *arg)
 143 {
 144         struct sk_buff_head *q;
 145         struct sk_buff *p, *next;
 146
 147         q = &pq->q[prec].skblist;
 148         skb_queue_walk_safe(q, p, next) {
 149                 if (fn == NULL || (*fn) (p, arg)) {
 150                         skb_unlink(p, q);
 151                         brcmu_pkt_buf_free_skb(p);
 152                         pq->len--;
 153                 }
 154         }
 155 }
 156 EXPORT_SYMBOL(brcmu_pktq_pflush);
 157
 158 void brcmu_pktq_flush(struct pktq *pq, bool dir,
 159                       bool (*fn)(struct sk_buff *, void *), void *arg)
 160 {
 161         int prec;
 162         for (prec = 0; prec < pq->num_prec; prec++)
 163                 brcmu_pktq_pflush(pq, prec, dir, fn, arg);
 164 }
 165 EXPORT_SYMBOL(brcmu_pktq_flush);
 166
 167 void brcmu_pktq_init(struct pktq *pq, int num_prec, int max_len)
 168 {
 169         int prec;
 170
 171         /* pq is variable size; only zero out what's requested */
 172         memset(pq, 0,
 173               offsetof(struct pktq, q) + (sizeof(struct pktq_prec) * num_prec));
 174
 175         pq->num_prec = (u16) num_prec;
 176
 177         pq->max = (u16) max_len;
 178
 179         for (prec = 0; prec < num_prec; prec++) {
 180                 pq->q[prec].max = pq->max;
 181                 skb_queue_head_init(&pq->q[prec].skblist);
 182         }
 183 }
 184 EXPORT_SYMBOL(brcmu_pktq_init);
 185
 186 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
 187 {
 188         int prec;
 189
 190         if (pq->len == 0)
 191                 return NULL;
 192
 193         for (prec = 0; prec < pq->hi_prec; prec++)
 194                 if (!skb_queue_empty(&pq->q[prec].skblist))
 195                         break;
 196
 197         if (prec_out)
 198                 *prec_out = prec;
 199
 200         return skb_peek_tail(&pq->q[prec].skblist);
 201 }
 202 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 203
 204 /* Return sum of lengths of a specific set of precedences */
 205 int brcmu_pktq_mlen(struct pktq *pq, uint prec_bmp)
 206 {
 207         int prec, len;
 208
 209         len = 0;
 210
 211         for (prec = 0; prec <= pq->hi_prec; prec++)
 212                 if (prec_bmp & (1 << prec))
 213                         len += pq->q[prec].skblist.qlen;
 214
 215         return len;
 216 }
 217 EXPORT_SYMBOL(brcmu_pktq_mlen);
 218
 219 /* Priority dequeue from a specific set of precedences */
 220 struct sk_buff *brcmu_pktq_mdeq(struct pktq *pq, uint prec_bmp,
 221                                       int *prec_out)
 222 {
 223         struct sk_buff_head *q;
 224         struct sk_buff *p;
 225         int prec;
 226
 227         if (pq->len == 0)
 228                 return NULL;
 229
 230         while ((prec = pq->hi_prec) > 0 &&
 231                skb_queue_empty(&pq->q[prec].skblist))
 232                 pq->hi_prec--;
 233
 234         while ((prec_bmp & (1 << prec)) == 0 ||
 235                skb_queue_empty(&pq->q[prec].skblist))
 236                 if (prec-- == 0)
 237                         return NULL;
 238
 239         q = &pq->q[prec].skblist;
 240         p = skb_dequeue(q);
 241         if (p == NULL)
 242                 return NULL;
 243
 244         pq->len--;
 245
 246         if (prec_out)
 247                 *prec_out = prec;
 248
 249         return p;
 250 }
 251 EXPORT_SYMBOL(brcmu_pktq_mdeq);
 252
 253 /* Produce a human-readable string for boardrev */
 254 char *brcmu_boardrev_str(u32 brev, char *buf)
 255 {
 256         char c;
 257
 258         if (brev < 0x100) {
 259                 snprintf(buf, BRCMU_BOARDREV_LEN, "%d.%d",
 260                          (brev & 0xf0) >> 4, brev & 0xf);
 261         } else {
 262                 c = (brev & 0xf000) == 0x1000 ? 'P' : 'A';
 263                 snprintf(buf, BRCMU_BOARDREV_LEN, "%c%03x", c, brev & 0xfff);
 264         }
 265         return buf;
 266 }
 267 EXPORT_SYMBOL(brcmu_boardrev_str);
 268
 269 char *brcmu_dotrev_str(u32 dotrev, char *buf)
 270 {
 271         u8 dotval[4];
 272
 273         if (!dotrev) {
 274                 snprintf(buf, BRCMU_DOTREV_LEN, "unknown");
 275                 return buf;
 276         }
 277         dotval[0] = (dotrev >> 24) & 0xFF;
 278         dotval[1] = (dotrev >> 16) & 0xFF;
 279         dotval[2] = (dotrev >> 8) & 0xFF;
 280         dotval[3] = dotrev & 0xFF;
 281
 282         if (dotval[3])
 283                 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d.%d.%d", dotval[0],
 284                         dotval[1], dotval[2], dotval[3]);
 285         else if (dotval[2])
 286                 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d.%d", dotval[0],
 287                         dotval[1], dotval[2]);
 288         else
 289                 snprintf(buf, BRCMU_DOTREV_LEN, "%d.%d", dotval[0],
 290                         dotval[1]);
 291
 292         return buf;
 293 }
 294 EXPORT_SYMBOL(brcmu_dotrev_str);
 295
 296 #if defined(DEBUG)
 297 /* pretty hex print a pkt buffer chain */
 298 void brcmu_prpkt(const char *msg, struct sk_buff *p0)
 299 {
 300         struct sk_buff *p;
 301
 302         if (msg && (msg[0] != '\0'))
 303                 pr_debug("%s:\n", msg);
 304
 305         for (p = p0; p; p = p->next)
 306                 print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, p->data, p->len);
 307 }
 308 EXPORT_SYMBOL(brcmu_prpkt);
 309
 310 void brcmu_dbg_hex_dump(const void *data, size_t size, const char *fmt, ...)
 311 {
 312         struct va_format vaf;
 313         va_list args;
 314
 315         va_start(args, fmt);
 316
 317         vaf.fmt = fmt;
 318         vaf.va = &args;
 319
 320         pr_debug("%pV", &vaf);
 321
 322         va_end(args);
 323
 324         print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, data, size);
 325 }
 326 EXPORT_SYMBOL(brcmu_dbg_hex_dump);
 327
 328 #endif                          /* defined(DEBUG) */