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