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