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