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 
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 
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 
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 
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 
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 
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 
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 
133 static void cw1200_queue_gc(struct timer_list *t)
134 {
135 	LIST_HEAD(list);
136 	struct cw1200_queue *queue =
137 		from_timer(queue, t, gc);
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 
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 
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 	timer_setup(&queue->gc, cw1200_queue_gc, 0);
183 
184 	queue->pool = kzalloc(sizeof(struct cw1200_queue_item) * capacity,
185 			GFP_KERNEL);
186 	if (!queue->pool)
187 		return -ENOMEM;
188 
189 	queue->link_map_cache = kzalloc(sizeof(int) * stats->map_capacity,
190 			GFP_KERNEL);
191 	if (!queue->link_map_cache) {
192 		kfree(queue->pool);
193 		queue->pool = NULL;
194 		return -ENOMEM;
195 	}
196 
197 	for (i = 0; i < capacity; ++i)
198 		list_add_tail(&queue->pool[i].head, &queue->free_pool);
199 
200 	return 0;
201 }
202 
203 int cw1200_queue_clear(struct cw1200_queue *queue)
204 {
205 	int i;
206 	LIST_HEAD(gc_list);
207 	struct cw1200_queue_stats *stats = queue->stats;
208 	struct cw1200_queue_item *item, *tmp;
209 
210 	spin_lock_bh(&queue->lock);
211 	queue->generation++;
212 	list_splice_tail_init(&queue->queue, &queue->pending);
213 	list_for_each_entry_safe(item, tmp, &queue->pending, head) {
214 		WARN_ON(!item->skb);
215 		cw1200_queue_register_post_gc(&gc_list, item);
216 		item->skb = NULL;
217 		list_move_tail(&item->head, &queue->free_pool);
218 	}
219 	queue->num_queued = 0;
220 	queue->num_pending = 0;
221 
222 	spin_lock_bh(&stats->lock);
223 	for (i = 0; i < stats->map_capacity; ++i) {
224 		stats->num_queued -= queue->link_map_cache[i];
225 		stats->link_map_cache[i] -= queue->link_map_cache[i];
226 		queue->link_map_cache[i] = 0;
227 	}
228 	spin_unlock_bh(&stats->lock);
229 	if (queue->overfull) {
230 		queue->overfull = false;
231 		__cw1200_queue_unlock(queue);
232 	}
233 	spin_unlock_bh(&queue->lock);
234 	wake_up(&stats->wait_link_id_empty);
235 	cw1200_queue_post_gc(stats, &gc_list);
236 	return 0;
237 }
238 
239 void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
240 {
241 	kfree(stats->link_map_cache);
242 	stats->link_map_cache = NULL;
243 }
244 
245 void cw1200_queue_deinit(struct cw1200_queue *queue)
246 {
247 	cw1200_queue_clear(queue);
248 	del_timer_sync(&queue->gc);
249 	INIT_LIST_HEAD(&queue->free_pool);
250 	kfree(queue->pool);
251 	kfree(queue->link_map_cache);
252 	queue->pool = NULL;
253 	queue->link_map_cache = NULL;
254 	queue->capacity = 0;
255 }
256 
257 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
258 				   u32 link_id_map)
259 {
260 	size_t ret;
261 	int i, bit;
262 	size_t map_capacity = queue->stats->map_capacity;
263 
264 	if (!link_id_map)
265 		return 0;
266 
267 	spin_lock_bh(&queue->lock);
268 	if (link_id_map == (u32)-1) {
269 		ret = queue->num_queued - queue->num_pending;
270 	} else {
271 		ret = 0;
272 		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
273 			if (link_id_map & bit)
274 				ret += queue->link_map_cache[i];
275 		}
276 	}
277 	spin_unlock_bh(&queue->lock);
278 	return ret;
279 }
280 
281 int cw1200_queue_put(struct cw1200_queue *queue,
282 		     struct sk_buff *skb,
283 		     struct cw1200_txpriv *txpriv)
284 {
285 	int ret = 0;
286 	LIST_HEAD(gc_list);
287 	struct cw1200_queue_stats *stats = queue->stats;
288 
289 	if (txpriv->link_id >= queue->stats->map_capacity)
290 		return -EINVAL;
291 
292 	spin_lock_bh(&queue->lock);
293 	if (!WARN_ON(list_empty(&queue->free_pool))) {
294 		struct cw1200_queue_item *item = list_first_entry(
295 			&queue->free_pool, struct cw1200_queue_item, head);
296 		BUG_ON(item->skb);
297 
298 		list_move_tail(&item->head, &queue->queue);
299 		item->skb = skb;
300 		item->txpriv = *txpriv;
301 		item->generation = 0;
302 		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
303 							    queue->queue_id,
304 							    item->generation,
305 							    item - queue->pool);
306 		item->queue_timestamp = jiffies;
307 
308 		++queue->num_queued;
309 		++queue->link_map_cache[txpriv->link_id];
310 
311 		spin_lock_bh(&stats->lock);
312 		++stats->num_queued;
313 		++stats->link_map_cache[txpriv->link_id];
314 		spin_unlock_bh(&stats->lock);
315 
316 		/* TX may happen in parallel sometimes.
317 		 * Leave extra queue slots so we don't overflow.
318 		 */
319 		if (queue->overfull == false &&
320 		    queue->num_queued >=
321 		    (queue->capacity - (num_present_cpus() - 1))) {
322 			queue->overfull = true;
323 			__cw1200_queue_lock(queue);
324 			mod_timer(&queue->gc, jiffies);
325 		}
326 	} else {
327 		ret = -ENOENT;
328 	}
329 	spin_unlock_bh(&queue->lock);
330 	return ret;
331 }
332 
333 int cw1200_queue_get(struct cw1200_queue *queue,
334 		     u32 link_id_map,
335 		     struct wsm_tx **tx,
336 		     struct ieee80211_tx_info **tx_info,
337 		     const struct cw1200_txpriv **txpriv)
338 {
339 	int ret = -ENOENT;
340 	struct cw1200_queue_item *item;
341 	struct cw1200_queue_stats *stats = queue->stats;
342 	bool wakeup_stats = false;
343 
344 	spin_lock_bh(&queue->lock);
345 	list_for_each_entry(item, &queue->queue, head) {
346 		if (link_id_map & BIT(item->txpriv.link_id)) {
347 			ret = 0;
348 			break;
349 		}
350 	}
351 
352 	if (!WARN_ON(ret)) {
353 		*tx = (struct wsm_tx *)item->skb->data;
354 		*tx_info = IEEE80211_SKB_CB(item->skb);
355 		*txpriv = &item->txpriv;
356 		(*tx)->packet_id = item->packet_id;
357 		list_move_tail(&item->head, &queue->pending);
358 		++queue->num_pending;
359 		--queue->link_map_cache[item->txpriv.link_id];
360 		item->xmit_timestamp = jiffies;
361 
362 		spin_lock_bh(&stats->lock);
363 		--stats->num_queued;
364 		if (!--stats->link_map_cache[item->txpriv.link_id])
365 			wakeup_stats = true;
366 		spin_unlock_bh(&stats->lock);
367 	}
368 	spin_unlock_bh(&queue->lock);
369 	if (wakeup_stats)
370 		wake_up(&stats->wait_link_id_empty);
371 	return ret;
372 }
373 
374 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id)
375 {
376 	int ret = 0;
377 	u8 queue_generation, queue_id, item_generation, item_id;
378 	struct cw1200_queue_item *item;
379 	struct cw1200_queue_stats *stats = queue->stats;
380 
381 	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
382 			      &item_generation, &item_id);
383 
384 	item = &queue->pool[item_id];
385 
386 	spin_lock_bh(&queue->lock);
387 	BUG_ON(queue_id != queue->queue_id);
388 	if (queue_generation != queue->generation) {
389 		ret = -ENOENT;
390 	} else if (item_id >= (unsigned) queue->capacity) {
391 		WARN_ON(1);
392 		ret = -EINVAL;
393 	} else if (item->generation != item_generation) {
394 		WARN_ON(1);
395 		ret = -ENOENT;
396 	} else {
397 		--queue->num_pending;
398 		++queue->link_map_cache[item->txpriv.link_id];
399 
400 		spin_lock_bh(&stats->lock);
401 		++stats->num_queued;
402 		++stats->link_map_cache[item->txpriv.link_id];
403 		spin_unlock_bh(&stats->lock);
404 
405 		item->generation = ++item_generation;
406 		item->packet_id = cw1200_queue_mk_packet_id(queue_generation,
407 							    queue_id,
408 							    item_generation,
409 							    item_id);
410 		list_move(&item->head, &queue->queue);
411 	}
412 	spin_unlock_bh(&queue->lock);
413 	return ret;
414 }
415 
416 int cw1200_queue_requeue_all(struct cw1200_queue *queue)
417 {
418 	struct cw1200_queue_item *item, *tmp;
419 	struct cw1200_queue_stats *stats = queue->stats;
420 	spin_lock_bh(&queue->lock);
421 
422 	list_for_each_entry_safe_reverse(item, tmp, &queue->pending, head) {
423 		--queue->num_pending;
424 		++queue->link_map_cache[item->txpriv.link_id];
425 
426 		spin_lock_bh(&stats->lock);
427 		++stats->num_queued;
428 		++stats->link_map_cache[item->txpriv.link_id];
429 		spin_unlock_bh(&stats->lock);
430 
431 		++item->generation;
432 		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
433 							    queue->queue_id,
434 							    item->generation,
435 							    item - queue->pool);
436 		list_move(&item->head, &queue->queue);
437 	}
438 	spin_unlock_bh(&queue->lock);
439 
440 	return 0;
441 }
442 
443 int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id)
444 {
445 	int ret = 0;
446 	u8 queue_generation, queue_id, item_generation, item_id;
447 	struct cw1200_queue_item *item;
448 	struct cw1200_queue_stats *stats = queue->stats;
449 	struct sk_buff *gc_skb = NULL;
450 	struct cw1200_txpriv gc_txpriv;
451 
452 	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
453 			      &item_generation, &item_id);
454 
455 	item = &queue->pool[item_id];
456 
457 	spin_lock_bh(&queue->lock);
458 	BUG_ON(queue_id != queue->queue_id);
459 	if (queue_generation != queue->generation) {
460 		ret = -ENOENT;
461 	} else if (item_id >= (unsigned) queue->capacity) {
462 		WARN_ON(1);
463 		ret = -EINVAL;
464 	} else if (item->generation != item_generation) {
465 		WARN_ON(1);
466 		ret = -ENOENT;
467 	} else {
468 		gc_txpriv = item->txpriv;
469 		gc_skb = item->skb;
470 		item->skb = NULL;
471 		--queue->num_pending;
472 		--queue->num_queued;
473 		++queue->num_sent;
474 		++item->generation;
475 		/* Do not use list_move_tail here, but list_move:
476 		 * try to utilize cache row.
477 		 */
478 		list_move(&item->head, &queue->free_pool);
479 
480 		if (queue->overfull &&
481 		    (queue->num_queued <= (queue->capacity >> 1))) {
482 			queue->overfull = false;
483 			__cw1200_queue_unlock(queue);
484 		}
485 	}
486 	spin_unlock_bh(&queue->lock);
487 
488 	if (gc_skb)
489 		stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv);
490 
491 	return ret;
492 }
493 
494 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id,
495 			 struct sk_buff **skb,
496 			 const struct cw1200_txpriv **txpriv)
497 {
498 	int ret = 0;
499 	u8 queue_generation, queue_id, item_generation, item_id;
500 	struct cw1200_queue_item *item;
501 	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
502 			      &item_generation, &item_id);
503 
504 	item = &queue->pool[item_id];
505 
506 	spin_lock_bh(&queue->lock);
507 	BUG_ON(queue_id != queue->queue_id);
508 	if (queue_generation != queue->generation) {
509 		ret = -ENOENT;
510 	} else if (item_id >= (unsigned) queue->capacity) {
511 		WARN_ON(1);
512 		ret = -EINVAL;
513 	} else if (item->generation != item_generation) {
514 		WARN_ON(1);
515 		ret = -ENOENT;
516 	} else {
517 		*skb = item->skb;
518 		*txpriv = &item->txpriv;
519 	}
520 	spin_unlock_bh(&queue->lock);
521 	return ret;
522 }
523 
524 void cw1200_queue_lock(struct cw1200_queue *queue)
525 {
526 	spin_lock_bh(&queue->lock);
527 	__cw1200_queue_lock(queue);
528 	spin_unlock_bh(&queue->lock);
529 }
530 
531 void cw1200_queue_unlock(struct cw1200_queue *queue)
532 {
533 	spin_lock_bh(&queue->lock);
534 	__cw1200_queue_unlock(queue);
535 	spin_unlock_bh(&queue->lock);
536 }
537 
538 bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
539 				     unsigned long *timestamp,
540 				     u32 pending_frame_id)
541 {
542 	struct cw1200_queue_item *item;
543 	bool ret;
544 
545 	spin_lock_bh(&queue->lock);
546 	ret = !list_empty(&queue->pending);
547 	if (ret) {
548 		list_for_each_entry(item, &queue->pending, head) {
549 			if (item->packet_id != pending_frame_id)
550 				if (time_before(item->xmit_timestamp,
551 						*timestamp))
552 					*timestamp = item->xmit_timestamp;
553 		}
554 	}
555 	spin_unlock_bh(&queue->lock);
556 	return ret;
557 }
558 
559 bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
560 				 u32 link_id_map)
561 {
562 	bool empty = true;
563 
564 	spin_lock_bh(&stats->lock);
565 	if (link_id_map == (u32)-1) {
566 		empty = stats->num_queued == 0;
567 	} else {
568 		int i;
569 		for (i = 0; i < stats->map_capacity; ++i) {
570 			if (link_id_map & BIT(i)) {
571 				if (stats->link_map_cache[i]) {
572 					empty = false;
573 					break;
574 				}
575 			}
576 		}
577 	}
578 	spin_unlock_bh(&stats->lock);
579 
580 	return empty;
581 }
582