1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers 4 * 5 * Copyright (c) 2010, ST-Ericsson 6 * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no> 7 */ 8 9 #include <net/mac80211.h> 10 #include <linux/sched.h> 11 #include <linux/jiffies.h> 12 #include "queue.h" 13 #include "cw1200.h" 14 #include "debug.h" 15 16 /* private */ struct cw1200_queue_item 17 { 18 struct list_head head; 19 struct sk_buff *skb; 20 u32 packet_id; 21 unsigned long queue_timestamp; 22 unsigned long xmit_timestamp; 23 struct cw1200_txpriv txpriv; 24 u8 generation; 25 }; 26 27 static inline void __cw1200_queue_lock(struct cw1200_queue *queue) 28 { 29 struct cw1200_queue_stats *stats = queue->stats; 30 if (queue->tx_locked_cnt++ == 0) { 31 pr_debug("[TX] Queue %d is locked.\n", 32 queue->queue_id); 33 ieee80211_stop_queue(stats->priv->hw, queue->queue_id); 34 } 35 } 36 37 static inline void __cw1200_queue_unlock(struct cw1200_queue *queue) 38 { 39 struct cw1200_queue_stats *stats = queue->stats; 40 BUG_ON(!queue->tx_locked_cnt); 41 if (--queue->tx_locked_cnt == 0) { 42 pr_debug("[TX] Queue %d is unlocked.\n", 43 queue->queue_id); 44 ieee80211_wake_queue(stats->priv->hw, queue->queue_id); 45 } 46 } 47 48 static inline void cw1200_queue_parse_id(u32 packet_id, u8 *queue_generation, 49 u8 *queue_id, u8 *item_generation, 50 u8 *item_id) 51 { 52 *item_id = (packet_id >> 0) & 0xFF; 53 *item_generation = (packet_id >> 8) & 0xFF; 54 *queue_id = (packet_id >> 16) & 0xFF; 55 *queue_generation = (packet_id >> 24) & 0xFF; 56 } 57 58 static inline u32 cw1200_queue_mk_packet_id(u8 queue_generation, u8 queue_id, 59 u8 item_generation, u8 item_id) 60 { 61 return ((u32)item_id << 0) | 62 ((u32)item_generation << 8) | 63 ((u32)queue_id << 16) | 64 ((u32)queue_generation << 24); 65 } 66 67 static void cw1200_queue_post_gc(struct cw1200_queue_stats *stats, 68 struct list_head *gc_list) 69 { 70 struct cw1200_queue_item *item, *tmp; 71 72 list_for_each_entry_safe(item, tmp, gc_list, head) { 73 list_del(&item->head); 74 stats->skb_dtor(stats->priv, item->skb, &item->txpriv); 75 kfree(item); 76 } 77 } 78 79 static void cw1200_queue_register_post_gc(struct list_head *gc_list, 80 struct cw1200_queue_item *item) 81 { 82 struct cw1200_queue_item *gc_item; 83 gc_item = kmemdup(item, sizeof(struct cw1200_queue_item), 84 GFP_ATOMIC); 85 BUG_ON(!gc_item); 86 list_add_tail(&gc_item->head, gc_list); 87 } 88 89 static void __cw1200_queue_gc(struct cw1200_queue *queue, 90 struct list_head *head, 91 bool unlock) 92 { 93 struct cw1200_queue_stats *stats = queue->stats; 94 struct cw1200_queue_item *item = NULL, *iter, *tmp; 95 bool wakeup_stats = false; 96 97 list_for_each_entry_safe(iter, tmp, &queue->queue, head) { 98 if (time_is_after_jiffies(iter->queue_timestamp + queue->ttl)) { 99 item = iter; 100 break; 101 } 102 --queue->num_queued; 103 --queue->link_map_cache[iter->txpriv.link_id]; 104 spin_lock_bh(&stats->lock); 105 --stats->num_queued; 106 if (!--stats->link_map_cache[iter->txpriv.link_id]) 107 wakeup_stats = true; 108 spin_unlock_bh(&stats->lock); 109 cw1200_debug_tx_ttl(stats->priv); 110 cw1200_queue_register_post_gc(head, iter); 111 iter->skb = NULL; 112 list_move_tail(&iter->head, &queue->free_pool); 113 } 114 115 if (wakeup_stats) 116 wake_up(&stats->wait_link_id_empty); 117 118 if (queue->overfull) { 119 if (queue->num_queued <= (queue->capacity >> 1)) { 120 queue->overfull = false; 121 if (unlock) 122 __cw1200_queue_unlock(queue); 123 } else if (item) { 124 unsigned long tmo = item->queue_timestamp + queue->ttl; 125 mod_timer(&queue->gc, tmo); 126 cw1200_pm_stay_awake(&stats->priv->pm_state, 127 tmo - jiffies); 128 } 129 } 130 } 131 132 static void cw1200_queue_gc(struct timer_list *t) 133 { 134 LIST_HEAD(list); 135 struct cw1200_queue *queue = 136 timer_container_of(queue, t, gc); 137 138 spin_lock_bh(&queue->lock); 139 __cw1200_queue_gc(queue, &list, true); 140 spin_unlock_bh(&queue->lock); 141 cw1200_queue_post_gc(queue->stats, &list); 142 } 143 144 int cw1200_queue_stats_init(struct cw1200_queue_stats *stats, 145 size_t map_capacity, 146 cw1200_queue_skb_dtor_t skb_dtor, 147 struct cw1200_common *priv) 148 { 149 memset(stats, 0, sizeof(*stats)); 150 stats->map_capacity = map_capacity; 151 stats->skb_dtor = skb_dtor; 152 stats->priv = priv; 153 spin_lock_init(&stats->lock); 154 init_waitqueue_head(&stats->wait_link_id_empty); 155 156 stats->link_map_cache = kzalloc_objs(int, map_capacity, GFP_KERNEL); 157 if (!stats->link_map_cache) 158 return -ENOMEM; 159 160 return 0; 161 } 162 163 int cw1200_queue_init(struct cw1200_queue *queue, 164 struct cw1200_queue_stats *stats, 165 u8 queue_id, 166 size_t capacity, 167 unsigned long ttl) 168 { 169 size_t i; 170 171 memset(queue, 0, sizeof(*queue)); 172 queue->stats = stats; 173 queue->capacity = capacity; 174 queue->queue_id = queue_id; 175 queue->ttl = ttl; 176 INIT_LIST_HEAD(&queue->queue); 177 INIT_LIST_HEAD(&queue->pending); 178 INIT_LIST_HEAD(&queue->free_pool); 179 spin_lock_init(&queue->lock); 180 timer_setup(&queue->gc, cw1200_queue_gc, 0); 181 182 queue->pool = kzalloc_objs(struct cw1200_queue_item, capacity, 183 GFP_KERNEL); 184 if (!queue->pool) 185 return -ENOMEM; 186 187 queue->link_map_cache = kzalloc_objs(int, stats->map_capacity, 188 GFP_KERNEL); 189 if (!queue->link_map_cache) { 190 kfree(queue->pool); 191 queue->pool = NULL; 192 return -ENOMEM; 193 } 194 195 for (i = 0; i < capacity; ++i) 196 list_add_tail(&queue->pool[i].head, &queue->free_pool); 197 198 return 0; 199 } 200 201 int cw1200_queue_clear(struct cw1200_queue *queue) 202 { 203 int i; 204 LIST_HEAD(gc_list); 205 struct cw1200_queue_stats *stats = queue->stats; 206 struct cw1200_queue_item *item, *tmp; 207 208 spin_lock_bh(&queue->lock); 209 queue->generation++; 210 list_splice_tail_init(&queue->queue, &queue->pending); 211 list_for_each_entry_safe(item, tmp, &queue->pending, head) { 212 WARN_ON(!item->skb); 213 cw1200_queue_register_post_gc(&gc_list, item); 214 item->skb = NULL; 215 list_move_tail(&item->head, &queue->free_pool); 216 } 217 queue->num_queued = 0; 218 queue->num_pending = 0; 219 220 spin_lock_bh(&stats->lock); 221 for (i = 0; i < stats->map_capacity; ++i) { 222 stats->num_queued -= queue->link_map_cache[i]; 223 stats->link_map_cache[i] -= queue->link_map_cache[i]; 224 queue->link_map_cache[i] = 0; 225 } 226 spin_unlock_bh(&stats->lock); 227 if (queue->overfull) { 228 queue->overfull = false; 229 __cw1200_queue_unlock(queue); 230 } 231 spin_unlock_bh(&queue->lock); 232 wake_up(&stats->wait_link_id_empty); 233 cw1200_queue_post_gc(stats, &gc_list); 234 return 0; 235 } 236 237 void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats) 238 { 239 kfree(stats->link_map_cache); 240 stats->link_map_cache = NULL; 241 } 242 243 void cw1200_queue_deinit(struct cw1200_queue *queue) 244 { 245 cw1200_queue_clear(queue); 246 timer_delete_sync(&queue->gc); 247 INIT_LIST_HEAD(&queue->free_pool); 248 kfree(queue->pool); 249 kfree(queue->link_map_cache); 250 queue->pool = NULL; 251 queue->link_map_cache = NULL; 252 queue->capacity = 0; 253 } 254 255 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue, 256 u32 link_id_map) 257 { 258 size_t ret; 259 int i, bit; 260 size_t map_capacity = queue->stats->map_capacity; 261 262 if (!link_id_map) 263 return 0; 264 265 spin_lock_bh(&queue->lock); 266 if (link_id_map == (u32)-1) { 267 ret = queue->num_queued - queue->num_pending; 268 } else { 269 ret = 0; 270 for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) { 271 if (link_id_map & bit) 272 ret += queue->link_map_cache[i]; 273 } 274 } 275 spin_unlock_bh(&queue->lock); 276 return ret; 277 } 278 279 int cw1200_queue_put(struct cw1200_queue *queue, 280 struct sk_buff *skb, 281 struct cw1200_txpriv *txpriv) 282 { 283 int ret = 0; 284 struct cw1200_queue_stats *stats = queue->stats; 285 286 if (txpriv->link_id >= queue->stats->map_capacity) 287 return -EINVAL; 288 289 spin_lock_bh(&queue->lock); 290 if (!WARN_ON(list_empty(&queue->free_pool))) { 291 struct cw1200_queue_item *item = list_first_entry( 292 &queue->free_pool, struct cw1200_queue_item, head); 293 BUG_ON(item->skb); 294 295 list_move_tail(&item->head, &queue->queue); 296 item->skb = skb; 297 item->txpriv = *txpriv; 298 item->generation = 0; 299 item->packet_id = cw1200_queue_mk_packet_id(queue->generation, 300 queue->queue_id, 301 item->generation, 302 item - queue->pool); 303 item->queue_timestamp = jiffies; 304 305 ++queue->num_queued; 306 ++queue->link_map_cache[txpriv->link_id]; 307 308 spin_lock_bh(&stats->lock); 309 ++stats->num_queued; 310 ++stats->link_map_cache[txpriv->link_id]; 311 spin_unlock_bh(&stats->lock); 312 313 /* TX may happen in parallel sometimes. 314 * Leave extra queue slots so we don't overflow. 315 */ 316 if (queue->overfull == false && 317 queue->num_queued >= 318 (queue->capacity - (num_present_cpus() - 1))) { 319 queue->overfull = true; 320 __cw1200_queue_lock(queue); 321 mod_timer(&queue->gc, jiffies); 322 } 323 } else { 324 ret = -ENOENT; 325 } 326 spin_unlock_bh(&queue->lock); 327 return ret; 328 } 329 330 int cw1200_queue_get(struct cw1200_queue *queue, 331 u32 link_id_map, 332 struct wsm_tx **tx, 333 struct ieee80211_tx_info **tx_info, 334 const struct cw1200_txpriv **txpriv) 335 { 336 int ret = -ENOENT; 337 struct cw1200_queue_item *item; 338 struct cw1200_queue_stats *stats = queue->stats; 339 bool wakeup_stats = false; 340 341 spin_lock_bh(&queue->lock); 342 list_for_each_entry(item, &queue->queue, head) { 343 if (link_id_map & BIT(item->txpriv.link_id)) { 344 ret = 0; 345 break; 346 } 347 } 348 349 if (!WARN_ON(ret)) { 350 *tx = (struct wsm_tx *)item->skb->data; 351 *tx_info = IEEE80211_SKB_CB(item->skb); 352 *txpriv = &item->txpriv; 353 (*tx)->packet_id = item->packet_id; 354 list_move_tail(&item->head, &queue->pending); 355 ++queue->num_pending; 356 --queue->link_map_cache[item->txpriv.link_id]; 357 item->xmit_timestamp = jiffies; 358 359 spin_lock_bh(&stats->lock); 360 --stats->num_queued; 361 if (!--stats->link_map_cache[item->txpriv.link_id]) 362 wakeup_stats = true; 363 spin_unlock_bh(&stats->lock); 364 } 365 spin_unlock_bh(&queue->lock); 366 if (wakeup_stats) 367 wake_up(&stats->wait_link_id_empty); 368 return ret; 369 } 370 371 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id) 372 { 373 int ret = 0; 374 u8 queue_generation, queue_id, item_generation, item_id; 375 struct cw1200_queue_item *item; 376 struct cw1200_queue_stats *stats = queue->stats; 377 378 cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id, 379 &item_generation, &item_id); 380 381 item = &queue->pool[item_id]; 382 383 spin_lock_bh(&queue->lock); 384 BUG_ON(queue_id != queue->queue_id); 385 if (queue_generation != queue->generation) { 386 ret = -ENOENT; 387 } else if (item_id >= (unsigned) queue->capacity) { 388 WARN_ON(1); 389 ret = -EINVAL; 390 } else if (item->generation != item_generation) { 391 WARN_ON(1); 392 ret = -ENOENT; 393 } else { 394 --queue->num_pending; 395 ++queue->link_map_cache[item->txpriv.link_id]; 396 397 spin_lock_bh(&stats->lock); 398 ++stats->num_queued; 399 ++stats->link_map_cache[item->txpriv.link_id]; 400 spin_unlock_bh(&stats->lock); 401 402 item->generation = ++item_generation; 403 item->packet_id = cw1200_queue_mk_packet_id(queue_generation, 404 queue_id, 405 item_generation, 406 item_id); 407 list_move(&item->head, &queue->queue); 408 } 409 spin_unlock_bh(&queue->lock); 410 return ret; 411 } 412 413 int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id) 414 { 415 int ret = 0; 416 u8 queue_generation, queue_id, item_generation, item_id; 417 struct cw1200_queue_item *item; 418 struct cw1200_queue_stats *stats = queue->stats; 419 struct sk_buff *gc_skb = NULL; 420 struct cw1200_txpriv gc_txpriv; 421 422 cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id, 423 &item_generation, &item_id); 424 425 item = &queue->pool[item_id]; 426 427 spin_lock_bh(&queue->lock); 428 BUG_ON(queue_id != queue->queue_id); 429 if (queue_generation != queue->generation) { 430 ret = -ENOENT; 431 } else if (item_id >= (unsigned) queue->capacity) { 432 WARN_ON(1); 433 ret = -EINVAL; 434 } else if (item->generation != item_generation) { 435 WARN_ON(1); 436 ret = -ENOENT; 437 } else { 438 gc_txpriv = item->txpriv; 439 gc_skb = item->skb; 440 item->skb = NULL; 441 --queue->num_pending; 442 --queue->num_queued; 443 ++queue->num_sent; 444 ++item->generation; 445 /* Do not use list_move_tail here, but list_move: 446 * try to utilize cache row. 447 */ 448 list_move(&item->head, &queue->free_pool); 449 450 if (queue->overfull && 451 (queue->num_queued <= (queue->capacity >> 1))) { 452 queue->overfull = false; 453 __cw1200_queue_unlock(queue); 454 } 455 } 456 spin_unlock_bh(&queue->lock); 457 458 if (gc_skb) 459 stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv); 460 461 return ret; 462 } 463 464 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id, 465 struct sk_buff **skb, 466 const struct cw1200_txpriv **txpriv) 467 { 468 int ret = 0; 469 u8 queue_generation, queue_id, item_generation, item_id; 470 struct cw1200_queue_item *item; 471 cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id, 472 &item_generation, &item_id); 473 474 item = &queue->pool[item_id]; 475 476 spin_lock_bh(&queue->lock); 477 BUG_ON(queue_id != queue->queue_id); 478 if (queue_generation != queue->generation) { 479 ret = -ENOENT; 480 } else if (item_id >= (unsigned) queue->capacity) { 481 WARN_ON(1); 482 ret = -EINVAL; 483 } else if (item->generation != item_generation) { 484 WARN_ON(1); 485 ret = -ENOENT; 486 } else { 487 *skb = item->skb; 488 *txpriv = &item->txpriv; 489 } 490 spin_unlock_bh(&queue->lock); 491 return ret; 492 } 493 494 void cw1200_queue_lock(struct cw1200_queue *queue) 495 { 496 spin_lock_bh(&queue->lock); 497 __cw1200_queue_lock(queue); 498 spin_unlock_bh(&queue->lock); 499 } 500 501 void cw1200_queue_unlock(struct cw1200_queue *queue) 502 { 503 spin_lock_bh(&queue->lock); 504 __cw1200_queue_unlock(queue); 505 spin_unlock_bh(&queue->lock); 506 } 507 508 bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue, 509 unsigned long *timestamp, 510 u32 pending_frame_id) 511 { 512 struct cw1200_queue_item *item; 513 bool ret; 514 515 spin_lock_bh(&queue->lock); 516 ret = !list_empty(&queue->pending); 517 if (ret) { 518 list_for_each_entry(item, &queue->pending, head) { 519 if (item->packet_id != pending_frame_id) 520 if (time_before(item->xmit_timestamp, 521 *timestamp)) 522 *timestamp = item->xmit_timestamp; 523 } 524 } 525 spin_unlock_bh(&queue->lock); 526 return ret; 527 } 528 529 bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats, 530 u32 link_id_map) 531 { 532 bool empty = true; 533 534 spin_lock_bh(&stats->lock); 535 if (link_id_map == (u32)-1) { 536 empty = stats->num_queued == 0; 537 } else { 538 int i; 539 for (i = 0; i < stats->map_capacity; ++i) { 540 if (link_id_map & BIT(i)) { 541 if (stats->link_map_cache[i]) { 542 empty = false; 543 break; 544 } 545 } 546 } 547 } 548 spin_unlock_bh(&stats->lock); 549 550 return empty; 551 } 552