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_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
16MODULE_LICENSE("Dual BSD/GPL");
17
18struct 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}
30EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
31
32/* Free the driver packet. Free the tag if present */
33void 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}
41EXPORT_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 */
47struct 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}
64EXPORT_SYMBOL(brcmu_pktq_penq);
65
66struct 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}
83EXPORT_SYMBOL(brcmu_pktq_penq_head);
84
85struct 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}
98EXPORT_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 */
106struct 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}
123EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
124
125struct 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}
138EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
139
140void
141brcmu_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}
156EXPORT_SYMBOL(brcmu_pktq_pflush);
157
158void 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}
165EXPORT_SYMBOL(brcmu_pktq_flush);
166
167void 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}
184EXPORT_SYMBOL(brcmu_pktq_init);
185
186struct 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}
202EXPORT_SYMBOL(brcmu_pktq_peek_tail);
203
204/* Return sum of lengths of a specific set of precedences */
205int 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}
217EXPORT_SYMBOL(brcmu_pktq_mlen);
218
219/* Priority dequeue from a specific set of precedences */
220struct 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}
251EXPORT_SYMBOL(brcmu_pktq_mdeq);
252
253/* Produce a human-readable string for boardrev */
254char *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}
267EXPORT_SYMBOL(brcmu_boardrev_str);
268
269char *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}
294EXPORT_SYMBOL(brcmu_dotrev_str);
295
296#if defined(DEBUG)
297/* pretty hex print a pkt buffer chain */
298void 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}
308EXPORT_SYMBOL(brcmu_prpkt);
309
310void 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}
326EXPORT_SYMBOL(brcmu_dbg_hex_dump);
327
328#endif				/* defined(DEBUG) */
329