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