328 lines · c
1// SPDX-License-Identifier: ISC2/*3 * Copyright (c) 2010 Broadcom Corporation4 */5 6#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt7 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 queue44 * hi_prec is always >= the number of the highest non-empty precedence45 */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 pointer101 * for the match function parameter is considered to be a wildcard so102 * any packet on the queue is returned. In that case it is no different103 * 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 139void140brcmu_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 (pktq_empty(pq))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 (pktq_empty(pq))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 else288 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