?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
treewide: remove redundant IS_ERR() before error code check
[linux/fpc-iii.git] / drivers / staging / wfx / queue.c
blob0bcc61feee1d24b1f765224229b53b718ad525a5
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 int wfx_tx_queue_get_num_queued(struct wfx_queue *queue, u32 link_id_map)
179 {
180 int ret, i;
181
182 if (!link_id_map)
183 return 0;
184
185 spin_lock_bh(&queue->queue.lock);
186 if (link_id_map == (u32)-1) {
187 ret = skb_queue_len(&queue->queue);
188 } else {
189 ret = 0;
190 for (i = 0; i < ARRAY_SIZE(queue->link_map_cache); i++)
191 if (link_id_map & BIT(i))
192 ret += queue->link_map_cache[i];
193 }
194 spin_unlock_bh(&queue->queue.lock);
195 return ret;
196 }
197
198 void wfx_tx_queue_put(struct wfx_dev *wdev, struct wfx_queue *queue,
199 struct sk_buff *skb)
200 {
201 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
202 struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
203
204 WARN(tx_priv->link_id >= ARRAY_SIZE(stats->link_map_cache), "invalid link-id value");
205 spin_lock_bh(&queue->queue.lock);
206 __skb_queue_tail(&queue->queue, skb);
207
208 ++queue->link_map_cache[tx_priv->link_id];
209
210 spin_lock_bh(&stats->pending.lock);
211 ++stats->link_map_cache[tx_priv->link_id];
212 spin_unlock_bh(&stats->pending.lock);
213 spin_unlock_bh(&queue->queue.lock);
214 }
215
216 static struct sk_buff *wfx_tx_queue_get(struct wfx_dev *wdev,
217 struct wfx_queue *queue,
218 u32 link_id_map)
219 {
220 struct sk_buff *skb = NULL;
221 struct sk_buff *item;
222 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
223 struct wfx_tx_priv *tx_priv;
224 bool wakeup_stats = false;
225
226 spin_lock_bh(&queue->queue.lock);
227 skb_queue_walk(&queue->queue, item) {
228 tx_priv = wfx_skb_tx_priv(item);
229 if (link_id_map & BIT(tx_priv->link_id)) {
230 skb = item;
231 break;
232 }
233 }
234 if (skb) {
235 tx_priv = wfx_skb_tx_priv(skb);
236 tx_priv->xmit_timestamp = ktime_get();
237 __skb_unlink(skb, &queue->queue);
238 --queue->link_map_cache[tx_priv->link_id];
239
240 spin_lock_bh(&stats->pending.lock);
241 __skb_queue_tail(&stats->pending, skb);
242 if (!--stats->link_map_cache[tx_priv->link_id])
243 wakeup_stats = true;
244 spin_unlock_bh(&stats->pending.lock);
245 }
246 spin_unlock_bh(&queue->queue.lock);
247 if (wakeup_stats)
248 wake_up(&stats->wait_link_id_empty);
249 return skb;
250 }
251
252 int wfx_pending_requeue(struct wfx_dev *wdev, struct sk_buff *skb)
253 {
254 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
255 struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
256 struct wfx_queue *queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
257
258 WARN_ON(skb_get_queue_mapping(skb) > 3);
259 spin_lock_bh(&queue->queue.lock);
260 ++queue->link_map_cache[tx_priv->link_id];
261
262 spin_lock_bh(&stats->pending.lock);
263 ++stats->link_map_cache[tx_priv->link_id];
264 __skb_unlink(skb, &stats->pending);
265 spin_unlock_bh(&stats->pending.lock);
266 __skb_queue_tail(&queue->queue, skb);
267 spin_unlock_bh(&queue->queue.lock);
268 return 0;
269 }
270
271 int wfx_pending_remove(struct wfx_dev *wdev, struct sk_buff *skb)
272 {
273 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
274
275 spin_lock_bh(&stats->pending.lock);
276 __skb_unlink(skb, &stats->pending);
277 spin_unlock_bh(&stats->pending.lock);
278 wfx_skb_dtor(wdev, skb);
279
280 return 0;
281 }
282
283 struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id)
284 {
285 struct sk_buff *skb;
286 struct hif_req_tx *req;
287 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
288
289 spin_lock_bh(&stats->pending.lock);
290 skb_queue_walk(&stats->pending, skb) {
291 req = wfx_skb_txreq(skb);
292 if (req->packet_id == packet_id) {
293 spin_unlock_bh(&stats->pending.lock);
294 return skb;
295 }
296 }
297 spin_unlock_bh(&stats->pending.lock);
298 WARN(1, "cannot find packet in pending queue");
299 return NULL;
300 }
301
302 void wfx_pending_dump_old_frames(struct wfx_dev *wdev, unsigned int limit_ms)
303 {
304 struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
305 ktime_t now = ktime_get();
306 struct wfx_tx_priv *tx_priv;
307 struct hif_req_tx *req;
308 struct sk_buff *skb;
309 bool first = true;
310
311 spin_lock_bh(&stats->pending.lock);
312 skb_queue_walk(&stats->pending, skb) {
313 tx_priv = wfx_skb_tx_priv(skb);
314 req = wfx_skb_txreq(skb);
315 if (ktime_after(now, ktime_add_ms(tx_priv->xmit_timestamp,
316 limit_ms))) {
317 if (first) {
318 dev_info(wdev->dev, "frames stuck in firmware since %dms or more:\n",
319 limit_ms);
320 first = false;
321 }
322 dev_info(wdev->dev, " id %08x sent %lldms ago\n",
323 req->packet_id,
324 ktime_ms_delta(now, tx_priv->xmit_timestamp));
325 }
326 }
327 spin_unlock_bh(&stats->pending.lock);
328 }
329
330 unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
331 struct sk_buff *skb)
332 {
333 ktime_t now = ktime_get();
334 struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
335
336 return ktime_us_delta(now, tx_priv->xmit_timestamp);
337 }
338
339 bool wfx_tx_queues_is_empty(struct wfx_dev *wdev)
340 {
341 int i;
342 struct sk_buff_head *queue;
343 bool ret = true;
344
345 for (i = 0; i < IEEE80211_NUM_ACS; i++) {
346 queue = &wdev->tx_queue[i].queue;
347 spin_lock_bh(&queue->lock);
348 if (!skb_queue_empty(queue))
349 ret = false;
350 spin_unlock_bh(&queue->lock);
351 }
352 return ret;
353 }
354
355 static bool hif_handle_tx_data(struct wfx_vif *wvif, struct sk_buff *skb,
356 struct wfx_queue *queue)
357 {
358 struct hif_req_tx *req = wfx_skb_txreq(skb);
359 struct ieee80211_key_conf *hw_key = wfx_skb_tx_priv(skb)->hw_key;
360 struct ieee80211_hdr *frame =
361 (struct ieee80211_hdr *)(req->frame + req->data_flags.fc_offset);
362
363 // FIXME: mac80211 is smart enough to handle BSS loss. Driver should not
364 // try to do anything about that.
365 if (ieee80211_is_nullfunc(frame->frame_control)) {
366 mutex_lock(&wvif->bss_loss_lock);
367 if (wvif->bss_loss_state) {
368 wvif->bss_loss_confirm_id = req->packet_id;
369 req->queue_id.queue_id = HIF_QUEUE_ID_VOICE;
370 }
371 mutex_unlock(&wvif->bss_loss_lock);
372 }
373
374 // FIXME: identify the exact scenario matched by this condition. Does it
375 // happen yet?
376 if (ieee80211_has_protected(frame->frame_control) &&
377 hw_key && hw_key->keyidx != wvif->wep_default_key_id &&
378 (hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
379 hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
380 wfx_tx_lock(wvif->wdev);
381 WARN_ON(wvif->wep_pending_skb);
382 wvif->wep_default_key_id = hw_key->keyidx;
383 wvif->wep_pending_skb = skb;
384 if (!schedule_work(&wvif->wep_key_work))
385 wfx_tx_unlock(wvif->wdev);
386 return true;
387 } else {
388 return false;
389 }
390 }
391
392 static int wfx_get_prio_queue(struct wfx_vif *wvif,
393 u32 tx_allowed_mask, int *total)
394 {
395 static const int urgent = BIT(WFX_LINK_ID_AFTER_DTIM) |
396 BIT(WFX_LINK_ID_UAPSD);
397 const struct ieee80211_tx_queue_params *edca;
398 unsigned int score, best = -1;
399 int winner = -1;
400 int i;
401
402 /* search for a winner using edca params */
403 for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
404 int queued;
405
406 edca = &wvif->edca_params[i];
407 queued = wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[i],
408 tx_allowed_mask);
409 if (!queued)
410 continue;
411 *total += queued;
412 score = ((edca->aifs + edca->cw_min) << 16) +
413 ((edca->cw_max - edca->cw_min) *
414 (get_random_int() & 0xFFFF));
415 if (score < best && (winner < 0 || i != 3)) {
416 best = score;
417 winner = i;
418 }
419 }
420
421 /* override winner if bursting */
422 if (winner >= 0 && wvif->wdev->tx_burst_idx >= 0 &&
423 winner != wvif->wdev->tx_burst_idx &&
424 !wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[winner],
425 tx_allowed_mask & urgent) &&
426 wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[wvif->wdev->tx_burst_idx], tx_allowed_mask))
427 winner = wvif->wdev->tx_burst_idx;
428
429 return winner;
430 }
431
432 static int wfx_tx_queue_mask_get(struct wfx_vif *wvif,
433 struct wfx_queue **queue_p,
434 u32 *tx_allowed_mask_p)
435 {
436 int idx;
437 u32 tx_allowed_mask;
438 int total = 0;
439
440 /* Search for unicast traffic */
441 tx_allowed_mask = ~wvif->sta_asleep_mask;
442 tx_allowed_mask |= BIT(WFX_LINK_ID_UAPSD);
443 if (wvif->sta_asleep_mask)
444 tx_allowed_mask &= ~BIT(WFX_LINK_ID_AFTER_DTIM);
445 else
446 tx_allowed_mask |= BIT(WFX_LINK_ID_AFTER_DTIM);
447 idx = wfx_get_prio_queue(wvif, tx_allowed_mask, &total);
448 if (idx < 0)
449 return -ENOENT;
450
451 *queue_p = &wvif->wdev->tx_queue[idx];
452 *tx_allowed_mask_p = tx_allowed_mask;
453 return 0;
454 }
455
456 struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif)
457 {
458 struct wfx_dev *wdev = wvif->wdev;
459 struct ieee80211_tx_info *tx_info;
460 struct hif_msg *hif;
461 struct sk_buff *skb;
462 int i;
463
464 for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
465 skb_queue_walk(&wdev->tx_queue[i].queue, skb) {
466 tx_info = IEEE80211_SKB_CB(skb);
467 hif = (struct hif_msg *)skb->data;
468 if ((tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
469 (hif->interface == wvif->id))
470 return (struct hif_msg *)skb->data;
471 }
472 }
473 return NULL;
474 }
475
476 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev)
477 {
478 struct sk_buff *skb;
479 struct hif_msg *hif = NULL;
480 struct wfx_queue *queue = NULL;
481 struct wfx_queue *vif_queue = NULL;
482 u32 tx_allowed_mask = 0;
483 u32 vif_tx_allowed_mask = 0;
484 const struct wfx_tx_priv *tx_priv = NULL;
485 struct wfx_vif *wvif;
486 int not_found;
487 int burst;
488 int i;
489
490 if (atomic_read(&wdev->tx_lock))
491 return NULL;
492
493 wvif = NULL;
494 while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
495 if (wvif->after_dtim_tx_allowed) {
496 for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
497 skb = wfx_tx_queue_get(wvif->wdev,
498 &wdev->tx_queue[i],
499 BIT(WFX_LINK_ID_AFTER_DTIM));
500 if (skb) {
501 hif = (struct hif_msg *)skb->data;
502 // Cannot happen since only one vif can
503 // be AP at time
504 WARN_ON(wvif->id != hif->interface);
505 return hif;
506 }
507 }
508 // No more multicast to sent
509 wvif->after_dtim_tx_allowed = false;
510 schedule_work(&wvif->update_tim_work);
511 }
512 }
513
514 for (;;) {
515 int ret = -ENOENT;
516 int queue_num;
517
518 wvif = NULL;
519 while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
520 spin_lock_bh(&wvif->ps_state_lock);
521
522 not_found = wfx_tx_queue_mask_get(wvif, &vif_queue,
523 &vif_tx_allowed_mask);
524
525 spin_unlock_bh(&wvif->ps_state_lock);
526
527 if (!not_found) {
528 if (queue && queue != vif_queue)
529 dev_info(wdev->dev, "vifs disagree about queue priority\n");
530 tx_allowed_mask |= vif_tx_allowed_mask;
531 queue = vif_queue;
532 ret = 0;
533 }
534 }
535
536 if (ret)
537 return NULL;
538
539 queue_num = queue - wdev->tx_queue;
540
541 skb = wfx_tx_queue_get(wdev, queue, tx_allowed_mask);
542 if (!skb)
543 continue;
544 tx_priv = wfx_skb_tx_priv(skb);
545 hif = (struct hif_msg *) skb->data;
546 wvif = wdev_to_wvif(wdev, hif->interface);
547 WARN_ON(!wvif);
548
549 if (hif_handle_tx_data(wvif, skb, queue))
550 continue; /* Handled by WSM */
551
552 /* allow bursting if txop is set */
553 if (wvif->edca_params[queue_num].txop)
554 burst = wfx_tx_queue_get_num_queued(queue, tx_allowed_mask) + 1;
555 else
556 burst = 1;
557
558 /* store index of bursting queue */
559 if (burst > 1)
560 wdev->tx_burst_idx = queue_num;
561 else
562 wdev->tx_burst_idx = -1;
563
564 return hif;
565 }
566 }
Linux with added FPC-III support