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 
13 MODULE_AUTHOR("Broadcom Corporation");
14 MODULE_DESCRIPTION("Broadcom 802.11n wireless LAN driver utilities.");
15 MODULE_LICENSE("Dual BSD/GPL");
16 
brcmu_pkt_buf_get_skb(uint len)17 struct 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 }
29 EXPORT_SYMBOL(brcmu_pkt_buf_get_skb);
30 
31 /* Free the driver packet. Free the tag if present */
brcmu_pkt_buf_free_skb(struct sk_buff * skb)32 void 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 }
40 EXPORT_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  */
brcmu_pktq_penq(struct pktq * pq,int prec,struct sk_buff * p)46 struct 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 }
63 EXPORT_SYMBOL(brcmu_pktq_penq);
64 
brcmu_pktq_penq_head(struct pktq * pq,int prec,struct sk_buff * p)65 struct 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 }
82 EXPORT_SYMBOL(brcmu_pktq_penq_head);
83 
brcmu_pktq_pdeq(struct pktq * pq,int prec)84 struct 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 }
97 EXPORT_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  */
brcmu_pktq_pdeq_match(struct pktq * pq,int prec,bool (* match_fn)(struct sk_buff * skb,void * arg),void * arg)105 struct 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 }
122 EXPORT_SYMBOL(brcmu_pktq_pdeq_match);
123 
brcmu_pktq_pdeq_tail(struct pktq * pq,int prec)124 struct 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 }
137 EXPORT_SYMBOL(brcmu_pktq_pdeq_tail);
138 
139 void
brcmu_pktq_pflush(struct pktq * pq,int prec,bool dir,bool (* fn)(struct sk_buff *,void *),void * arg)140 brcmu_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 }
155 EXPORT_SYMBOL(brcmu_pktq_pflush);
156 
brcmu_pktq_flush(struct pktq * pq,bool dir,bool (* fn)(struct sk_buff *,void *),void * arg)157 void 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 }
164 EXPORT_SYMBOL(brcmu_pktq_flush);
165 
brcmu_pktq_init(struct pktq * pq,int num_prec,int max_len)166 void 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 }
183 EXPORT_SYMBOL(brcmu_pktq_init);
184 
brcmu_pktq_peek_tail(struct pktq * pq,int * prec_out)185 struct sk_buff *brcmu_pktq_peek_tail(struct pktq *pq, int *prec_out)
186 {
187 	int prec;
188 
189 	if (pq->len == 0)
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 }
201 EXPORT_SYMBOL(brcmu_pktq_peek_tail);
202 
203 /* Return sum of lengths of a specific set of precedences */
brcmu_pktq_mlen(struct pktq * pq,uint prec_bmp)204 int 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 }
216 EXPORT_SYMBOL(brcmu_pktq_mlen);
217 
218 /* Priority dequeue from a specific set of precedences */
brcmu_pktq_mdeq(struct pktq * pq,uint prec_bmp,int * prec_out)219 struct 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 (pq->len == 0)
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 }
250 EXPORT_SYMBOL(brcmu_pktq_mdeq);
251 
252 /* Produce a human-readable string for boardrev */
brcmu_boardrev_str(u32 brev,char * buf)253 char *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 }
266 EXPORT_SYMBOL(brcmu_boardrev_str);
267 
brcmu_dotrev_str(u32 dotrev,char * buf)268 char *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 }
293 EXPORT_SYMBOL(brcmu_dotrev_str);
294 
295 #if defined(DEBUG)
296 /* pretty hex print a pkt buffer chain */
brcmu_prpkt(const char * msg,struct sk_buff * p0)297 void 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 }
307 EXPORT_SYMBOL(brcmu_prpkt);
308 
brcmu_dbg_hex_dump(const void * data,size_t size,const char * fmt,...)309 void 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 }
325 EXPORT_SYMBOL(brcmu_dbg_hex_dump);
326 
327 #endif				/* defined(DEBUG) */
328