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
brcmu_pkt_buf_get_skb(uint len)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 */
brcmu_pkt_buf_free_skb(struct sk_buff * skb)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 */
brcmu_pktq_penq(struct pktq * pq,int prec,struct sk_buff * p)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
brcmu_pktq_penq_head(struct pktq * pq,int prec,struct sk_buff * p)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
brcmu_pktq_pdeq(struct pktq * pq,int prec)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 */
brcmu_pktq_pdeq_match(struct pktq * pq,int prec,bool (* match_fn)(struct sk_buff * skb,void * arg),void * arg)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
brcmu_pktq_pdeq_tail(struct pktq * pq,int prec)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
brcmu_pktq_pflush(struct pktq * pq,int prec,bool dir,bool (* fn)(struct sk_buff *,void *),void * arg)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
brcmu_pktq_flush(struct pktq * pq,bool dir,bool (* fn)(struct sk_buff *,void *),void * arg)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
brcmu_pktq_init(struct pktq * pq,int num_prec,int max_len)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
brcmu_pktq_peek_tail(struct pktq * pq,int * prec_out)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 */
brcmu_pktq_mlen(struct pktq * pq,uint prec_bmp)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 */
brcmu_pktq_mdeq(struct pktq * pq,uint prec_bmp,int * prec_out)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 */
brcmu_boardrev_str(u32 brev,char * buf)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
brcmu_dotrev_str(u32 dotrev,char * buf)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 */
brcmu_prpkt(const char * msg,struct sk_buff * p0)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
brcmu_dbg_hex_dump(const void * data,size_t size,const char * fmt,...)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) */
329