1 /* Copyright (c) 2014 Broadcom Corporation
2  *
3  * Permission to use, copy, modify, and/or distribute this software for any
4  * purpose with or without fee is hereby granted, provided that the above
5  * copyright notice and this permission notice appear in all copies.
6  *
7  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
8  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
9  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
10  * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
11  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
12  * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
13  * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
14  */
15 
16 
17 #include <linux/types.h>
18 #include <linux/netdevice.h>
19 #include <linux/etherdevice.h>
20 #include <brcmu_utils.h>
21 
22 #include "core.h"
23 #include "debug.h"
24 #include "bus.h"
25 #include "proto.h"
26 #include "flowring.h"
27 #include "msgbuf.h"
28 #include "common.h"
29 
30 
31 #define BRCMF_FLOWRING_HIGH		1024
32 #define BRCMF_FLOWRING_LOW		(BRCMF_FLOWRING_HIGH - 256)
33 #define BRCMF_FLOWRING_INVALID_IFIDX	0xff
34 
35 #define BRCMF_FLOWRING_HASH_AP(da, fifo, ifidx) (da[5] * 2 + fifo + ifidx * 16)
36 #define BRCMF_FLOWRING_HASH_STA(fifo, ifidx) (fifo + ifidx * 16)
37 
38 static const u8 brcmf_flowring_prio2fifo[] = {
39 	1,
40 	0,
41 	0,
42 	1,
43 	2,
44 	2,
45 	3,
46 	3
47 };
48 
49 
50 static bool
51 brcmf_flowring_is_tdls_mac(struct brcmf_flowring *flow, u8 mac[ETH_ALEN])
52 {
53 	struct brcmf_flowring_tdls_entry *search;
54 
55 	search = flow->tdls_entry;
56 
57 	while (search) {
58 		if (memcmp(search->mac, mac, ETH_ALEN) == 0)
59 			return true;
60 		search = search->next;
61 	}
62 
63 	return false;
64 }
65 
66 
67 u32 brcmf_flowring_lookup(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
68 			  u8 prio, u8 ifidx)
69 {
70 	struct brcmf_flowring_hash *hash;
71 	u16 hash_idx;
72 	u32 i;
73 	bool found;
74 	bool sta;
75 	u8 fifo;
76 	u8 *mac;
77 
78 	fifo = brcmf_flowring_prio2fifo[prio];
79 	sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
80 	mac = da;
81 	if ((!sta) && (is_multicast_ether_addr(da))) {
82 		mac = (u8 *)ALLFFMAC;
83 		fifo = 0;
84 	}
85 	if ((sta) && (flow->tdls_active) &&
86 	    (brcmf_flowring_is_tdls_mac(flow, da))) {
87 		sta = false;
88 	}
89 	hash_idx =  sta ? BRCMF_FLOWRING_HASH_STA(fifo, ifidx) :
90 			  BRCMF_FLOWRING_HASH_AP(mac, fifo, ifidx);
91 	hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
92 	found = false;
93 	hash = flow->hash;
94 	for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
95 		if ((sta || (memcmp(hash[hash_idx].mac, mac, ETH_ALEN) == 0)) &&
96 		    (hash[hash_idx].fifo == fifo) &&
97 		    (hash[hash_idx].ifidx == ifidx)) {
98 			found = true;
99 			break;
100 		}
101 		hash_idx++;
102 		hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
103 	}
104 	if (found)
105 		return hash[hash_idx].flowid;
106 
107 	return BRCMF_FLOWRING_INVALID_ID;
108 }
109 
110 
111 u32 brcmf_flowring_create(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
112 			  u8 prio, u8 ifidx)
113 {
114 	struct brcmf_flowring_ring *ring;
115 	struct brcmf_flowring_hash *hash;
116 	u16 hash_idx;
117 	u32 i;
118 	bool found;
119 	u8 fifo;
120 	bool sta;
121 	u8 *mac;
122 
123 	fifo = brcmf_flowring_prio2fifo[prio];
124 	sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
125 	mac = da;
126 	if ((!sta) && (is_multicast_ether_addr(da))) {
127 		mac = (u8 *)ALLFFMAC;
128 		fifo = 0;
129 	}
130 	if ((sta) && (flow->tdls_active) &&
131 	    (brcmf_flowring_is_tdls_mac(flow, da))) {
132 		sta = false;
133 	}
134 	hash_idx =  sta ? BRCMF_FLOWRING_HASH_STA(fifo, ifidx) :
135 			  BRCMF_FLOWRING_HASH_AP(mac, fifo, ifidx);
136 	hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
137 	found = false;
138 	hash = flow->hash;
139 	for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
140 		if ((hash[hash_idx].ifidx == BRCMF_FLOWRING_INVALID_IFIDX) &&
141 		    (is_zero_ether_addr(hash[hash_idx].mac))) {
142 			found = true;
143 			break;
144 		}
145 		hash_idx++;
146 		hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
147 	}
148 	if (found) {
149 		for (i = 0; i < flow->nrofrings; i++) {
150 			if (flow->rings[i] == NULL)
151 				break;
152 		}
153 		if (i == flow->nrofrings)
154 			return -ENOMEM;
155 
156 		ring = kzalloc(sizeof(*ring), GFP_ATOMIC);
157 		if (!ring)
158 			return -ENOMEM;
159 
160 		memcpy(hash[hash_idx].mac, mac, ETH_ALEN);
161 		hash[hash_idx].fifo = fifo;
162 		hash[hash_idx].ifidx = ifidx;
163 		hash[hash_idx].flowid = i;
164 
165 		ring->hash_id = hash_idx;
166 		ring->status = RING_CLOSED;
167 		skb_queue_head_init(&ring->skblist);
168 		flow->rings[i] = ring;
169 
170 		return i;
171 	}
172 	return BRCMF_FLOWRING_INVALID_ID;
173 }
174 
175 
176 u8 brcmf_flowring_tid(struct brcmf_flowring *flow, u16 flowid)
177 {
178 	struct brcmf_flowring_ring *ring;
179 
180 	ring = flow->rings[flowid];
181 
182 	return flow->hash[ring->hash_id].fifo;
183 }
184 
185 
186 static void brcmf_flowring_block(struct brcmf_flowring *flow, u16 flowid,
187 				 bool blocked)
188 {
189 	struct brcmf_flowring_ring *ring;
190 	struct brcmf_bus *bus_if;
191 	struct brcmf_pub *drvr;
192 	struct brcmf_if *ifp;
193 	bool currently_blocked;
194 	int i;
195 	u8 ifidx;
196 	unsigned long flags;
197 
198 	spin_lock_irqsave(&flow->block_lock, flags);
199 
200 	ring = flow->rings[flowid];
201 	if (ring->blocked == blocked) {
202 		spin_unlock_irqrestore(&flow->block_lock, flags);
203 		return;
204 	}
205 	ifidx = brcmf_flowring_ifidx_get(flow, flowid);
206 
207 	currently_blocked = false;
208 	for (i = 0; i < flow->nrofrings; i++) {
209 		if ((flow->rings[i]) && (i != flowid)) {
210 			ring = flow->rings[i];
211 			if ((ring->status == RING_OPEN) &&
212 			    (brcmf_flowring_ifidx_get(flow, i) == ifidx)) {
213 				if (ring->blocked) {
214 					currently_blocked = true;
215 					break;
216 				}
217 			}
218 		}
219 	}
220 	flow->rings[flowid]->blocked = blocked;
221 	if (currently_blocked) {
222 		spin_unlock_irqrestore(&flow->block_lock, flags);
223 		return;
224 	}
225 
226 	bus_if = dev_get_drvdata(flow->dev);
227 	drvr = bus_if->drvr;
228 	ifp = brcmf_get_ifp(drvr, ifidx);
229 	brcmf_txflowblock_if(ifp, BRCMF_NETIF_STOP_REASON_FLOW, blocked);
230 
231 	spin_unlock_irqrestore(&flow->block_lock, flags);
232 }
233 
234 
235 void brcmf_flowring_delete(struct brcmf_flowring *flow, u16 flowid)
236 {
237 	struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
238 	struct brcmf_flowring_ring *ring;
239 	struct brcmf_if *ifp;
240 	u16 hash_idx;
241 	u8 ifidx;
242 	struct sk_buff *skb;
243 
244 	ring = flow->rings[flowid];
245 	if (!ring)
246 		return;
247 
248 	ifidx = brcmf_flowring_ifidx_get(flow, flowid);
249 	ifp = brcmf_get_ifp(bus_if->drvr, ifidx);
250 
251 	brcmf_flowring_block(flow, flowid, false);
252 	hash_idx = ring->hash_id;
253 	flow->hash[hash_idx].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
254 	eth_zero_addr(flow->hash[hash_idx].mac);
255 	flow->rings[flowid] = NULL;
256 
257 	skb = skb_dequeue(&ring->skblist);
258 	while (skb) {
259 		brcmf_txfinalize(ifp, skb, false);
260 		skb = skb_dequeue(&ring->skblist);
261 	}
262 
263 	kfree(ring);
264 }
265 
266 
267 u32 brcmf_flowring_enqueue(struct brcmf_flowring *flow, u16 flowid,
268 			   struct sk_buff *skb)
269 {
270 	struct brcmf_flowring_ring *ring;
271 
272 	ring = flow->rings[flowid];
273 
274 	skb_queue_tail(&ring->skblist, skb);
275 
276 	if (!ring->blocked &&
277 	    (skb_queue_len(&ring->skblist) > BRCMF_FLOWRING_HIGH)) {
278 		brcmf_flowring_block(flow, flowid, true);
279 		brcmf_dbg(MSGBUF, "Flowcontrol: BLOCK for ring %d\n", flowid);
280 		/* To prevent (work around) possible race condition, check
281 		 * queue len again. It is also possible to use locking to
282 		 * protect, but that is undesirable for every enqueue and
283 		 * dequeue. This simple check will solve a possible race
284 		 * condition if it occurs.
285 		 */
286 		if (skb_queue_len(&ring->skblist) < BRCMF_FLOWRING_LOW)
287 			brcmf_flowring_block(flow, flowid, false);
288 	}
289 	return skb_queue_len(&ring->skblist);
290 }
291 
292 
293 struct sk_buff *brcmf_flowring_dequeue(struct brcmf_flowring *flow, u16 flowid)
294 {
295 	struct brcmf_flowring_ring *ring;
296 	struct sk_buff *skb;
297 
298 	ring = flow->rings[flowid];
299 	if (ring->status != RING_OPEN)
300 		return NULL;
301 
302 	skb = skb_dequeue(&ring->skblist);
303 
304 	if (ring->blocked &&
305 	    (skb_queue_len(&ring->skblist) < BRCMF_FLOWRING_LOW)) {
306 		brcmf_flowring_block(flow, flowid, false);
307 		brcmf_dbg(MSGBUF, "Flowcontrol: OPEN for ring %d\n", flowid);
308 	}
309 
310 	return skb;
311 }
312 
313 
314 void brcmf_flowring_reinsert(struct brcmf_flowring *flow, u16 flowid,
315 			     struct sk_buff *skb)
316 {
317 	struct brcmf_flowring_ring *ring;
318 
319 	ring = flow->rings[flowid];
320 
321 	skb_queue_head(&ring->skblist, skb);
322 }
323 
324 
325 u32 brcmf_flowring_qlen(struct brcmf_flowring *flow, u16 flowid)
326 {
327 	struct brcmf_flowring_ring *ring;
328 
329 	ring = flow->rings[flowid];
330 	if (!ring)
331 		return 0;
332 
333 	if (ring->status != RING_OPEN)
334 		return 0;
335 
336 	return skb_queue_len(&ring->skblist);
337 }
338 
339 
340 void brcmf_flowring_open(struct brcmf_flowring *flow, u16 flowid)
341 {
342 	struct brcmf_flowring_ring *ring;
343 
344 	ring = flow->rings[flowid];
345 	if (!ring) {
346 		brcmf_err("Ring NULL, for flowid %d\n", flowid);
347 		return;
348 	}
349 
350 	ring->status = RING_OPEN;
351 }
352 
353 
354 u8 brcmf_flowring_ifidx_get(struct brcmf_flowring *flow, u16 flowid)
355 {
356 	struct brcmf_flowring_ring *ring;
357 	u16 hash_idx;
358 
359 	ring = flow->rings[flowid];
360 	hash_idx = ring->hash_id;
361 
362 	return flow->hash[hash_idx].ifidx;
363 }
364 
365 
366 struct brcmf_flowring *brcmf_flowring_attach(struct device *dev, u16 nrofrings)
367 {
368 	struct brcmf_flowring *flow;
369 	u32 i;
370 
371 	flow = kzalloc(sizeof(*flow), GFP_KERNEL);
372 	if (flow) {
373 		flow->dev = dev;
374 		flow->nrofrings = nrofrings;
375 		spin_lock_init(&flow->block_lock);
376 		for (i = 0; i < ARRAY_SIZE(flow->addr_mode); i++)
377 			flow->addr_mode[i] = ADDR_INDIRECT;
378 		for (i = 0; i < ARRAY_SIZE(flow->hash); i++)
379 			flow->hash[i].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
380 		flow->rings = kcalloc(nrofrings, sizeof(*flow->rings),
381 				      GFP_KERNEL);
382 		if (!flow->rings) {
383 			kfree(flow);
384 			flow = NULL;
385 		}
386 	}
387 
388 	return flow;
389 }
390 
391 
392 void brcmf_flowring_detach(struct brcmf_flowring *flow)
393 {
394 	struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
395 	struct brcmf_pub *drvr = bus_if->drvr;
396 	struct brcmf_flowring_tdls_entry *search;
397 	struct brcmf_flowring_tdls_entry *remove;
398 	u16 flowid;
399 
400 	for (flowid = 0; flowid < flow->nrofrings; flowid++) {
401 		if (flow->rings[flowid])
402 			brcmf_msgbuf_delete_flowring(drvr, flowid);
403 	}
404 
405 	search = flow->tdls_entry;
406 	while (search) {
407 		remove = search;
408 		search = search->next;
409 		kfree(remove);
410 	}
411 	kfree(flow->rings);
412 	kfree(flow);
413 }
414 
415 
416 void brcmf_flowring_configure_addr_mode(struct brcmf_flowring *flow, int ifidx,
417 					enum proto_addr_mode addr_mode)
418 {
419 	struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
420 	struct brcmf_pub *drvr = bus_if->drvr;
421 	u32 i;
422 	u16 flowid;
423 
424 	if (flow->addr_mode[ifidx] != addr_mode) {
425 		for (i = 0; i < ARRAY_SIZE(flow->hash); i++) {
426 			if (flow->hash[i].ifidx == ifidx) {
427 				flowid = flow->hash[i].flowid;
428 				if (flow->rings[flowid]->status != RING_OPEN)
429 					continue;
430 				flow->rings[flowid]->status = RING_CLOSING;
431 				brcmf_msgbuf_delete_flowring(drvr, flowid);
432 			}
433 		}
434 		flow->addr_mode[ifidx] = addr_mode;
435 	}
436 }
437 
438 
439 void brcmf_flowring_delete_peer(struct brcmf_flowring *flow, int ifidx,
440 				u8 peer[ETH_ALEN])
441 {
442 	struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
443 	struct brcmf_pub *drvr = bus_if->drvr;
444 	struct brcmf_flowring_hash *hash;
445 	struct brcmf_flowring_tdls_entry *prev;
446 	struct brcmf_flowring_tdls_entry *search;
447 	u32 i;
448 	u16 flowid;
449 	bool sta;
450 
451 	sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
452 
453 	search = flow->tdls_entry;
454 	prev = NULL;
455 	while (search) {
456 		if (memcmp(search->mac, peer, ETH_ALEN) == 0) {
457 			sta = false;
458 			break;
459 		}
460 		prev = search;
461 		search = search->next;
462 	}
463 
464 	hash = flow->hash;
465 	for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
466 		if ((sta || (memcmp(hash[i].mac, peer, ETH_ALEN) == 0)) &&
467 		    (hash[i].ifidx == ifidx)) {
468 			flowid = flow->hash[i].flowid;
469 			if (flow->rings[flowid]->status == RING_OPEN) {
470 				flow->rings[flowid]->status = RING_CLOSING;
471 				brcmf_msgbuf_delete_flowring(drvr, flowid);
472 			}
473 		}
474 	}
475 
476 	if (search) {
477 		if (prev)
478 			prev->next = search->next;
479 		else
480 			flow->tdls_entry = search->next;
481 		kfree(search);
482 		if (flow->tdls_entry == NULL)
483 			flow->tdls_active = false;
484 	}
485 }
486 
487 
488 void brcmf_flowring_add_tdls_peer(struct brcmf_flowring *flow, int ifidx,
489 				  u8 peer[ETH_ALEN])
490 {
491 	struct brcmf_flowring_tdls_entry *tdls_entry;
492 	struct brcmf_flowring_tdls_entry *search;
493 
494 	tdls_entry = kzalloc(sizeof(*tdls_entry), GFP_ATOMIC);
495 	if (tdls_entry == NULL)
496 		return;
497 
498 	memcpy(tdls_entry->mac, peer, ETH_ALEN);
499 	tdls_entry->next = NULL;
500 	if (flow->tdls_entry == NULL) {
501 		flow->tdls_entry = tdls_entry;
502 	} else {
503 		search = flow->tdls_entry;
504 		if (memcmp(search->mac, peer, ETH_ALEN) == 0)
505 			goto free_entry;
506 		while (search->next) {
507 			search = search->next;
508 			if (memcmp(search->mac, peer, ETH_ALEN) == 0)
509 				goto free_entry;
510 		}
511 		search->next = tdls_entry;
512 	}
513 
514 	flow->tdls_active = true;
515 	return;
516 
517 free_entry:
518 	kfree(tdls_entry);
519 }
520