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(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 brcmf_msgbuf_delete_flowring(drvr, flowid); 423 } 424 } 425 flow->addr_mode[ifidx] = addr_mode; 426 } 427 } 428 429 430 void 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 477 void 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 506 free_entry: 507 kfree(tdls_entry); 508 } 509