[linux-2.6-block.git] / drivers / net / wireless / st / cw1200 / queue.c

/*
 * O(1) TX queue with built-in allocator for ST-Ericsson CW1200 drivers
 *
 * Copyright (c) 2010, ST-Ericsson
 * Author: Dmitry Tarnyagin <dmitry.tarnyagin@lockless.no>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <net/mac80211.h>
#include <linux/sched.h>
#include "queue.h"
#include "cw1200.h"
#include "debug.h"

/* private */ struct cw1200_queue_item
{
	struct list_head	head;
	struct sk_buff		*skb;
	u32			packet_id;
	unsigned long		queue_timestamp;
	unsigned long		xmit_timestamp;
	struct cw1200_txpriv	txpriv;
	u8			generation;
};

static inline void __cw1200_queue_lock(struct cw1200_queue *queue)
{
	struct cw1200_queue_stats *stats = queue->stats;
	if (queue->tx_locked_cnt++ == 0) {
		pr_debug("[TX] Queue %d is locked.\n",
			 queue->queue_id);
		ieee80211_stop_queue(stats->priv->hw, queue->queue_id);
	}
}

static inline void __cw1200_queue_unlock(struct cw1200_queue *queue)
{
	struct cw1200_queue_stats *stats = queue->stats;
	BUG_ON(!queue->tx_locked_cnt);
	if (--queue->tx_locked_cnt == 0) {
		pr_debug("[TX] Queue %d is unlocked.\n",
			 queue->queue_id);
		ieee80211_wake_queue(stats->priv->hw, queue->queue_id);
	}
}

static inline void cw1200_queue_parse_id(u32 packet_id, u8 *queue_generation,
					 u8 *queue_id, u8 *item_generation,
					 u8 *item_id)
{
	*item_id		= (packet_id >>  0) & 0xFF;
	*item_generation	= (packet_id >>  8) & 0xFF;
	*queue_id		= (packet_id >> 16) & 0xFF;
	*queue_generation	= (packet_id >> 24) & 0xFF;
}

static inline u32 cw1200_queue_mk_packet_id(u8 queue_generation, u8 queue_id,
					    u8 item_generation, u8 item_id)
{
	return ((u32)item_id << 0) |
		((u32)item_generation << 8) |
		((u32)queue_id << 16) |
		((u32)queue_generation << 24);
}

static void cw1200_queue_post_gc(struct cw1200_queue_stats *stats,
				 struct list_head *gc_list)
{
	struct cw1200_queue_item *item, *tmp;

	list_for_each_entry_safe(item, tmp, gc_list, head) {
		list_del(&item->head);
		stats->skb_dtor(stats->priv, item->skb, &item->txpriv);
		kfree(item);
	}
}

static void cw1200_queue_register_post_gc(struct list_head *gc_list,
					  struct cw1200_queue_item *item)
{
	struct cw1200_queue_item *gc_item;
	gc_item = kmalloc(sizeof(struct cw1200_queue_item),
			GFP_ATOMIC);
	BUG_ON(!gc_item);
	memcpy(gc_item, item, sizeof(struct cw1200_queue_item));
	list_add_tail(&gc_item->head, gc_list);
}

static void __cw1200_queue_gc(struct cw1200_queue *queue,
			      struct list_head *head,
			      bool unlock)
{
	struct cw1200_queue_stats *stats = queue->stats;
	struct cw1200_queue_item *item = NULL, *tmp;
	bool wakeup_stats = false;

	list_for_each_entry_safe(item, tmp, &queue->queue, head) {
		if (jiffies - item->queue_timestamp < queue->ttl)
			break;
		--queue->num_queued;
		--queue->link_map_cache[item->txpriv.link_id];
		spin_lock_bh(&stats->lock);
		--stats->num_queued;
		if (!--stats->link_map_cache[item->txpriv.link_id])
			wakeup_stats = true;
		spin_unlock_bh(&stats->lock);
		cw1200_debug_tx_ttl(stats->priv);
		cw1200_queue_register_post_gc(head, item);
		item->skb = NULL;
		list_move_tail(&item->head, &queue->free_pool);
	}

	if (wakeup_stats)
		wake_up(&stats->wait_link_id_empty);

	if (queue->overfull) {
		if (queue->num_queued <= (queue->capacity >> 1)) {
			queue->overfull = false;
			if (unlock)
				__cw1200_queue_unlock(queue);
		} else if (item) {
			unsigned long tmo = item->queue_timestamp + queue->ttl;
			mod_timer(&queue->gc, tmo);
			cw1200_pm_stay_awake(&stats->priv->pm_state,
					     tmo - jiffies);
		}
	}
}

static void cw1200_queue_gc(struct timer_list *t)
{
	LIST_HEAD(list);
	struct cw1200_queue *queue =
		from_timer(queue, t, gc);

	spin_lock_bh(&queue->lock);
	__cw1200_queue_gc(queue, &list, true);
	spin_unlock_bh(&queue->lock);
	cw1200_queue_post_gc(queue->stats, &list);
}

int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
			    size_t map_capacity,
			    cw1200_queue_skb_dtor_t skb_dtor,
			    struct cw1200_common *priv)
{
	memset(stats, 0, sizeof(*stats));
	stats->map_capacity = map_capacity;
	stats->skb_dtor = skb_dtor;
	stats->priv = priv;
	spin_lock_init(&stats->lock);
	init_waitqueue_head(&stats->wait_link_id_empty);

	stats->link_map_cache = kcalloc(map_capacity, sizeof(int),
					GFP_KERNEL);
	if (!stats->link_map_cache)
		return -ENOMEM;

	return 0;
}

int cw1200_queue_init(struct cw1200_queue *queue,
		      struct cw1200_queue_stats *stats,
		      u8 queue_id,
		      size_t capacity,
		      unsigned long ttl)
{
	size_t i;

	memset(queue, 0, sizeof(*queue));
	queue->stats = stats;
	queue->capacity = capacity;
	queue->queue_id = queue_id;
	queue->ttl = ttl;
	INIT_LIST_HEAD(&queue->queue);
	INIT_LIST_HEAD(&queue->pending);
	INIT_LIST_HEAD(&queue->free_pool);
	spin_lock_init(&queue->lock);
	timer_setup(&queue->gc, cw1200_queue_gc, 0);

	queue->pool = kcalloc(capacity, sizeof(struct cw1200_queue_item),
			      GFP_KERNEL);
	if (!queue->pool)
		return -ENOMEM;

	queue->link_map_cache = kcalloc(stats->map_capacity, sizeof(int),
					GFP_KERNEL);
	if (!queue->link_map_cache) {
		kfree(queue->pool);
		queue->pool = NULL;
		return -ENOMEM;
	}

	for (i = 0; i < capacity; ++i)
		list_add_tail(&queue->pool[i].head, &queue->free_pool);

	return 0;
}

int cw1200_queue_clear(struct cw1200_queue *queue)
{
	int i;
	LIST_HEAD(gc_list);
	struct cw1200_queue_stats *stats = queue->stats;
	struct cw1200_queue_item *item, *tmp;

	spin_lock_bh(&queue->lock);
	queue->generation++;
	list_splice_tail_init(&queue->queue, &queue->pending);
	list_for_each_entry_safe(item, tmp, &queue->pending, head) {
		WARN_ON(!item->skb);
		cw1200_queue_register_post_gc(&gc_list, item);
		item->skb = NULL;
		list_move_tail(&item->head, &queue->free_pool);
	}
	queue->num_queued = 0;
	queue->num_pending = 0;

	spin_lock_bh(&stats->lock);
	for (i = 0; i < stats->map_capacity; ++i) {
		stats->num_queued -= queue->link_map_cache[i];
		stats->link_map_cache[i] -= queue->link_map_cache[i];
		queue->link_map_cache[i] = 0;
	}
	spin_unlock_bh(&stats->lock);
	if (queue->overfull) {
		queue->overfull = false;
		__cw1200_queue_unlock(queue);
	}
	spin_unlock_bh(&queue->lock);
	wake_up(&stats->wait_link_id_empty);
	cw1200_queue_post_gc(stats, &gc_list);
	return 0;
}

void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
{
	kfree(stats->link_map_cache);
	stats->link_map_cache = NULL;
}

void cw1200_queue_deinit(struct cw1200_queue *queue)
{
	cw1200_queue_clear(queue);
	del_timer_sync(&queue->gc);
	INIT_LIST_HEAD(&queue->free_pool);
	kfree(queue->pool);
	kfree(queue->link_map_cache);
	queue->pool = NULL;
	queue->link_map_cache = NULL;
	queue->capacity = 0;
}

size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
				   u32 link_id_map)
{
	size_t ret;
	int i, bit;
	size_t map_capacity = queue->stats->map_capacity;

	if (!link_id_map)
		return 0;

	spin_lock_bh(&queue->lock);
	if (link_id_map == (u32)-1) {
		ret = queue->num_queued - queue->num_pending;
	} else {
		ret = 0;
		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
			if (link_id_map & bit)
				ret += queue->link_map_cache[i];
		}
	}
	spin_unlock_bh(&queue->lock);
	return ret;
}

int cw1200_queue_put(struct cw1200_queue *queue,
		     struct sk_buff *skb,
		     struct cw1200_txpriv *txpriv)
{
	int ret = 0;
	LIST_HEAD(gc_list);
	struct cw1200_queue_stats *stats = queue->stats;

	if (txpriv->link_id >= queue->stats->map_capacity)
		return -EINVAL;

	spin_lock_bh(&queue->lock);
	if (!WARN_ON(list_empty(&queue->free_pool))) {
		struct cw1200_queue_item *item = list_first_entry(
			&queue->free_pool, struct cw1200_queue_item, head);
		BUG_ON(item->skb);

		list_move_tail(&item->head, &queue->queue);
		item->skb = skb;
		item->txpriv = *txpriv;
		item->generation = 0;
		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
							    queue->queue_id,
							    item->generation,
							    item - queue->pool);
		item->queue_timestamp = jiffies;

		++queue->num_queued;
		++queue->link_map_cache[txpriv->link_id];

		spin_lock_bh(&stats->lock);
		++stats->num_queued;
		++stats->link_map_cache[txpriv->link_id];
		spin_unlock_bh(&stats->lock);

		/* TX may happen in parallel sometimes.
		 * Leave extra queue slots so we don't overflow.
		 */
		if (queue->overfull == false &&
		    queue->num_queued >=
		    (queue->capacity - (num_present_cpus() - 1))) {
			queue->overfull = true;
			__cw1200_queue_lock(queue);
			mod_timer(&queue->gc, jiffies);
		}
	} else {
		ret = -ENOENT;
	}
	spin_unlock_bh(&queue->lock);
	return ret;
}

int 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)
{
	int ret = -ENOENT;
	struct cw1200_queue_item *item;
	struct cw1200_queue_stats *stats = queue->stats;
	bool wakeup_stats = false;

	spin_lock_bh(&queue->lock);
	list_for_each_entry(item, &queue->queue, head) {
		if (link_id_map & BIT(item->txpriv.link_id)) {
			ret = 0;
			break;
		}
	}

	if (!WARN_ON(ret)) {
		*tx = (struct wsm_tx *)item->skb->data;
		*tx_info = IEEE80211_SKB_CB(item->skb);
		*txpriv = &item->txpriv;
		(*tx)->packet_id = item->packet_id;
		list_move_tail(&item->head, &queue->pending);
		++queue->num_pending;
		--queue->link_map_cache[item->txpriv.link_id];
		item->xmit_timestamp = jiffies;

		spin_lock_bh(&stats->lock);
		--stats->num_queued;
		if (!--stats->link_map_cache[item->txpriv.link_id])
			wakeup_stats = true;
		spin_unlock_bh(&stats->lock);
	}
	spin_unlock_bh(&queue->lock);
	if (wakeup_stats)
		wake_up(&stats->wait_link_id_empty);
	return ret;
}

int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packet_id)
{
	int ret = 0;
	u8 queue_generation, queue_id, item_generation, item_id;
	struct cw1200_queue_item *item;
	struct cw1200_queue_stats *stats = queue->stats;

	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
			      &item_generation, &item_id);

	item = &queue->pool[item_id];

	spin_lock_bh(&queue->lock);
	BUG_ON(queue_id != queue->queue_id);
	if (queue_generation != queue->generation) {
		ret = -ENOENT;
	} else if (item_id >= (unsigned) queue->capacity) {
		WARN_ON(1);
		ret = -EINVAL;
	} else if (item->generation != item_generation) {
		WARN_ON(1);
		ret = -ENOENT;
	} else {
		--queue->num_pending;
		++queue->link_map_cache[item->txpriv.link_id];

		spin_lock_bh(&stats->lock);
		++stats->num_queued;
		++stats->link_map_cache[item->txpriv.link_id];
		spin_unlock_bh(&stats->lock);

		item->generation = ++item_generation;
		item->packet_id = cw1200_queue_mk_packet_id(queue_generation,
							    queue_id,
							    item_generation,
							    item_id);
		list_move(&item->head, &queue->queue);
	}
	spin_unlock_bh(&queue->lock);
	return ret;
}

int cw1200_queue_requeue_all(struct cw1200_queue *queue)
{
	struct cw1200_queue_item *item, *tmp;
	struct cw1200_queue_stats *stats = queue->stats;
	spin_lock_bh(&queue->lock);

	list_for_each_entry_safe_reverse(item, tmp, &queue->pending, head) {
		--queue->num_pending;
		++queue->link_map_cache[item->txpriv.link_id];

		spin_lock_bh(&stats->lock);
		++stats->num_queued;
		++stats->link_map_cache[item->txpriv.link_id];
		spin_unlock_bh(&stats->lock);

		++item->generation;
		item->packet_id = cw1200_queue_mk_packet_id(queue->generation,
							    queue->queue_id,
							    item->generation,
							    item - queue->pool);
		list_move(&item->head, &queue->queue);
	}
	spin_unlock_bh(&queue->lock);

	return 0;
}

int cw1200_queue_remove(struct cw1200_queue *queue, u32 packet_id)
{
	int ret = 0;
	u8 queue_generation, queue_id, item_generation, item_id;
	struct cw1200_queue_item *item;
	struct cw1200_queue_stats *stats = queue->stats;
	struct sk_buff *gc_skb = NULL;
	struct cw1200_txpriv gc_txpriv;

	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
			      &item_generation, &item_id);

	item = &queue->pool[item_id];

	spin_lock_bh(&queue->lock);
	BUG_ON(queue_id != queue->queue_id);
	if (queue_generation != queue->generation) {
		ret = -ENOENT;
	} else if (item_id >= (unsigned) queue->capacity) {
		WARN_ON(1);
		ret = -EINVAL;
	} else if (item->generation != item_generation) {
		WARN_ON(1);
		ret = -ENOENT;
	} else {
		gc_txpriv = item->txpriv;
		gc_skb = item->skb;
		item->skb = NULL;
		--queue->num_pending;
		--queue->num_queued;
		++queue->num_sent;
		++item->generation;
		/* Do not use list_move_tail here, but list_move:
		 * try to utilize cache row.
		 */
		list_move(&item->head, &queue->free_pool);

		if (queue->overfull &&
		    (queue->num_queued <= (queue->capacity >> 1))) {
			queue->overfull = false;
			__cw1200_queue_unlock(queue);
		}
	}
	spin_unlock_bh(&queue->lock);

	if (gc_skb)
		stats->skb_dtor(stats->priv, gc_skb, &gc_txpriv);

	return ret;
}

int cw1200_queue_get_skb(struct cw1200_queue *queue, u32 packet_id,
			 struct sk_buff **skb,
			 const struct cw1200_txpriv **txpriv)
{
	int ret = 0;
	u8 queue_generation, queue_id, item_generation, item_id;
	struct cw1200_queue_item *item;
	cw1200_queue_parse_id(packet_id, &queue_generation, &queue_id,
			      &item_generation, &item_id);

	item = &queue->pool[item_id];

	spin_lock_bh(&queue->lock);
	BUG_ON(queue_id != queue->queue_id);
	if (queue_generation != queue->generation) {
		ret = -ENOENT;
	} else if (item_id >= (unsigned) queue->capacity) {
		WARN_ON(1);
		ret = -EINVAL;
	} else if (item->generation != item_generation) {
		WARN_ON(1);
		ret = -ENOENT;
	} else {
		*skb = item->skb;
		*txpriv = &item->txpriv;
	}
	spin_unlock_bh(&queue->lock);
	return ret;
}

void cw1200_queue_lock(struct cw1200_queue *queue)
{
	spin_lock_bh(&queue->lock);
	__cw1200_queue_lock(queue);
	spin_unlock_bh(&queue->lock);
}

void cw1200_queue_unlock(struct cw1200_queue *queue)
{
	spin_lock_bh(&queue->lock);
	__cw1200_queue_unlock(queue);
	spin_unlock_bh(&queue->lock);
}

bool cw1200_queue_get_xmit_timestamp(struct cw1200_queue *queue,
				     unsigned long *timestamp,
				     u32 pending_frame_id)
{
	struct cw1200_queue_item *item;
	bool ret;

	spin_lock_bh(&queue->lock);
	ret = !list_empty(&queue->pending);
	if (ret) {
		list_for_each_entry(item, &queue->pending, head) {
			if (item->packet_id != pending_frame_id)
				if (time_before(item->xmit_timestamp,
						*timestamp))
					*timestamp = item->xmit_timestamp;
		}
	}
	spin_unlock_bh(&queue->lock);
	return ret;
}

bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
				 u32 link_id_map)
{
	bool empty = true;

	spin_lock_bh(&stats->lock);
	if (link_id_map == (u32)-1) {
		empty = stats->num_queued == 0;
	} else {
		int i;
		for (i = 0; i < stats->map_capacity; ++i) {
			if (link_id_map & BIT(i)) {
				if (stats->link_map_cache[i]) {
					empty = false;
					break;
				}
			}
		}
	}
	spin_unlock_bh(&stats->lock);

	return empty;
}
Commit	Line	Data
a910e4a9 SP	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
e3dcf8bb	133	static void cw1200_queue_gc(struct timer_list *t)
a910e4a9 SP	134	{
	135	LIST_HEAD(list);
	136	struct cw1200_queue *queue =
e3dcf8bb	137	from_timer(queue, t, gc);
a910e4a9 SP	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
6396bb22	157	stats->link_map_cache = kcalloc(map_capacity, sizeof(int),
a910e4a9 SP	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);
e3dcf8bb	182	timer_setup(&queue->gc, cw1200_queue_gc, 0);
a910e4a9	183
6396bb22 KC	184	queue->pool = kcalloc(capacity, sizeof(struct cw1200_queue_item),
6396bb22 KC	185	GFP_KERNEL);
a910e4a9 SP	186	if (!queue->pool)
	187	return -ENOMEM;
	188
6396bb22 KC	189	queue->link_map_cache = kcalloc(stats->map_capacity, sizeof(int),
6396bb22 KC	190	GFP_KERNEL);
a910e4a9 SP	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;
7258416c	356	(*tx)->packet_id = item->packet_id;
a910e4a9 SP	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	}