st/cw1200/queue.c

3  * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
12 #include "queue.h"
27 static inline void __cw1200_queue_lock(struct cw1200_queue *queue)
29 	struct cw1200_queue_stats *stats = queue->stats;
30 	if (queue->tx_locked_cnt++ == 0) {
32 			 queue->queue_id);
33 		ieee80211_stop_queue(stats->priv->hw, queue->queue_id);
37 static inline void __cw1200_queue_unlock(struct cw1200_queue *queue)
39 	struct cw1200_queue_stats *stats = queue->stats;
40 	BUG_ON(!queue->tx_locked_cnt);
41 	if (--queue->tx_locked_cnt == 0) {
43 			 queue->queue_id);
44 		ieee80211_wake_queue(stats->priv->hw, queue->queue_id);
89 static void __cw1200_queue_gc(struct cw1200_queue *queue,
93 	struct cw1200_queue_stats *stats = queue->stats;
97 	list_for_each_entry_safe(iter, tmp, &queue->queue, head) {
98 		if (time_is_after_jiffies(iter->queue_timestamp + queue->ttl)) {
102 		--queue->num_queued;
103 		--queue->link_map_cache[iter->txpriv.link_id];
112 		list_move_tail(&iter->head, &queue->free_pool);
118 	if (queue->overfull) {
119 		if (queue->num_queued <= (queue->capacity >> 1)) {
120 			queue->overfull = false;
122 				__cw1200_queue_unlock(queue);
124 			unsigned long tmo = item->queue_timestamp + queue->ttl;
125 			mod_timer(&queue->gc, tmo);
135 	struct cw1200_queue *queue =
136 		from_timer(queue, t, gc);
138 	spin_lock_bh(&queue->lock);
139 	__cw1200_queue_gc(queue, &list, true);
140 	spin_unlock_bh(&queue->lock);
141 	cw1200_queue_post_gc(queue->stats, &list);
164 int cw1200_queue_init(struct cw1200_queue *queue,
172 	memset(queue, 0, sizeof(*queue));
173 	queue->stats = stats;
174 	queue->capacity = capacity;
175 	queue->queue_id = queue_id;
176 	queue->ttl = ttl;
177 	INIT_LIST_HEAD(&queue->queue);
178 	INIT_LIST_HEAD(&queue->pending);
179 	INIT_LIST_HEAD(&queue->free_pool);
180 	spin_lock_init(&queue->lock);
181 	timer_setup(&queue->gc, cw1200_queue_gc, 0);
183 	queue->pool = kcalloc(capacity, sizeof(struct cw1200_queue_item),
185 	if (!queue->pool)
188 	queue->link_map_cache = kcalloc(stats->map_capacity, sizeof(int),
190 	if (!queue->link_map_cache) {
191 		kfree(queue->pool);
192 		queue->pool = NULL;
197 		list_add_tail(&queue->pool[i].head, &queue->free_pool);
202 int cw1200_queue_clear(struct cw1200_queue *queue)
206 	struct cw1200_queue_stats *stats = queue->stats;
209 	spin_lock_bh(&queue->lock);
210 	queue->generation++;
211 	list_splice_tail_init(&queue->queue, &queue->pending);
212 	list_for_each_entry_safe(item, tmp, &queue->pending, head) {
216 		list_move_tail(&item->head, &queue->free_pool);
218 	queue->num_queued = 0;
219 	queue->num_pending = 0;
223 		stats->num_queued -= queue->link_map_cache[i];
224 		stats->link_map_cache[i] -= queue->link_map_cache[i];
225 		queue->link_map_cache[i] = 0;
228 	if (queue->overfull) {
229 		queue->overfull = false;
230 		__cw1200_queue_unlock(queue);
232 	spin_unlock_bh(&queue->lock);
244 void cw1200_queue_deinit(struct cw1200_queue *queue)
246 	cw1200_queue_clear(queue);
247 	del_timer_sync(&queue->gc);
248 	INIT_LIST_HEAD(&queue->free_pool);
249 	kfree(queue->pool);
250 	kfree(queue->link_map_cache);
251 	queue->pool = NULL;
252 	queue->link_map_cache = NULL;
253 	queue->capacity = 0;
256 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
261 	size_t map_capacity = queue->stats->map_capacity;
266 	spin_lock_bh(&queue->lock);
268 		ret = queue->num_queued - queue->num_pending;
273 				ret += queue->link_map_cache[i];
276 	spin_unlock_bh(&queue->lock);
280 int cw1200_queue_put(struct cw1200_queue *queue,
285 	struct cw1200_queue_stats *stats = queue->stats;
287 	if (txpriv->link_id >= queue->stats->map_capacity)
290 	spin_lock_bh(&queue->lock);
291 	if (!WARN_ON(list_empty(&queue->free_pool))) {
293 			&queue->free_pool, struct cw1200_queue_item, head);
296 		list_move_tail(&item->head, &queue->queue);
300 		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
301 							    queue->queue_id,
303 							    item - queue->pool);
306 		++queue->num_queued;
307 		++queue->link_map_cache[txpriv->link_id];
315 		 * Leave extra queue slots so we don't overflow.
317 		if (queue->overfull == false &&
318 		    queue->num_queued >=
319 		    (queue->capacity - (num_present_cpus() - 1))) {
320 			queue->overfull = true;
321 			__cw1200_queue_lock(queue);
322 			mod_timer(&queue->gc, jiffies);
327 	spin_unlock_bh(&queue->lock);
331 int cw1200_queue_get(struct cw1200_queue *queue,
339 	struct cw1200_queue_stats *stats = queue->stats;
342 	spin_lock_bh(&queue->lock);
343 	list_for_each_entry(item, &queue->queue, head) {
355 		list_move_tail(&item->head, &queue->pending);
356 		++queue->num_pending;
357 		--queue->link_map_cache[item->txpriv.link_id];
366 	spin_unlock_bh(&queue->lock);
372 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id)
377 	struct cw1200_queue_stats *stats = queue->stats;
382 	item = &queue->pool[item_id];
384 	spin_lock_bh(&queue->lock);
385 	BUG_ON(queue_id != queue->queue_id);
386 	if (queue_generation != queue->generation) {
388 	} else if (item_id >= (unsigned) queue->capacity) {
395 		--queue->num_pending;
396 		++queue->link_map_cache[item->txpriv.link_id];
408 		list_move(&item->head, &queue->queue);
410 	spin_unlock_bh(&queue->lock);
414 int cw1200_queue_requeue_all(struct cw1200_queue *queue)
417 	struct cw1200_queue_stats *stats = queue->stats;
418 	spin_lock_bh(&queue->lock);
420 	list_for_each_entry_safe_reverse(item, tmp, &queue->pending, head) {
421 		--queue->num_pending;
422 		++queue->link_map_cache[item->txpriv.link_id];
430 		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
431 							    queue->queue_id,
433 							    item - queue->pool);
434 		list_move(&item->head, &queue->queue);
436 	spin_unlock_bh(&queue->lock);
441 int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id)
446 	struct cw1200_queue_stats *stats = queue->stats;
453 	item = &queue->pool[item_id];
455 	spin_lock_bh(&queue->lock);
456 	BUG_ON(queue_id != queue->queue_id);
457 	if (queue_generation != queue->generation) {
459 	} else if (item_id >= (unsigned) queue->capacity) {
469 		--queue->num_pending;
470 		--queue->num_queued;
471 		++queue->num_sent;
476 		list_move(&item->head, &queue->free_pool);
478 		if (queue->overfull &&
479 		    (queue->num_queued <= (queue->capacity >> 1))) {
480 			queue->overfull = false;
481 			__cw1200_queue_unlock(queue);
484 	spin_unlock_bh(&queue->lock);
492 int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id,
502 	item = &queue->pool[item_id];
504 	spin_lock_bh(&queue->lock);
505 	BUG_ON(queue_id != queue->queue_id);
506 	if (queue_generation != queue->generation) {
508 	} else if (item_id >= (unsigned) queue->capacity) {
518 	spin_unlock_bh(&queue->lock);
522 void cw1200_queue_lock(struct cw1200_queue *queue)
524 	spin_lock_bh(&queue->lock);
525 	__cw1200_queue_lock(queue);
526 	spin_unlock_bh(&queue->lock);
529 void cw1200_queue_unlock(struct cw1200_queue *queue)
531 	spin_lock_bh(&queue->lock);
532 	__cw1200_queue_unlock(queue);
533 	spin_unlock_bh(&queue->lock);
536 bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
543 	spin_lock_bh(&queue->lock);
544 	ret = !list_empty(&queue->pending);
546 		list_for_each_entry(item, &queue->pending, head) {
553 	spin_unlock_bh(&queue->lock);