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         struct hif_req_tx *req = wfx_skb_txreq(skb);
 363         struct ieee80211_key_conf *hw_key = wfx_skb_tx_priv(skb)->hw_key;
 364         struct ieee80211_hdr *frame =
 365                 (struct ieee80211_hdr *)(req->frame + req->data_flags.fc_offset);
 366
 367         // FIXME: mac80211 is smart enough to handle BSS loss. Driver should not
 368         // try to do anything about that.
 369         if (ieee80211_is_nullfunc(frame->frame_control)) {
 370                 mutex_lock(&wvif->bss_loss_lock);
 371                 if (wvif->bss_loss_state) {
 372                         wvif->bss_loss_confirm_id = req->packet_id;
 373                         req->queue_id.queue_id = HIF_QUEUE_ID_VOICE;
 374                 }
 375                 mutex_unlock(&wvif->bss_loss_lock);
 376         }
 377
 378         // FIXME: identify the exact scenario matched by this condition. Does it
 379         // happen yet?
 380         if (ieee80211_has_protected(frame->frame_control) &&
 381             hw_key && hw_key->keyidx != wvif->wep_default_key_id &&
 382             (hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
 383              hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
 384                 wfx_tx_lock(wvif->wdev);
 385                 WARN_ON(wvif->wep_pending_skb);
 386                 wvif->wep_default_key_id = hw_key->keyidx;
 387                 wvif->wep_pending_skb = skb;
 388                 if (!schedule_work(&wvif->wep_key_work))
 389                         wfx_tx_unlock(wvif->wdev);
 390                 return true;
 391         } else {
 392                 return false;
 393         }
 394 }
 395
 396 static int wfx_get_prio_queue(struct wfx_vif *wvif,
 397                                  u32 tx_allowed_mask, int *total)
 398 {
 399         static const int urgent = BIT(WFX_LINK_ID_AFTER_DTIM) |
 400                 BIT(WFX_LINK_ID_UAPSD);
 401         const struct ieee80211_tx_queue_params *edca;
 402         unsigned int score, best = -1;
 403         int winner = -1;
 404         int i;
 405
 406         /* search for a winner using edca params */
 407         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 408                 int queued;
 409
 410                 edca = &wvif->edca_params[i];
 411                 queued = wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[i],
 412                                 tx_allowed_mask);
 413                 if (!queued)
 414                         continue;
 415                 *total += queued;
 416                 score = ((edca->aifs + edca->cw_min) << 16) +
 417                         ((edca->cw_max - edca->cw_min) *
 418                          (get_random_int() & 0xFFFF));
 419                 if (score < best && (winner < 0 || i != 3)) {
 420                         best = score;
 421                         winner = i;
 422                 }
 423         }
 424
 425         /* override winner if bursting */
 426         if (winner >= 0 && wvif->wdev->tx_burst_idx >= 0 &&
 427             winner != wvif->wdev->tx_burst_idx &&
 428             !wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[winner],
 429                                          tx_allowed_mask & urgent) &&
 430             wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[wvif->wdev->tx_burst_idx], tx_allowed_mask))
 431                 winner = wvif->wdev->tx_burst_idx;
 432
 433         return winner;
 434 }
 435
 436 static int wfx_tx_queue_mask_get(struct wfx_vif *wvif,
 437                                      struct wfx_queue **queue_p,
 438                                      u32 *tx_allowed_mask_p)
 439 {
 440         int idx;
 441         u32 tx_allowed_mask;
 442         int total = 0;
 443
 444         /* Search for unicast traffic */
 445         tx_allowed_mask = ~wvif->sta_asleep_mask;
 446         tx_allowed_mask |= BIT(WFX_LINK_ID_UAPSD);
 447         if (wvif->sta_asleep_mask)
 448                 tx_allowed_mask &= ~BIT(WFX_LINK_ID_AFTER_DTIM);
 449         else
 450                 tx_allowed_mask |= BIT(WFX_LINK_ID_AFTER_DTIM);
 451         idx = wfx_get_prio_queue(wvif, tx_allowed_mask, &total);
 452         if (idx < 0)
 453                 return -ENOENT;
 454
 455         *queue_p = &wvif->wdev->tx_queue[idx];
 456         *tx_allowed_mask_p = tx_allowed_mask;
 457         return 0;
 458 }
 459
 460 struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif)
 461 {
 462         struct wfx_dev *wdev = wvif->wdev;
 463         struct ieee80211_tx_info *tx_info;
 464         struct hif_msg *hif;
 465         struct sk_buff *skb;
 466         int i;
 467
 468         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 469                 skb_queue_walk(&wdev->tx_queue[i].queue, skb) {
 470                         tx_info = IEEE80211_SKB_CB(skb);
 471                         hif = (struct hif_msg *)skb->data;
 472                         if ((tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
 473                             (hif->interface == wvif->id))
 474                                 return (struct hif_msg *)skb->data;
 475                 }
 476         }
 477         return NULL;
 478 }
 479
 480 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev)
 481 {
 482         struct sk_buff *skb;
 483         struct hif_msg *hif = NULL;
 484         struct wfx_queue *queue = NULL;
 485         struct wfx_queue *vif_queue = NULL;
 486         u32 tx_allowed_mask = 0;
 487         u32 vif_tx_allowed_mask = 0;
 488         const struct wfx_tx_priv *tx_priv = NULL;
 489         struct wfx_vif *wvif;
 490         int not_found;
 491         int burst;
 492         int i;
 493
 494         if (atomic_read(&wdev->tx_lock))
 495                 return NULL;
 496
 497         wvif = NULL;
 498         while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 499                 if (wvif->after_dtim_tx_allowed) {
 500                         for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
 501                                 skb = wfx_tx_queue_get(wvif->wdev,
 502                                                        &wdev->tx_queue[i],
 503                                                        BIT(WFX_LINK_ID_AFTER_DTIM));
 504                                 if (skb) {
 505                                         hif = (struct hif_msg *)skb->data;
 506                                         // Cannot happen since only one vif can
 507                                         // be AP at time
 508                                         WARN_ON(wvif->id != hif->interface);
 509                                         return hif;
 510                                 }
 511                         }
 512                         // No more multicast to sent
 513                         wvif->after_dtim_tx_allowed = false;
 514                         schedule_work(&wvif->update_tim_work);
 515                 }
 516         }
 517
 518         for (;;) {
 519                 int ret = -ENOENT;
 520                 int queue_num;
 521
 522                 wvif = NULL;
 523                 while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 524                         spin_lock_bh(&wvif->ps_state_lock);
 525
 526                         not_found = wfx_tx_queue_mask_get(wvif, &vif_queue,
 527                                                           &vif_tx_allowed_mask);
 528
 529                         spin_unlock_bh(&wvif->ps_state_lock);
 530
 531                         if (!not_found) {
 532                                 if (queue && queue != vif_queue)
 533                                         dev_info(wdev->dev, "vifs disagree about queue priority\n");
 534                                 tx_allowed_mask |= vif_tx_allowed_mask;
 535                                 queue = vif_queue;
 536                                 ret = 0;
 537                         }
 538                 }
 539
 540                 if (ret)
 541                         return NULL;
 542
 543                 queue_num = queue - wdev->tx_queue;
 544
 545                 skb = wfx_tx_queue_get(wdev, queue, tx_allowed_mask);
 546                 if (!skb)
 547                         continue;
 548                 tx_priv = wfx_skb_tx_priv(skb);
 549                 hif = (struct hif_msg *) skb->data;
 550                 wvif = wdev_to_wvif(wdev, hif->interface);
 551                 WARN_ON(!wvif);
 552
 553                 if (hif_handle_tx_data(wvif, skb, queue))
 554                         continue;  /* Handled by WSM */
 555
 556                 /* allow bursting if txop is set */
 557                 if (wvif->edca_params[queue_num].txop)
 558                         burst = (int)wfx_tx_queue_get_num_queued(queue, tx_allowed_mask) + 1;
 559                 else
 560                         burst = 1;
 561
 562                 /* store index of bursting queue */
 563                 if (burst > 1)
 564                         wdev->tx_burst_idx = queue_num;
 565                 else
 566                         wdev->tx_burst_idx = -1;
 567
 568                 return hif;
 569         }
 570 }