| 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_LICENSE("Dual BSD/GPL"); |
| 16 |
|
| 17 |
struct 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 |
} |
| 29 |
EXPORT_SYMBOL(brcmu_pkt_buf_get_skb); |
| 30 |
|
| 31 |
/* Free the driver packet. Free the tag if present */ |
| 32 |
void 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 |
} |
| 40 |
EXPORT_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 |
*/ |
| 46 |
struct 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 |
} |
| 63 |
EXPORT_SYMBOL(brcmu_pktq_penq); |
| 64 |
|
| 65 |
struct 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 |
} |
| 82 |
EXPORT_SYMBOL(brcmu_pktq_penq_head); |
| 83 |
|
| 84 |
struct 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 |
} |
| 97 |
EXPORT_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 |
*/ |
| 105 |
struct 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 |
} |
| 122 |
EXPORT_SYMBOL(brcmu_pktq_pdeq_match); |
| 123 |
|
| 124 |
struct 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 |
} |
| 137 |
EXPORT_SYMBOL(brcmu_pktq_pdeq_tail); |
| 138 |
|
| 139 |
void |
| 140 |
brcmu_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 |
} |
| 155 |
EXPORT_SYMBOL(brcmu_pktq_pflush); |
| 156 |
|
| 157 |
void 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 |
} |
| 164 |
EXPORT_SYMBOL(brcmu_pktq_flush); |
| 165 |
|
| 166 |
void 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 |
} |
| 183 |
EXPORT_SYMBOL(brcmu_pktq_init); |
| 184 |
|
| 185 |
struct 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 |
} |
| 201 |
EXPORT_SYMBOL(brcmu_pktq_peek_tail); |
| 202 |
|
| 203 |
/* Return sum of lengths of a specific set of precedences */ |
| 204 |
int 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 |
} |
| 216 |
EXPORT_SYMBOL(brcmu_pktq_mlen); |
| 217 |
|
| 218 |
/* Priority dequeue from a specific set of precedences */ |
| 219 |
struct 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 |
} |
| 250 |
EXPORT_SYMBOL(brcmu_pktq_mdeq); |
| 251 |
|
| 252 |
/* Produce a human-readable string for boardrev */ |
| 253 |
char *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 |
} |
| 266 |
EXPORT_SYMBOL(brcmu_boardrev_str); |
| 267 |
|
| 268 |
char *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 |
} |
| 293 |
EXPORT_SYMBOL(brcmu_dotrev_str); |
| 294 |
|
| 295 |
#if defined(DEBUG) |
| 296 |
/* pretty hex print a pkt buffer chain */ |
| 297 |
void 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 |
} |
| 307 |
EXPORT_SYMBOL(brcmu_prpkt); |
| 308 |
|
| 309 |
void 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 |
} |
| 325 |
EXPORT_SYMBOL(brcmu_dbg_hex_dump); |
| 326 |
|
| 327 |
#endif /* defined(DEBUG) */ |