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 0, 30 1, 31 1, 32 0, 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_obj(*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_flex(*flow, rings, nrofrings); 364 if (flow) { 365 flow->nrofrings = nrofrings; 366 flow->dev = dev; 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 } 373 374 return flow; 375 } 376 377 378 void brcmf_flowring_detach(struct brcmf_flowring *flow) 379 { 380 struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev); 381 struct brcmf_pub *drvr = bus_if->drvr; 382 struct brcmf_flowring_tdls_entry *search; 383 struct brcmf_flowring_tdls_entry *remove; 384 u16 flowid; 385 386 for (flowid = 0; flowid < flow->nrofrings; flowid++) { 387 if (flow->rings[flowid]) 388 brcmf_msgbuf_delete_flowring(drvr, flowid); 389 } 390 391 search = flow->tdls_entry; 392 while (search) { 393 remove = search; 394 search = search->next; 395 kfree(remove); 396 } 397 kfree(flow); 398 } 399 400 401 void brcmf_flowring_configure_addr_mode(struct brcmf_flowring *flow, int ifidx, 402 enum proto_addr_mode addr_mode) 403 { 404 struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev); 405 struct brcmf_pub *drvr = bus_if->drvr; 406 u32 i; 407 u16 flowid; 408 409 if (flow->addr_mode[ifidx] != addr_mode) { 410 for (i = 0; i < ARRAY_SIZE(flow->hash); i++) { 411 if (flow->hash[i].ifidx == ifidx) { 412 flowid = flow->hash[i].flowid; 413 if (flow->rings[flowid]->status != RING_OPEN) 414 continue; 415 brcmf_msgbuf_delete_flowring(drvr, flowid); 416 } 417 } 418 flow->addr_mode[ifidx] = addr_mode; 419 } 420 } 421 422 423 void brcmf_flowring_delete_peer(struct brcmf_flowring *flow, int ifidx, 424 u8 peer[ETH_ALEN]) 425 { 426 struct brcmf_bus *bus_if = dev_get_drvdata(flow->dev); 427 struct brcmf_pub *drvr = bus_if->drvr; 428 struct brcmf_flowring_hash *hash; 429 struct brcmf_flowring_tdls_entry *prev; 430 struct brcmf_flowring_tdls_entry *search; 431 u32 i; 432 u16 flowid; 433 bool sta; 434 435 sta = (flow->addr_mode[ifidx] == ADDR_INDIRECT); 436 437 search = flow->tdls_entry; 438 prev = NULL; 439 while (search) { 440 if (memcmp(search->mac, peer, ETH_ALEN) == 0) { 441 sta = false; 442 break; 443 } 444 prev = search; 445 search = search->next; 446 } 447 448 hash = flow->hash; 449 for (i = 0; i < BRCMF_FLOWRING_HASHSIZE; i++) { 450 if ((sta || (memcmp(hash[i].mac, peer, ETH_ALEN) == 0)) && 451 (hash[i].ifidx == ifidx)) { 452 flowid = flow->hash[i].flowid; 453 if (flow->rings[flowid]->status == RING_OPEN) 454 brcmf_msgbuf_delete_flowring(drvr, flowid); 455 } 456 } 457 458 if (search) { 459 if (prev) 460 prev->next = search->next; 461 else 462 flow->tdls_entry = search->next; 463 kfree(search); 464 if (flow->tdls_entry == NULL) 465 flow->tdls_active = false; 466 } 467 } 468 469 470 void brcmf_flowring_add_tdls_peer(struct brcmf_flowring *flow, int ifidx, 471 u8 peer[ETH_ALEN]) 472 { 473 struct brcmf_flowring_tdls_entry *tdls_entry; 474 struct brcmf_flowring_tdls_entry *search; 475 476 tdls_entry = kzalloc_obj(*tdls_entry, GFP_ATOMIC); 477 if (tdls_entry == NULL) 478 return; 479 480 memcpy(tdls_entry->mac, peer, ETH_ALEN); 481 tdls_entry->next = NULL; 482 if (flow->tdls_entry == NULL) { 483 flow->tdls_entry = tdls_entry; 484 } else { 485 search = flow->tdls_entry; 486 if (memcmp(search->mac, peer, ETH_ALEN) == 0) 487 goto free_entry; 488 while (search->next) { 489 search = search->next; 490 if (memcmp(search->mac, peer, ETH_ALEN) == 0) 491 goto free_entry; 492 } 493 search->next = tdls_entry; 494 } 495 496 flow->tdls_active = true; 497 return; 498 499 free_entry: 500 kfree(tdls_entry); 501 } 502