drivers/net/wireless/st/cw1200/queue.c

   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                 from_timer(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 = kcalloc(map_capacity, sizeof(int),
 157                                         GFP_KERNEL);
 158         if (!stats->link_map_cache)
 159                 return -ENOMEM;
 160
 161         return 0;
 162 }
 163
 164 int cw1200_queue_init(struct cw1200_queue *queue,
 165                       struct cw1200_queue_stats *stats,
 166                       u8 queue_id,
 167                       size_t capacity,
 168                       unsigned long ttl)
 169 {
 170         size_t i;
 171
 172         memset(queue, 0, sizeof(*queue));
 173         queue->stats = stats;
 174         queue->capacity = capacity;
 175         queue->queue_id = queue_id;
 176         queue->ttl = ttl;
 177         INIT_LIST_HEAD(&queue->queue);
 178         INIT_LIST_HEAD(&queue->pending);
 179         INIT_LIST_HEAD(&queue->free_pool);
 180         spin_lock_init(&queue->lock);
 181         timer_setup(&queue->gc, cw1200_queue_gc, 0);
 182
 183         queue->pool = kcalloc(capacity, sizeof(struct cw1200_queue_item),
 184                               GFP_KERNEL);
 185         if (!queue->pool)
 186                 return -ENOMEM;
 187
 188         queue->link_map_cache = kcalloc(stats->map_capacity, sizeof(int),
 189                                         GFP_KERNEL);
 190         if (!queue->link_map_cache) {
 191                 kfree(queue->pool);
 192                 queue->pool = NULL;
 193                 return -ENOMEM;
 194         }
 195
 196         for (i = 0; i < capacity; ++i)
 197                 list_add_tail(&queue->pool[i].head, &queue->free_pool);
 198
 199         return 0;
 200 }
 201
 202 int cw1200_queue_clear(struct cw1200_queue *queue)
 203 {
 204         int i;
 205         LIST_HEAD(gc_list);
 206         struct cw1200_queue_stats *stats = queue->stats;
 207         struct cw1200_queue_item *item, *tmp;
 208
 209         spin_lock_bh(&queue->lock);
 210         queue->generation++;
 211         list_splice_tail_init(&queue->queue, &queue->pending);
 212         list_for_each_entry_safe(item, tmp, &queue->pending, head) {
 213                 WARN_ON(!item->skb);
 214                 cw1200_queue_register_post_gc(&gc_list, item);
 215                 item->skb = NULL;
 216                 list_move_tail(&item->head, &queue->free_pool);
 217         }
 218         queue->num_queued = 0;
 219         queue->num_pending = 0;
 220
 221         spin_lock_bh(&stats->lock);
 222         for (i = 0; i < stats->map_capacity; ++i) {
 223                 stats->num_queued -= queue->link_map_cache[i];
 224                 stats->link_map_cache[i] -= queue->link_map_cache[i];
 225                 queue->link_map_cache[i] = 0;
 226         }
 227         spin_unlock_bh(&stats->lock);
 228         if (queue->overfull) {
 229                 queue->overfull = false;
 230                 __cw1200_queue_unlock(queue);
 231         }
 232         spin_unlock_bh(&queue->lock);
 233         wake_up(&stats->wait_link_id_empty);
 234         cw1200_queue_post_gc(stats, &gc_list);
 235         return 0;
 236 }
 237
 238 void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
 239 {
 240         kfree(stats->link_map_cache);
 241         stats->link_map_cache = NULL;
 242 }
 243
 244 void cw1200_queue_deinit(struct cw1200_queue *queue)
 245 {
 246         cw1200_queue_clear(queue);
 247         del_timer_sync(&queue->gc);
 248         INIT_LIST_HEAD(&queue->free_pool);
 249         kfree(queue->pool);
 250         kfree(queue->link_map_cache);
 251         queue->pool = NULL;
 252         queue->link_map_cache = NULL;
 253         queue->capacity = 0;
 254 }
 255
 256 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 257                                    u32 link_id_map)
 258 {
 259         size_t ret;
 260         int i, bit;
 261         size_t map_capacity = queue->stats->map_capacity;
 262
 263         if (!link_id_map)
 264                 return 0;
 265
 266         spin_lock_bh(&queue->lock);
 267         if (link_id_map == (u32)-1) {
 268                 ret = queue->num_queued - queue->num_pending;
 269         } else {
 270                 ret = 0;
 271                 for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
 272                         if (link_id_map & bit)
 273                                 ret += queue->link_map_cache[i];
 274                 }
 275         }
 276         spin_unlock_bh(&queue->lock);
 277         return ret;
 278 }
 279
 280 int cw1200_queue_put(struct cw1200_queue *queue,
 281                      struct sk_buff *skb,
 282                      struct cw1200_txpriv *txpriv)
 283 {
 284         int ret = 0;
 285         struct cw1200_queue_stats *stats = queue->stats;
 286
 287         if (txpriv->link_id >= queue->stats->map_capacity)
 288                 return -EINVAL;
 289
 290         spin_lock_bh(&queue->lock);
 291         if (!WARN_ON(list_empty(&queue->free_pool))) {
 292                 struct cw1200_queue_item *item = list_first_entry(
 293                         &queue->free_pool, struct cw1200_queue_item, head);
 294                 BUG_ON(item->skb);
 295
 296                 list_move_tail(&item->head, &queue->queue);
 297                 item->skb = skb;
 298                 item->txpriv = *txpriv;
 299                 item->generation = 0;
 300                 item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
 301                                                             queue->queue_id,
 302                                                             item->generation,
 303                                                             item - queue->pool);
 304                 item->queue_timestamp = jiffies;
 305
 306                 ++queue->num_queued;
 307                 ++queue->link_map_cache[txpriv->link_id];
 308
 309                 spin_lock_bh(&stats->lock);
 310                 ++stats->num_queued;
 311                 ++stats->link_map_cache[txpriv->link_id];
 312                 spin_unlock_bh(&stats->lock);
 313
 314                 /* TX may happen in parallel sometimes.
 315                  * Leave extra queue slots so we don't overflow.
 316                  */
 317                 if (queue->overfull == false &&
 318                     queue->num_queued >=
 319                     (queue->capacity - (num_present_cpus() - 1))) {
 320                         queue->overfull = true;
 321                         __cw1200_queue_lock(queue);
 322                         mod_timer(&queue->gc, jiffies);
 323                 }
 324         } else {
 325                 ret = -ENOENT;
 326         }
 327         spin_unlock_bh(&queue->lock);
 328         return ret;
 329 }
 330
 331 int cw1200_queue_get(struct cw1200_queue *queue,
 332                      u32 link_id_map,
 333                      struct wsm_tx **tx,
 334                      struct ieee80211_tx_info **tx_info,
 335                      const struct cw1200_txpriv **txpriv)
 336 {
 337         int ret = -ENOENT;
 338         struct cw1200_queue_item *item;
 339         struct cw1200_queue_stats *stats = queue->stats;
 340         bool wakeup_stats = false;
 341
 342         spin_lock_bh(&queue->lock);
 343         list_for_each_entry(item, &queue->queue, head) {
 344                 if (link_id_map & BIT(item->txpriv.link_id)) {
 345                         ret = 0;
 346                         break;
 347                 }
 348         }
 349
 350         if (!WARN_ON(ret)) {
 351                 *tx = (struct wsm_tx *)item->skb->data;
 352                 *tx_info = IEEE80211_SKB_CB(item->skb);
 353                 *txpriv = &item->txpriv;
 354                 (*tx)->packet_id = item->packet_id;
 355                 list_move_tail(&item->head, &queue->pending);
 356                 ++queue->num_pending;
 357                 --queue->link_map_cache[item->txpriv.link_id];
 358                 item->xmit_timestamp = jiffies;
 359
 360                 spin_lock_bh(&stats->lock);
 361                 --stats->num_queued;
 362                 if (!--stats->link_map_cache[item->txpriv.link_id])
 363                         wakeup_stats = true;
 364                 spin_unlock_bh(&stats->lock);
 365         }
 366         spin_unlock_bh(&queue->lock);
 367         if (wakeup_stats)
 368                 wake_up(&stats->wait_link_id_empty);
 369         return ret;
 370 }
 371
 372 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id)
 373 {
 374         int ret = 0;
 375         u8 queue_generation, queue_id, item_generation, item_id;
 376         struct cw1200_queue_item *item;
 377         struct cw1200_queue_stats *stats = queue->stats;
 378
 379         cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
 380                               &item_generation, &item_id);
 381
 382         item = &queue->pool[item_id];
 383
 384         spin_lock_bh(&queue->lock);
 385         BUG_ON(queue_id != queue->queue_id);
 386         if (queue_generation != queue->generation) {
 387                 ret = -ENOENT;
 388         } else if (item_id >= (unsigned) queue->capacity) {
 389                 WARN_ON(1);
 390                 ret = -EINVAL;
 391         } else if (item->generation != item_generation) {
 392                 WARN_ON(1);
 393                 ret = -ENOENT;
 394         } else {
 395                 --queue->num_pending;
 396                 ++queue->link_map_cache[item->txpriv.link_id];
 397
 398                 spin_lock_bh(&stats->lock);
 399                 ++stats->num_queued;
 400                 ++stats->link_map_cache[item->txpriv.link_id];
 401                 spin_unlock_bh(&stats->lock);
 402
 403                 item->generation = ++item_generation;
 404                 item->packet_id = cw1200_queue_mk_packet_id(queue_generation,
 405                                                             queue_id,
 406                                                             item_generation,
 407                                                             item_id);
 408                 list_move(&item->head, &queue->queue);
 409         }
 410         spin_unlock_bh(&queue->lock);
 411         return ret;
 412 }
 413
 414 int cw1200_queue_requeue_all(struct cw1200_queue *queue)
 415 {
 416         struct cw1200_queue_item *item, *tmp;
 417         struct cw1200_queue_stats *stats = queue->stats;
 418         spin_lock_bh(&queue->lock);
 419
 420         list_for_each_entry_safe_reverse(item, tmp, &queue->pending, head) {
 421                 --queue->num_pending;
 422                 ++queue->link_map_cache[item->txpriv.link_id];
 423
 424                 spin_lock_bh(&stats->lock);
 425                 ++stats->num_queued;
 426                 ++stats->link_map_cache[item->txpriv.link_id];
 427                 spin_unlock_bh(&stats->lock);
 428
 429                 ++item->generation;
 430                 item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
 431                                                             queue->queue_id,
 432                                                             item->generation,
 433                                                             item - queue->pool);
 434                 list_move(&item->head, &queue->queue);
 435         }
 436         spin_unlock_bh(&queue->lock);
 437
 438         return 0;
 439 }
 440
 441 int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id)
 442 {
 443         int ret = 0;
 444         u8 queue_generation, queue_id, item_generation, item_id;
 445         struct cw1200_queue_item *item;
 446         struct cw1200_queue_stats *stats = queue->stats;
 447         struct sk_buff *gc_skb = NULL;
 448         struct cw1200_txpriv gc_txpriv;
 449
 450         cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
 451                               &item_generation, &item_id);
 452
 453         item = &queue->pool[item_id];
 454
 455         spin_lock_bh(&queue->lock);
 456         BUG_ON(queue_id != queue->queue_id);
 457         if (queue_generation != queue->generation) {
 458                 ret = -ENOENT;
 459         } else if (item_id >= (unsigned) queue->capacity) {
 460                 WARN_ON(1);
 461                 ret = -EINVAL;
 462         } else if (item->generation != item_generation) {
 463                 WARN_ON(1);
 464                 ret = -ENOENT;
 465         } else {
 466                 gc_txpriv = item->txpriv;
 467                 gc_skb = item->skb;
 468                 item->skb = NULL;
 469                 --queue->num_pending;
 470                 --queue->num_queued;
 471                 ++queue->num_sent;
 472                 ++item->generation;
 473                 /* Do not use list_move_tail here, but list_move:
 474                  * try to utilize cache row.
 475                  */
 476                 list_move(&item->head, &queue->free_pool);
 477
 478                 if (queue->overfull &&
 479                     (queue->num_queued <= (queue->capacity >> 1))) {
 480                         queue->overfull = false;
 481                         __cw1200_queue_unlock(queue);
 482                 }
 483         }
 484         spin_unlock_bh(&queue->lock);
 485
 486         if (gc_skb)
 487                 stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv);
 488
 489         return ret;
 490 }
 491
 492 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id,
 493                          struct sk_buff **skb,
 494                          const struct cw1200_txpriv **txpriv)
 495 {
 496         int ret = 0;
 497         u8 queue_generation, queue_id, item_generation, item_id;
 498         struct cw1200_queue_item *item;
 499         cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
 500                               &item_generation, &item_id);
 501
 502         item = &queue->pool[item_id];
 503
 504         spin_lock_bh(&queue->lock);
 505         BUG_ON(queue_id != queue->queue_id);
 506         if (queue_generation != queue->generation) {
 507                 ret = -ENOENT;
 508         } else if (item_id >= (unsigned) queue->capacity) {
 509                 WARN_ON(1);
 510                 ret = -EINVAL;
 511         } else if (item->generation != item_generation) {
 512                 WARN_ON(1);
 513                 ret = -ENOENT;
 514         } else {
 515                 *skb = item->skb;
 516                 *txpriv = &item->txpriv;
 517         }
 518         spin_unlock_bh(&queue->lock);
 519         return ret;
 520 }
 521
 522 void cw1200_queue_lock(struct cw1200_queue *queue)
 523 {
 524         spin_lock_bh(&queue->lock);
 525         __cw1200_queue_lock(queue);
 526         spin_unlock_bh(&queue->lock);
 527 }
 528
 529 void cw1200_queue_unlock(struct cw1200_queue *queue)
 530 {
 531         spin_lock_bh(&queue->lock);
 532         __cw1200_queue_unlock(queue);
 533         spin_unlock_bh(&queue->lock);
 534 }
 535
 536 bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
 537                                      unsigned long *timestamp,
 538                                      u32 pending_frame_id)
 539 {
 540         struct cw1200_queue_item *item;
 541         bool ret;
 542
 543         spin_lock_bh(&queue->lock);
 544         ret = !list_empty(&queue->pending);
 545         if (ret) {
 546                 list_for_each_entry(item, &queue->pending, head) {
 547                         if (item->packet_id != pending_frame_id)
 548                                 if (time_before(item->xmit_timestamp,
 549                                                 *timestamp))
 550                                         *timestamp = item->xmit_timestamp;
 551                 }
 552         }
 553         spin_unlock_bh(&queue->lock);
 554         return ret;
 555 }
 556
 557 bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
 558                                  u32 link_id_map)
 559 {
 560         bool empty = true;
 561
 562         spin_lock_bh(&stats->lock);
 563         if (link_id_map == (u32)-1) {
 564                 empty = stats->num_queued == 0;
 565         } else {
 566                 int i;
 567                 for (i = 0; i < stats->map_capacity; ++i) {
 568                         if (link_id_map & BIT(i)) {
 569                                 if (stats->link_map_cache[i]) {
 570                                         empty = false;
 571                                         break;
 572                                 }
 573                         }
 574                 }
 575         }
 576         spin_unlock_bh(&stats->lock);
 577
 578         return empty;
 579 }