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