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_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 queue
44 * hi_prec is always >= the number of the highest non-empty precedence
45 */
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 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 */
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
139void
140brcmu_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	else
288		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