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