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
__cw1200_queue_lock(struct cw1200_queue * queue)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
__cw1200_queue_unlock(struct cw1200_queue * queue)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
cw1200_queue_parse_id(u32 packet_id,u8 * queue_generation,u8 * queue_id,u8 * item_generation,u8 * item_id)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
cw1200_queue_mk_packet_id(u8 queue_generation,u8 queue_id,u8 item_generation,u8 item_id)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
cw1200_queue_post_gc(struct cw1200_queue_stats * stats,struct list_head * gc_list)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
cw1200_queue_register_post_gc(struct list_head * gc_list,struct cw1200_queue_item * item)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
__cw1200_queue_gc(struct cw1200_queue * queue,struct list_head * head,bool unlock)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
cw1200_queue_gc(struct timer_list * t)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
cw1200_queue_stats_init(struct cw1200_queue_stats * stats,size_t map_capacity,cw1200_queue_skb_dtor_t skb_dtor,struct cw1200_common * priv)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
cw1200_queue_init(struct cw1200_queue * queue,struct cw1200_queue_stats * stats,u8 queue_id,size_t capacity,unsigned long ttl)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
cw1200_queue_clear(struct cw1200_queue * queue)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
cw1200_queue_stats_deinit(struct cw1200_queue_stats * stats)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
cw1200_queue_deinit(struct cw1200_queue * queue)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
cw1200_queue_get_num_queued(struct cw1200_queue * queue,u32 link_id_map)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
cw1200_queue_put(struct cw1200_queue * queue,struct sk_buff * skb,struct cw1200_txpriv * txpriv)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
cw1200_queue_get(struct cw1200_queue * queue,u32 link_id_map,struct wsm_tx ** tx,struct ieee80211_tx_info ** tx_info,const struct cw1200_txpriv ** txpriv)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
cw1200_queue_requeue(struct cw1200_queue * queue,u32 packet_id)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
cw1200_queue_requeue_all(struct cw1200_queue * queue)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
cw1200_queue_remove(struct cw1200_queue * queue,u32 packet_id)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
cw1200_queue_get_skb(struct cw1200_queue * queue,u32 packet_id,struct sk_buff ** skb,const struct cw1200_txpriv ** txpriv)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
cw1200_queue_lock(struct cw1200_queue * queue)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
cw1200_queue_unlock(struct cw1200_queue * queue)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
cw1200_queue_get_xmit_timestamp(struct cw1200_queue * queue,unsigned long * timestamp,u32 pending_frame_id)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
cw1200_queue_stats_is_empty(struct cw1200_queue_stats * stats,u32 link_id_map)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 }
577