brcm80211/brcmfmac/flowring.c

0001 // SPDX-License-Identifier: ISC
0002 /*
0003  * Copyright (c) 2014 Broadcom Corporation
0004  */
0005
0006
0007 #include <linux/types.h>
0008 #include <linux/netdevice.h>
0009 #include <linux/etherdevice.h>
0010 #include <brcmu_utils.h>
0011
0012 #include "core.h"
0013 #include "debug.h"
0014 #include "bus.h"
0015 #include "proto.h"
0016 #include "flowring.h"
0017 #include "msgbuf.h"
0018 #include "common.h"
0019
0020
0021 #define BRCMF_FLOWRING_HIGH     1024
0022 #define BRCMF_FLOWRING_LOW      (BRCMF_FLOWRING_HIGH - 256)
0023 #define BRCMF_FLOWRING_INVALID_IFIDX    0xff
0024
0025 #define BRCMF_FLOWRING_HASH_AP(da, fifo, ifidx) (da[5] * 2 + fifo + ifidx * 16)
0026 #define BRCMF_FLOWRING_HASH_STA(fifo, ifidx) (fifo + ifidx * 16)
0027
0028 static const u8 brcmf_flowring_prio2fifo[] = {
0029     0,
0030     1,
0031     1,
0032     0,
0033     2,
0034     2,
0035     3,
0036     3
0037 };
0038
0039 static const u8 ALLFFMAC[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
0040
0041
0042 static bool
0043 brcmf_flowring_is_tdls_mac(struct brcmf_flowring *flow, u8 mac[ETH_ALEN])
0044 {
0045     struct brcmf_flowring_tdls_entry *search;
0046
0047     search = flow->tdls_entry;
0048
0049     while (search) {
0050         if (memcmp(search->mac, mac, ETH_ALEN) == 0)
0051             return true;
0052         search = search->next;
0053     }
0054
0055     return false;
0056 }
0057
0058
0059 u32 brcmf_flowring_lookup(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
0060               u8 prio, u8 ifidx)
0061 {
0062     struct brcmf_flowring_hash *hash;
0063     u16 hash_idx;
0064     u32 i;
0065     bool found;
0066     bool sta;
0067     u8 fifo;
0068     u8 *mac;
0069
0070     fifo = brcmf_flowring_prio2fifo[prio];
0071     sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
0072     mac = da;
0073     if ((!sta) && (is_multicast_ether_addr(da))) {
0074         mac = (u8 *)ALLFFMAC;
0075         fifo = 0;
0076     }
0077     if ((sta) && (flow->tdls_active) &&
0078         (brcmf_flowring_is_tdls_mac(flow, da))) {
0079         sta = false;
0080     }
0081     hash_idx =  sta ? BRCMF_FLOWRING_HASH_STA(fifo, ifidx) :
0082               BRCMF_FLOWRING_HASH_AP(mac, fifo, ifidx);
0083     hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
0084     found = false;
0085     hash = flow->hash;
0086     for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
0087         if ((sta || (memcmp(hash[hash_idx].mac, mac, ETH_ALEN) == 0)) &&
0088             (hash[hash_idx].fifo == fifo) &&
0089             (hash[hash_idx].ifidx == ifidx)) {
0090             found = true;
0091             break;
0092         }
0093         hash_idx++;
0094         hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
0095     }
0096     if (found)
0097         return hash[hash_idx].flowid;
0098
0099     return BRCMF_FLOWRING_INVALID_ID;
0100 }
0101
0102
0103 u32 brcmf_flowring_create(struct brcmf_flowring *flow, u8 da[ETH_ALEN],
0104               u8 prio, u8 ifidx)
0105 {
0106     struct brcmf_flowring_ring *ring;
0107     struct brcmf_flowring_hash *hash;
0108     u16 hash_idx;
0109     u32 i;
0110     bool found;
0111     u8 fifo;
0112     bool sta;
0113     u8 *mac;
0114
0115     fifo = brcmf_flowring_prio2fifo[prio];
0116     sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
0117     mac = da;
0118     if ((!sta) && (is_multicast_ether_addr(da))) {
0119         mac = (u8 *)ALLFFMAC;
0120         fifo = 0;
0121     }
0122     if ((sta) && (flow->tdls_active) &&
0123         (brcmf_flowring_is_tdls_mac(flow, da))) {
0124         sta = false;
0125     }
0126     hash_idx =  sta ? BRCMF_FLOWRING_HASH_STA(fifo, ifidx) :
0127               BRCMF_FLOWRING_HASH_AP(mac, fifo, ifidx);
0128     hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
0129     found = false;
0130     hash = flow->hash;
0131     for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
0132         if ((hash[hash_idx].ifidx == BRCMF_FLOWRING_INVALID_IFIDX) &&
0133             (is_zero_ether_addr(hash[hash_idx].mac))) {
0134             found = true;
0135             break;
0136         }
0137         hash_idx++;
0138         hash_idx &= (BRCMF_FLOWRING_HASHSIZE - 1);
0139     }
0140     if (found) {
0141         for (i = 0; i < flow->nrofrings; i++) {
0142             if (flow->rings[i] == NULL)
0143                 break;
0144         }
0145         if (i == flow->nrofrings)
0146             return -ENOMEM;
0147
0148         ring = kzalloc(sizeof(*ring), GFP_ATOMIC);
0149         if (!ring)
0150             return -ENOMEM;
0151
0152         memcpy(hash[hash_idx].mac, mac, ETH_ALEN);
0153         hash[hash_idx].fifo = fifo;
0154         hash[hash_idx].ifidx = ifidx;
0155         hash[hash_idx].flowid = i;
0156
0157         ring->hash_id = hash_idx;
0158         ring->status = RING_CLOSED;
0159         skb_queue_head_init(&ring->skblist);
0160         flow->rings[i] = ring;
0161
0162         return i;
0163     }
0164     return BRCMF_FLOWRING_INVALID_ID;
0165 }
0166
0167
0168 u8 brcmf_flowring_tid(struct brcmf_flowring *flow, u16 flowid)
0169 {
0170     struct brcmf_flowring_ring *ring;
0171
0172     ring = flow->rings[flowid];
0173
0174     return flow->hash[ring->hash_id].fifo;
0175 }
0176
0177
0178 static void brcmf_flowring_block(struct brcmf_flowring *flow, u16 flowid,
0179                  bool blocked)
0180 {
0181     struct brcmf_flowring_ring *ring;
0182     struct brcmf_bus *bus_if;
0183     struct brcmf_pub *drvr;
0184     struct brcmf_if *ifp;
0185     bool currently_blocked;
0186     int i;
0187     u8 ifidx;
0188     unsigned long flags;
0189
0190     spin_lock_irqsave(&flow->block_lock, flags);
0191
0192     ring = flow->rings[flowid];
0193     if (ring->blocked == blocked) {
0194         spin_unlock_irqrestore(&flow->block_lock, flags);
0195         return;
0196     }
0197     ifidx = brcmf_flowring_ifidx_get(flow, flowid);
0198
0199     currently_blocked = false;
0200     for (i = 0; i < flow->nrofrings; i++) {
0201         if ((flow->rings[i]) && (i != flowid)) {
0202             ring = flow->rings[i];
0203             if ((ring->status == RING_OPEN) &&
0204                 (brcmf_flowring_ifidx_get(flow, i) == ifidx)) {
0205                 if (ring->blocked) {
0206                     currently_blocked = true;
0207                     break;
0208                 }
0209             }
0210         }
0211     }
0212     flow->rings[flowid]->blocked = blocked;
0213     if (currently_blocked) {
0214         spin_unlock_irqrestore(&flow->block_lock, flags);
0215         return;
0216     }
0217
0218     bus_if = dev_get_drvdata(flow->dev);
0219     drvr = bus_if->drvr;
0220     ifp = brcmf_get_ifp(drvr, ifidx);
0221     brcmf_txflowblock_if(ifp, BRCMF_NETIF_STOP_REASON_FLOW, blocked);
0222
0223     spin_unlock_irqrestore(&flow->block_lock, flags);
0224 }
0225
0226
0227 void brcmf_flowring_delete(struct brcmf_flowring *flow, u16 flowid)
0228 {
0229     struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
0230     struct brcmf_flowring_ring *ring;
0231     struct brcmf_if *ifp;
0232     u16 hash_idx;
0233     u8 ifidx;
0234     struct sk_buff *skb;
0235
0236     ring = flow->rings[flowid];
0237     if (!ring)
0238         return;
0239
0240     ifidx = brcmf_flowring_ifidx_get(flow, flowid);
0241     ifp = brcmf_get_ifp(bus_if->drvr, ifidx);
0242
0243     brcmf_flowring_block(flow, flowid, false);
0244     hash_idx = ring->hash_id;
0245     flow->hash[hash_idx].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
0246     eth_zero_addr(flow->hash[hash_idx].mac);
0247     flow->rings[flowid] = NULL;
0248
0249     skb = skb_dequeue(&ring->skblist);
0250     while (skb) {
0251         brcmf_txfinalize(ifp, skb, false);
0252         skb = skb_dequeue(&ring->skblist);
0253     }
0254
0255     kfree(ring);
0256 }
0257
0258
0259 u32 brcmf_flowring_enqueue(struct brcmf_flowring *flow, u16 flowid,
0260                struct sk_buff *skb)
0261 {
0262     struct brcmf_flowring_ring *ring;
0263
0264     ring = flow->rings[flowid];
0265
0266     skb_queue_tail(&ring->skblist, skb);
0267
0268     if (!ring->blocked &&
0269         (skb_queue_len(&ring->skblist) > BRCMF_FLOWRING_HIGH)) {
0270         brcmf_flowring_block(flow, flowid, true);
0271         brcmf_dbg(MSGBUF, "Flowcontrol: BLOCK for ring %d\n", flowid);
0272         /* To prevent (work around) possible race condition, check
0273          * queue len again. It is also possible to use locking to
0274          * protect, but that is undesirable for every enqueue and
0275          * dequeue. This simple check will solve a possible race
0276          * condition if it occurs.
0277          */
0278         if (skb_queue_len(&ring->skblist) < BRCMF_FLOWRING_LOW)
0279             brcmf_flowring_block(flow, flowid, false);
0280     }
0281     return skb_queue_len(&ring->skblist);
0282 }
0283
0284
0285 struct sk_buff *brcmf_flowring_dequeue(struct brcmf_flowring *flow, u16 flowid)
0286 {
0287     struct brcmf_flowring_ring *ring;
0288     struct sk_buff *skb;
0289
0290     ring = flow->rings[flowid];
0291     if (ring->status != RING_OPEN)
0292         return NULL;
0293
0294     skb = skb_dequeue(&ring->skblist);
0295
0296     if (ring->blocked &&
0297         (skb_queue_len(&ring->skblist) < BRCMF_FLOWRING_LOW)) {
0298         brcmf_flowring_block(flow, flowid, false);
0299         brcmf_dbg(MSGBUF, "Flowcontrol: OPEN for ring %d\n", flowid);
0300     }
0301
0302     return skb;
0303 }
0304
0305
0306 void brcmf_flowring_reinsert(struct brcmf_flowring *flow, u16 flowid,
0307                  struct sk_buff *skb)
0308 {
0309     struct brcmf_flowring_ring *ring;
0310
0311     ring = flow->rings[flowid];
0312
0313     skb_queue_head(&ring->skblist, skb);
0314 }
0315
0316
0317 u32 brcmf_flowring_qlen(struct brcmf_flowring *flow, u16 flowid)
0318 {
0319     struct brcmf_flowring_ring *ring;
0320
0321     ring = flow->rings[flowid];
0322     if (!ring)
0323         return 0;
0324
0325     if (ring->status != RING_OPEN)
0326         return 0;
0327
0328     return skb_queue_len(&ring->skblist);
0329 }
0330
0331
0332 void brcmf_flowring_open(struct brcmf_flowring *flow, u16 flowid)
0333 {
0334     struct brcmf_flowring_ring *ring;
0335
0336     ring = flow->rings[flowid];
0337     if (!ring) {
0338         brcmf_err("Ring NULL, for flowid %d\n", flowid);
0339         return;
0340     }
0341
0342     ring->status = RING_OPEN;
0343 }
0344
0345
0346 u8 brcmf_flowring_ifidx_get(struct brcmf_flowring *flow, u16 flowid)
0347 {
0348     struct brcmf_flowring_ring *ring;
0349     u16 hash_idx;
0350
0351     ring = flow->rings[flowid];
0352     hash_idx = ring->hash_id;
0353
0354     return flow->hash[hash_idx].ifidx;
0355 }
0356
0357
0358 struct brcmf_flowring *brcmf_flowring_attach(struct device *dev, u16 nrofrings)
0359 {
0360     struct brcmf_flowring *flow;
0361     u32 i;
0362
0363     flow = kzalloc(sizeof(*flow), GFP_KERNEL);
0364     if (flow) {
0365         flow->dev = dev;
0366         flow->nrofrings = nrofrings;
0367         spin_lock_init(&flow->block_lock);
0368         for (i = 0; i < ARRAY_SIZE(flow->addr_mode); i++)
0369             flow->addr_mode[i] = ADDR_INDIRECT;
0370         for (i = 0; i < ARRAY_SIZE(flow->hash); i++)
0371             flow->hash[i].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
0372         flow->rings = kcalloc(nrofrings, sizeof(*flow->rings),
0373                       GFP_KERNEL);
0374         if (!flow->rings) {
0375             kfree(flow);
0376             flow = NULL;
0377         }
0378     }
0379
0380     return flow;
0381 }
0382
0383
0384 void brcmf_flowring_detach(struct brcmf_flowring *flow)
0385 {
0386     struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
0387     struct brcmf_pub *drvr = bus_if->drvr;
0388     struct brcmf_flowring_tdls_entry *search;
0389     struct brcmf_flowring_tdls_entry *remove;
0390     u16 flowid;
0391
0392     for (flowid = 0; flowid < flow->nrofrings; flowid++) {
0393         if (flow->rings[flowid])
0394             brcmf_msgbuf_delete_flowring(drvr, flowid);
0395     }
0396
0397     search = flow->tdls_entry;
0398     while (search) {
0399         remove = search;
0400         search = search->next;
0401         kfree(remove);
0402     }
0403     kfree(flow->rings);
0404     kfree(flow);
0405 }
0406
0407
0408 void brcmf_flowring_configure_addr_mode(struct brcmf_flowring *flow, int ifidx,
0409                     enum proto_addr_mode addr_mode)
0410 {
0411     struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
0412     struct brcmf_pub *drvr = bus_if->drvr;
0413     u32 i;
0414     u16 flowid;
0415
0416     if (flow->addr_mode[ifidx] != addr_mode) {
0417         for (i = 0; i < ARRAY_SIZE(flow->hash); i++) {
0418             if (flow->hash[i].ifidx == ifidx) {
0419                 flowid = flow->hash[i].flowid;
0420                 if (flow->rings[flowid]->status != RING_OPEN)
0421                     continue;
0422                 flow->rings[flowid]->status = RING_CLOSING;
0423                 brcmf_msgbuf_delete_flowring(drvr, flowid);
0424             }
0425         }
0426         flow->addr_mode[ifidx] = addr_mode;
0427     }
0428 }
0429
0430
0431 void brcmf_flowring_delete_peer(struct brcmf_flowring *flow, int ifidx,
0432                 u8 peer[ETH_ALEN])
0433 {
0434     struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
0435     struct brcmf_pub *drvr = bus_if->drvr;
0436     struct brcmf_flowring_hash *hash;
0437     struct brcmf_flowring_tdls_entry *prev;
0438     struct brcmf_flowring_tdls_entry *search;
0439     u32 i;
0440     u16 flowid;
0441     bool sta;
0442
0443     sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
0444
0445     search = flow->tdls_entry;
0446     prev = NULL;
0447     while (search) {
0448         if (memcmp(search->mac, peer, ETH_ALEN) == 0) {
0449             sta = false;
0450             break;
0451         }
0452         prev = search;
0453         search = search->next;
0454     }
0455
0456     hash = flow->hash;
0457     for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
0458         if ((sta || (memcmp(hash[i].mac, peer, ETH_ALEN) == 0)) &&
0459             (hash[i].ifidx == ifidx)) {
0460             flowid = flow->hash[i].flowid;
0461             if (flow->rings[flowid]->status == RING_OPEN) {
0462                 flow->rings[flowid]->status = RING_CLOSING;
0463                 brcmf_msgbuf_delete_flowring(drvr, flowid);
0464             }
0465         }
0466     }
0467
0468     if (search) {
0469         if (prev)
0470             prev->next = search->next;
0471         else
0472             flow->tdls_entry = search->next;
0473         kfree(search);
0474         if (flow->tdls_entry == NULL)
0475             flow->tdls_active = false;
0476     }
0477 }
0478
0479
0480 void brcmf_flowring_add_tdls_peer(struct brcmf_flowring *flow, int ifidx,
0481                   u8 peer[ETH_ALEN])
0482 {
0483     struct brcmf_flowring_tdls_entry *tdls_entry;
0484     struct brcmf_flowring_tdls_entry *search;
0485
0486     tdls_entry = kzalloc(sizeof(*tdls_entry), GFP_ATOMIC);
0487     if (tdls_entry == NULL)
0488         return;
0489
0490     memcpy(tdls_entry->mac, peer, ETH_ALEN);
0491     tdls_entry->next = NULL;
0492     if (flow->tdls_entry == NULL) {
0493         flow->tdls_entry = tdls_entry;
0494     } else {
0495         search = flow->tdls_entry;
0496         if (memcmp(search->mac, peer, ETH_ALEN) == 0)
0497             goto free_entry;
0498         while (search->next) {
0499             search = search->next;
0500             if (memcmp(search->mac, peer, ETH_ALEN) == 0)
0501                 goto free_entry;
0502         }
0503         search->next = tdls_entry;
0504     }
0505
0506     flow->tdls_active = true;
0507     return;
0508
0509 free_entry:
0510     kfree(tdls_entry);
0511 }