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