drivers/staging/wfx/queue.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * O(1) TX queue with built-in allocator.
   4  *
   5  * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
   6  * Copyright (c) 2010, ST-Ericsson
   7  */
   8 #include <linux/sched.h>
   9 #include <net/mac80211.h>
  10
  11 #include "queue.h"
  12 #include "wfx.h"
  13 #include "sta.h"
  14 #include "data_tx.h"
  15
  16 void wfx_tx_lock(struct wfx_dev *wdev)
  17 {
  18         atomic_inc(&wdev->tx_lock);
  19 }
  20
  21 void wfx_tx_unlock(struct wfx_dev *wdev)
  22 {
  23         int tx_lock = atomic_dec_return(&wdev->tx_lock);
  24
  25         WARN(tx_lock < 0, "inconsistent tx_lock value");
  26         if (!tx_lock)
  27                 wfx_bh_request_tx(wdev);
  28 }
  29
  30 void wfx_tx_flush(struct wfx_dev *wdev)
  31 {
  32         int ret;
  33
  34         // Do not wait for any reply if chip is frozen
  35         if (wdev->chip_frozen)
  36                 return;
  37
  38         mutex_lock(&wdev->hif_cmd.lock);
  39         ret = wait_event_timeout(wdev->hif.tx_buffers_empty,
  40                                  !wdev->hif.tx_buffers_used,
  41                                  msecs_to_jiffies(3000));
  42         if (!ret) {
  43                 dev_warn(wdev->dev, "cannot flush tx buffers (%d still busy)\n",
  44                          wdev->hif.tx_buffers_used);
  45                 wfx_pending_dump_old_frames(wdev, 3000);
  46                 // FIXME: drop pending frames here
  47                 wdev->chip_frozen = 1;
  48         }
  49         mutex_unlock(&wdev->hif_cmd.lock);
  50 }
  51
  52 void wfx_tx_lock_flush(struct wfx_dev *wdev)
  53 {
  54         wfx_tx_lock(wdev);
  55         wfx_tx_flush(wdev);
  56 }
  57
  58 void wfx_tx_queues_lock(struct wfx_dev *wdev)
  59 {
  60         int i;
  61         struct wfx_queue *queue;
  62
  63         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
  64                 queue = &wdev->tx_queue[i];
  65                 spin_lock_bh(&queue->queue.lock);
  66                 if (queue->tx_locked_cnt++ == 0)
  67                         ieee80211_stop_queue(wdev->hw, queue->queue_id);
  68                 spin_unlock_bh(&queue->queue.lock);
  69         }
  70 }
  71
  72 void wfx_tx_queues_unlock(struct wfx_dev *wdev)
  73 {
  74         int i;
  75         struct wfx_queue *queue;
  76
  77         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
  78                 queue = &wdev->tx_queue[i];
  79                 spin_lock_bh(&queue->queue.lock);
  80                 WARN(!queue->tx_locked_cnt, "queue already unlocked");
  81                 if (--queue->tx_locked_cnt == 0)
  82                         ieee80211_wake_queue(wdev->hw, queue->queue_id);
  83                 spin_unlock_bh(&queue->queue.lock);
  84         }
  85 }
  86
  87 /* If successful, LOCKS the TX queue! */
  88 void wfx_tx_queues_wait_empty_vif(struct wfx_vif *wvif)
  89 {
  90         int i;
  91         bool done;
  92         struct wfx_queue *queue;
  93         struct sk_buff *item;
  94         struct wfx_dev *wdev = wvif->wdev;
  95         struct hif_msg *hif;
  96
  97         if (wvif->wdev->chip_frozen) {
  98                 wfx_tx_lock_flush(wdev);
  99                 wfx_tx_queues_clear(wdev);
 100                 return;
 101         }
 102
 103         do {
 104                 done = true;
 105                 wfx_tx_lock_flush(wdev);
 106                 for (i = 0; i < IEEE80211_NUM_ACS && done; ++i) {
 107                         queue = &wdev->tx_queue[i];
 108                         spin_lock_bh(&queue->queue.lock);
 109                         skb_queue_walk(&queue->queue, item) {
 110                                 hif = (struct hif_msg *) item->data;
 111                                 if (hif->interface == wvif->id)
 112                                         done = false;
 113                         }
 114                         spin_unlock_bh(&queue->queue.lock);
 115                 }
 116                 if (!done) {
 117                         wfx_tx_unlock(wdev);
 118                         msleep(20);
 119                 }
 120         } while (!done);
 121 }
 122
 123 static void wfx_tx_queue_clear(struct wfx_dev *wdev, struct wfx_queue *queue,
 124                                struct sk_buff_head *gc_list)
 125 {
 126         int i;
 127         struct sk_buff *item;
 128         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 129
 130         spin_lock_bh(&queue->queue.lock);
 131         while ((item = __skb_dequeue(&queue->queue)) != NULL)
 132                 skb_queue_head(gc_list, item);
 133         spin_lock_bh(&stats->pending.lock);
 134         for (i = 0; i < ARRAY_SIZE(stats->link_map_cache); ++i) {
 135                 stats->link_map_cache[i] -= queue->link_map_cache[i];
 136                 queue->link_map_cache[i] = 0;
 137         }
 138         spin_unlock_bh(&stats->pending.lock);
 139         spin_unlock_bh(&queue->queue.lock);
 140 }
 141
 142 void wfx_tx_queues_clear(struct wfx_dev *wdev)
 143 {
 144         int i;
 145         struct sk_buff *item;
 146         struct sk_buff_head gc_list;
 147         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 148
 149         skb_queue_head_init(&gc_list);
 150         for (i = 0; i < IEEE80211_NUM_ACS; ++i)
 151                 wfx_tx_queue_clear(wdev, &wdev->tx_queue[i], &gc_list);
 152         wake_up(&stats->wait_link_id_empty);
 153         while ((item = skb_dequeue(&gc_list)) != NULL)
 154                 wfx_skb_dtor(wdev, item);
 155 }
 156
 157 void wfx_tx_queues_init(struct wfx_dev *wdev)
 158 {
 159         int i;
 160
 161         memset(&wdev->tx_queue_stats, 0, sizeof(wdev->tx_queue_stats));
 162         memset(wdev->tx_queue, 0, sizeof(wdev->tx_queue));
 163         skb_queue_head_init(&wdev->tx_queue_stats.pending);
 164         init_waitqueue_head(&wdev->tx_queue_stats.wait_link_id_empty);
 165
 166         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 167                 wdev->tx_queue[i].queue_id = i;
 168                 skb_queue_head_init(&wdev->tx_queue[i].queue);
 169         }
 170 }
 171
 172 void wfx_tx_queues_deinit(struct wfx_dev *wdev)
 173 {
 174         WARN_ON(!skb_queue_empty(&wdev->tx_queue_stats.pending));
 175         wfx_tx_queues_clear(wdev);
 176 }
 177
 178 size_t wfx_tx_queue_get_num_queued(struct wfx_queue *queue,
 179                                    u32 link_id_map)
 180 {
 181         size_t ret;
 182         int i, bit;
 183
 184         if (!link_id_map)
 185                 return 0;
 186
 187         spin_lock_bh(&queue->queue.lock);
 188         if (link_id_map == (u32)-1) {
 189                 ret = skb_queue_len(&queue->queue);
 190         } else {
 191                 ret = 0;
 192                 for (i = 0, bit = 1; i < ARRAY_SIZE(queue->link_map_cache);
 193                      ++i, bit <<= 1) {
 194                         if (link_id_map & bit)
 195                                 ret += queue->link_map_cache[i];
 196                 }
 197         }
 198         spin_unlock_bh(&queue->queue.lock);
 199         return ret;
 200 }
 201
 202 void wfx_tx_queue_put(struct wfx_dev *wdev, struct wfx_queue *queue,
 203                       struct sk_buff *skb)
 204 {
 205         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 206         struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
 207
 208         WARN(tx_priv->link_id >= ARRAY_SIZE(stats->link_map_cache), "invalid link-id value");
 209         spin_lock_bh(&queue->queue.lock);
 210         __skb_queue_tail(&queue->queue, skb);
 211
 212         ++queue->link_map_cache[tx_priv->link_id];
 213
 214         spin_lock_bh(&stats->pending.lock);
 215         ++stats->link_map_cache[tx_priv->link_id];
 216         spin_unlock_bh(&stats->pending.lock);
 217         spin_unlock_bh(&queue->queue.lock);
 218 }
 219
 220 static struct sk_buff *wfx_tx_queue_get(struct wfx_dev *wdev,
 221                                         struct wfx_queue *queue,
 222                                         u32 link_id_map)
 223 {
 224         struct sk_buff *skb = NULL;
 225         struct sk_buff *item;
 226         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 227         struct wfx_tx_priv *tx_priv;
 228         bool wakeup_stats = false;
 229
 230         spin_lock_bh(&queue->queue.lock);
 231         skb_queue_walk(&queue->queue, item) {
 232                 tx_priv = wfx_skb_tx_priv(item);
 233                 if (link_id_map & BIT(tx_priv->link_id)) {
 234                         skb = item;
 235                         break;
 236                 }
 237         }
 238         if (skb) {
 239                 tx_priv = wfx_skb_tx_priv(skb);
 240                 tx_priv->xmit_timestamp = ktime_get();
 241                 __skb_unlink(skb, &queue->queue);
 242                 --queue->link_map_cache[tx_priv->link_id];
 243
 244                 spin_lock_bh(&stats->pending.lock);
 245                 __skb_queue_tail(&stats->pending, skb);
 246                 if (!--stats->link_map_cache[tx_priv->link_id])
 247                         wakeup_stats = true;
 248                 spin_unlock_bh(&stats->pending.lock);
 249         }
 250         spin_unlock_bh(&queue->queue.lock);
 251         if (wakeup_stats)
 252                 wake_up(&stats->wait_link_id_empty);
 253         return skb;
 254 }
 255
 256 int wfx_pending_requeue(struct wfx_dev *wdev, struct sk_buff *skb)
 257 {
 258         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 259         struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
 260         struct wfx_queue *queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
 261
 262         WARN_ON(skb_get_queue_mapping(skb) > 3);
 263         spin_lock_bh(&queue->queue.lock);
 264         ++queue->link_map_cache[tx_priv->link_id];
 265
 266         spin_lock_bh(&stats->pending.lock);
 267         ++stats->link_map_cache[tx_priv->link_id];
 268         __skb_unlink(skb, &stats->pending);
 269         spin_unlock_bh(&stats->pending.lock);
 270         __skb_queue_tail(&queue->queue, skb);
 271         spin_unlock_bh(&queue->queue.lock);
 272         return 0;
 273 }
 274
 275 int wfx_pending_remove(struct wfx_dev *wdev, struct sk_buff *skb)
 276 {
 277         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 278
 279         spin_lock_bh(&stats->pending.lock);
 280         __skb_unlink(skb, &stats->pending);
 281         spin_unlock_bh(&stats->pending.lock);
 282         wfx_skb_dtor(wdev, skb);
 283
 284         return 0;
 285 }
 286
 287 struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id)
 288 {
 289         struct sk_buff *skb;
 290         struct hif_req_tx *req;
 291         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 292
 293         spin_lock_bh(&stats->pending.lock);
 294         skb_queue_walk(&stats->pending, skb) {
 295                 req = wfx_skb_txreq(skb);
 296                 if (req->packet_id == packet_id) {
 297                         spin_unlock_bh(&stats->pending.lock);
 298                         return skb;
 299                 }
 300         }
 301         spin_unlock_bh(&stats->pending.lock);
 302         WARN(1, "cannot find packet in pending queue");
 303         return NULL;
 304 }
 305
 306 void wfx_pending_dump_old_frames(struct wfx_dev *wdev, unsigned int limit_ms)
 307 {
 308         struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 309         ktime_t now = ktime_get();
 310         struct wfx_tx_priv *tx_priv;
 311         struct hif_req_tx *req;
 312         struct sk_buff *skb;
 313         bool first = true;
 314
 315         spin_lock_bh(&stats->pending.lock);
 316         skb_queue_walk(&stats->pending, skb) {
 317                 tx_priv = wfx_skb_tx_priv(skb);
 318                 req = wfx_skb_txreq(skb);
 319                 if (ktime_after(now, ktime_add_ms(tx_priv->xmit_timestamp,
 320                                                   limit_ms))) {
 321                         if (first) {
 322                                 dev_info(wdev->dev, "frames stuck in firmware since %dms or more:\n",
 323                                          limit_ms);
 324                                 first = false;
 325                         }
 326                         dev_info(wdev->dev, "   id %08x sent %lldms ago\n",
 327                                  req->packet_id,
 328                                  ktime_ms_delta(now, tx_priv->xmit_timestamp));
 329                 }
 330         }
 331         spin_unlock_bh(&stats->pending.lock);
 332 }
 333
 334 unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
 335                                           struct sk_buff *skb)
 336 {
 337         ktime_t now = ktime_get();
 338         struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
 339
 340         return ktime_us_delta(now, tx_priv->xmit_timestamp);
 341 }
 342
 343 bool wfx_tx_queues_is_empty(struct wfx_dev *wdev)
 344 {
 345         int i;
 346         struct sk_buff_head *queue;
 347         bool ret = true;
 348
 349         for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 350                 queue = &wdev->tx_queue[i].queue;
 351                 spin_lock_bh(&queue->lock);
 352                 if (!skb_queue_empty(queue))
 353                         ret = false;
 354                 spin_unlock_bh(&queue->lock);
 355         }
 356         return ret;
 357 }
 358
 359 static bool hif_handle_tx_data(struct wfx_vif *wvif, struct sk_buff *skb,
 360                                struct wfx_queue *queue)
 361 {
 362         bool handled = false;
 363         struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
 364         struct hif_req_tx *req = wfx_skb_txreq(skb);
 365         struct ieee80211_hdr *frame = (struct ieee80211_hdr *) (req->frame + req->data_flags.fc_offset);
 366
 367         enum {
 368                 do_wep,
 369                 do_tx,
 370         } action = do_tx;
 371
 372         if (ieee80211_is_nullfunc(frame->frame_control)) {
 373                 mutex_lock(&wvif->bss_loss_lock);
 374                 if (wvif->bss_loss_state) {
 375                         wvif->bss_loss_confirm_id = req->packet_id;
 376                         req->queue_id.queue_id = HIF_QUEUE_ID_VOICE;
 377                 }
 378                 mutex_unlock(&wvif->bss_loss_lock);
 379         } else if (ieee80211_has_protected(frame->frame_control) &&
 380                    tx_priv->hw_key &&
 381                    tx_priv->hw_key->keyidx != wvif->wep_default_key_id &&
 382                    (tx_priv->hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
 383                     tx_priv->hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
 384                 action = do_wep;
 385         }
 386
 387         switch (action) {
 388         case do_wep:
 389                 wfx_tx_lock(wvif->wdev);
 390                 WARN_ON(wvif->wep_pending_skb);
 391                 wvif->wep_default_key_id = tx_priv->hw_key->keyidx;
 392                 wvif->wep_pending_skb = skb;
 393                 if (!schedule_work(&wvif->wep_key_work))
 394                         wfx_tx_unlock(wvif->wdev);
 395                 handled = true;
 396                 break;
 397         case do_tx:
 398                 break;
 399         default:
 400                 /* Do nothing */
 401                 break;
 402         }
 403         return handled;
 404 }
 405
 406 static int wfx_get_prio_queue(struct wfx_vif *wvif,
 407                                  u32 tx_allowed_mask, int *total)
 408 {
 409         static const int urgent = BIT(WFX_LINK_ID_AFTER_DTIM) |
 410                 BIT(WFX_LINK_ID_UAPSD);
 411         const struct ieee80211_tx_queue_params *edca;
 412         unsigned int score, best = -1;
 413         int winner = -1;
 414         int i;
 415
 416         /* search for a winner using edca params */
 417         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 418                 int queued;
 419
 420                 edca = &wvif->edca_params[i];
 421                 queued = wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[i],
 422                                 tx_allowed_mask);
 423                 if (!queued)
 424                         continue;
 425                 *total += queued;
 426                 score = ((edca->aifs + edca->cw_min) << 16) +
 427                         ((edca->cw_max - edca->cw_min) *
 428                          (get_random_int() & 0xFFFF));
 429                 if (score < best && (winner < 0 || i != 3)) {
 430                         best = score;
 431                         winner = i;
 432                 }
 433         }
 434
 435         /* override winner if bursting */
 436         if (winner >= 0 && wvif->wdev->tx_burst_idx >= 0 &&
 437             winner != wvif->wdev->tx_burst_idx &&
 438             !wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[winner],
 439                                          tx_allowed_mask & urgent) &&
 440             wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[wvif->wdev->tx_burst_idx], tx_allowed_mask))
 441                 winner = wvif->wdev->tx_burst_idx;
 442
 443         return winner;
 444 }
 445
 446 static int wfx_tx_queue_mask_get(struct wfx_vif *wvif,
 447                                      struct wfx_queue **queue_p,
 448                                      u32 *tx_allowed_mask_p)
 449 {
 450         int idx;
 451         u32 tx_allowed_mask;
 452         int total = 0;
 453
 454         /* Search for unicast traffic */
 455         tx_allowed_mask = ~wvif->sta_asleep_mask;
 456         tx_allowed_mask |= BIT(WFX_LINK_ID_UAPSD);
 457         if (wvif->sta_asleep_mask)
 458                 tx_allowed_mask &= ~BIT(WFX_LINK_ID_AFTER_DTIM);
 459         else
 460                 tx_allowed_mask |= BIT(WFX_LINK_ID_AFTER_DTIM);
 461         idx = wfx_get_prio_queue(wvif, tx_allowed_mask, &total);
 462         if (idx < 0)
 463                 return -ENOENT;
 464
 465         *queue_p = &wvif->wdev->tx_queue[idx];
 466         *tx_allowed_mask_p = tx_allowed_mask;
 467         return 0;
 468 }
 469
 470 struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif)
 471 {
 472         struct wfx_dev *wdev = wvif->wdev;
 473         struct ieee80211_tx_info *tx_info;
 474         struct hif_msg *hif;
 475         struct sk_buff *skb;
 476         int i;
 477
 478         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 479                 skb_queue_walk(&wdev->tx_queue[i].queue, skb) {
 480                         tx_info = IEEE80211_SKB_CB(skb);
 481                         hif = (struct hif_msg *)skb->data;
 482                         if ((tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
 483                             (hif->interface == wvif->id))
 484                                 return (struct hif_msg *)skb->data;
 485                 }
 486         }
 487         return NULL;
 488 }
 489
 490 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev)
 491 {
 492         struct sk_buff *skb;
 493         struct hif_msg *hif = NULL;
 494         struct wfx_queue *queue = NULL;
 495         struct wfx_queue *vif_queue = NULL;
 496         u32 tx_allowed_mask = 0;
 497         u32 vif_tx_allowed_mask = 0;
 498         const struct wfx_tx_priv *tx_priv = NULL;
 499         struct wfx_vif *wvif;
 500         int not_found;
 501         int burst;
 502         int i;
 503
 504         if (atomic_read(&wdev->tx_lock))
 505                 return NULL;
 506
 507         wvif = NULL;
 508         while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 509                 if (wvif->after_dtim_tx_allowed) {
 510                         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 511                                 skb = wfx_tx_queue_get(wvif->wdev,
 512                                                        &wdev->tx_queue[i],
 513                                                        BIT(WFX_LINK_ID_AFTER_DTIM));
 514                                 if (skb) {
 515                                         hif = (struct hif_msg *)skb->data;
 516                                         // Cannot happen since only one vif can
 517                                         // be AP at time
 518                                         WARN_ON(wvif->id != hif->interface);
 519                                         return hif;
 520                                 }
 521                         }
 522                         // No more multicast to sent
 523                         wvif->after_dtim_tx_allowed = false;
 524                         schedule_work(&wvif->update_tim_work);
 525                 }
 526         }
 527
 528         for (;;) {
 529                 int ret = -ENOENT;
 530                 int queue_num;
 531
 532                 wvif = NULL;
 533                 while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 534                         spin_lock_bh(&wvif->ps_state_lock);
 535
 536                         not_found = wfx_tx_queue_mask_get(wvif, &vif_queue,
 537                                                           &vif_tx_allowed_mask);
 538
 539                         spin_unlock_bh(&wvif->ps_state_lock);
 540
 541                         if (!not_found) {
 542                                 if (queue && queue != vif_queue)
 543                                         dev_info(wdev->dev, "vifs disagree about queue priority\n");
 544                                 tx_allowed_mask |= vif_tx_allowed_mask;
 545                                 queue = vif_queue;
 546                                 ret = 0;
 547                         }
 548                 }
 549
 550                 if (ret)
 551                         return NULL;
 552
 553                 queue_num = queue - wdev->tx_queue;
 554
 555                 skb = wfx_tx_queue_get(wdev, queue, tx_allowed_mask);
 556                 if (!skb)
 557                         continue;
 558                 tx_priv = wfx_skb_tx_priv(skb);
 559                 hif = (struct hif_msg *) skb->data;
 560                 wvif = wdev_to_wvif(wdev, hif->interface);
 561                 WARN_ON(!wvif);
 562
 563                 if (hif_handle_tx_data(wvif, skb, queue))
 564                         continue;  /* Handled by WSM */
 565
 566                 /* allow bursting if txop is set */
 567                 if (wvif->edca_params[queue_num].txop)
 568                         burst = (int)wfx_tx_queue_get_num_queued(queue, tx_allowed_mask) + 1;
 569                 else
 570                         burst = 1;
 571
 572                 /* store index of bursting queue */
 573                 if (burst > 1)
 574                         wdev->tx_burst_idx = queue_num;
 575                 else
 576                         wdev->tx_burst_idx = -1;
 577
 578                 return hif;
 579         }
 580 }