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 
24 MODULE_AUTHOR("Broadcom Corporation");
25 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
26 MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN cards");
27 MODULE_LICENSE("Dual BSD/GPL");
28 
29 struct 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 }
41 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
42 
43 /* Free the driver packet. Free the tag if present */
44 void 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 }
52 EXPORT_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  */
58 struct 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 }
75 EXPORT_SYMBOL(brcmu_pktq_penq);
76 
77 struct 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 }
94 EXPORT_SYMBOL(brcmu_pktq_penq_head);
95 
96 struct 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 }
109 EXPORT_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  */
117 struct 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 }
134 EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
135 
136 struct 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 }
149 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
150 
151 void
152 brcmu_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 }
167 EXPORT_SYMBOL(brcmu_pktq_pflush);
168 
169 void 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 }
176 EXPORT_SYMBOL(brcmu_pktq_flush);
177 
178 void 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 }
195 EXPORT_SYMBOL(brcmu_pktq_init);
196 
197 struct 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 }
213 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
214 
215 /* Return sum of lengths of a specific set of precedences */
216 int 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 }
228 EXPORT_SYMBOL(brcmu_pktq_mlen);
229 
230 /* Priority dequeue from a specific set of precedences */
231 struct 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 }
262 EXPORT_SYMBOL(brcmu_pktq_mdeq);
263 
264 /* Produce a human-readable string for boardrev */
265 char *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 }
278 EXPORT_SYMBOL(brcmu_boardrev_str);
279 
280 char *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 }
305 EXPORT_SYMBOL(brcmu_dotrev_str);
306 
307 #if defined(DEBUG)
308 /* pretty hex print a pkt buffer chain */
309 void 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 }
319 EXPORT_SYMBOL(brcmu_prpkt);
320 
321 void 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 }
337 EXPORT_SYMBOL(brcmu_dbg_hex_dump);
338 
339 #endif				/* defined(DEBUG) */
340