LXR linux/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
  13MODULE_AUTHOR("Broadcom Corporation");
  14MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
  15MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
  16MODULE_LICENSE("Dual BSD/GPL");
  17
  18struct 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}
  30EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
  31
  32/* Free the driver packet. Free the tag if present */
  33void 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}
  41EXPORT_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 */
  47struct 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}
  64EXPORT_SYMBOL(brcmu_pktq_penq);
  65
  66struct 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}
  83EXPORT_SYMBOL(brcmu_pktq_penq_head);
  84
  85struct 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}
  98EXPORT_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 */
 106struct 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}
 123EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
 124
 125struct 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}
 138EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
 139
 140void
 141brcmu_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}
 156EXPORT_SYMBOL(brcmu_pktq_pflush);
 157
 158void 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}
 165EXPORT_SYMBOL(brcmu_pktq_flush);
 166
 167void 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}
 184EXPORT_SYMBOL(brcmu_pktq_init);
 185
 186struct 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}
 202EXPORT_SYMBOL(brcmu_pktq_peek_tail);
 203
 204/* Return sum of lengths of a specific set of precedences */
 205int 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}
 217EXPORT_SYMBOL(brcmu_pktq_mlen);
 218
 219/* Priority dequeue from a specific set of precedences */
 220struct 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}
 251EXPORT_SYMBOL(brcmu_pktq_mdeq);
 252
 253/* Produce a human-readable string for boardrev */
 254char *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}
 267EXPORT_SYMBOL(brcmu_boardrev_str);
 268
 269char *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}
 294EXPORT_SYMBOL(brcmu_dotrev_str);
 295
 296#if defined(DEBUG)
 297/* pretty hex print a pkt buffer chain */
 298void 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}
 308EXPORT_SYMBOL(brcmu_prpkt);
 309
 310void 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}
 326EXPORT_SYMBOL(brcmu_dbg_hex_dump);
 327
 328#endif                          /* defined(DEBUG) */
 329