drivers/net/wireless/broadcom/brcm80211/brcmfmac/flowring.c

   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
  28 static const u8 brcmf_flowring_prio2fifo[] = {
  29         1,
  30         0,
  31         0,
  32         1,
  33         2,
  34         2,
  35         3,
  36         3
  37 };
  38
  39 static const u8 ALLFFMAC[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
  40
  41
  42 static bool
  43 brcmf_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
  59 u32 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
 103 u32 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
 168 u8 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
 178 static 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
 227 void 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
 259 u32 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
 285 struct 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
 306 void 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
 317 u32 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
 332 void 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
 346 u8 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
 358 struct 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
 384 void 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
 408 void 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                                 flow->rings[flowid]->status = RING_CLOSING;
 423                                 brcmf_msgbuf_delete_flowring(drvr, flowid);
 424                         }
 425                 }
 426                 flow->addr_mode[ifidx] = addr_mode;
 427         }
 428 }
 429
 430
 431 void brcmf_flowring_delete_peer(struct brcmf_flowring *flow, int ifidx,
 432                                 u8 peer[ETH_ALEN])
 433 {
 434         struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev);
 435         struct brcmf_pub *drvr = bus_if->drvr;
 436         struct brcmf_flowring_hash *hash;
 437         struct brcmf_flowring_tdls_entry *prev;
 438         struct brcmf_flowring_tdls_entry *search;
 439         u32 i;
 440         u16 flowid;
 441         bool sta;
 442
 443         sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT);
 444
 445         search = flow->tdls_entry;
 446         prev = NULL;
 447         while (search) {
 448                 if (memcmp(search->mac, peer, ETH_ALEN) == 0) {
 449                         sta = false;
 450                         break;
 451                 }
 452                 prev = search;
 453                 search = search->next;
 454         }
 455
 456         hash = flow->hash;
 457         for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) {
 458                 if ((sta || (memcmp(hash[i].mac, peer, ETH_ALEN) == 0)) &&
 459                     (hash[i].ifidx == ifidx)) {
 460                         flowid = flow->hash[i].flowid;
 461                         if (flow->rings[flowid]->status == RING_OPEN) {
 462                                 flow->rings[flowid]->status = RING_CLOSING;
 463                                 brcmf_msgbuf_delete_flowring(drvr, flowid);
 464                         }
 465                 }
 466         }
 467
 468         if (search) {
 469                 if (prev)
 470                         prev->next = search->next;
 471                 else
 472                         flow->tdls_entry = search->next;
 473                 kfree(search);
 474                 if (flow->tdls_entry == NULL)
 475                         flow->tdls_active = false;
 476         }
 477 }
 478
 479
 480 void brcmf_flowring_add_tdls_peer(struct brcmf_flowring *flow, int ifidx,
 481                                   u8 peer[ETH_ALEN])
 482 {
 483         struct brcmf_flowring_tdls_entry *tdls_entry;
 484         struct brcmf_flowring_tdls_entry *search;
 485
 486         tdls_entry = kzalloc(sizeof(*tdls_entry), GFP_ATOMIC);
 487         if (tdls_entry == NULL)
 488                 return;
 489
 490         memcpy(tdls_entry->mac, peer, ETH_ALEN);
 491         tdls_entry->next = NULL;
 492         if (flow->tdls_entry == NULL) {
 493                 flow->tdls_entry = tdls_entry;
 494         } else {
 495                 search = flow->tdls_entry;
 496                 if (memcmp(search->mac, peer, ETH_ALEN) == 0)
 497                         goto free_entry;
 498                 while (search->next) {
 499                         search = search->next;
 500                         if (memcmp(search->mac, peer, ETH_ALEN) == 0)
 501                                 goto free_entry;
 502                 }
 503                 search->next = tdls_entry;
 504         }
 505
 506         flow->tdls_active = true;
 507         return;
 508
 509 free_entry:
 510         kfree(tdls_entry);
 511 }