[net-next-2.6.git] / drivers / net / wireless / rt2x00 / rt2x00queue.c

/*
	Copyright (C) 2004 - 2008 rt2x00 SourceForge Project
	<http://rt2x00.serialmonkey.com>

	This program is free software; you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation; either version 2 of the License, or
	(at your option) any later version.

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program; if not, write to the
	Free Software Foundation, Inc.,
	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

/*
	Module: rt2x00lib
	Abstract: rt2x00 queue specific routines.
 */

#include <linux/kernel.h>
#include <linux/module.h>

#include "rt2x00.h"
#include "rt2x00lib.h"

void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
				      struct txentry_desc *txdesc,
				      struct ieee80211_tx_control *control)
{
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
	struct ieee80211_rate *rate =
	    ieee80211_get_tx_rate(rt2x00dev->hw, control);
	const struct rt2x00_rate *hwrate;
	unsigned int data_length;
	unsigned int duration;
	unsigned int residual;
	u16 frame_control;

	memset(txdesc, 0, sizeof(*txdesc));

	/*
	 * Initialize information from queue
	 */
	txdesc->queue = entry->queue->qid;
	txdesc->cw_min = entry->queue->cw_min;
	txdesc->cw_max = entry->queue->cw_max;
	txdesc->aifs = entry->queue->aifs;

	/* Data length should be extended with 4 bytes for CRC */
	data_length = entry->skb->len + 4;

	/*
	 * Read required fields from ieee80211 header.
	 */
	frame_control = le16_to_cpu(hdr->frame_control);

	/*
	 * Check whether this frame is to be acked.
	 */
	if (!(control->flags & IEEE80211_TXCTL_NO_ACK))
		__set_bit(ENTRY_TXD_ACK, &txdesc->flags);

	/*
	 * Check if this is a RTS/CTS frame
	 */
	if (is_rts_frame(frame_control) || is_cts_frame(frame_control)) {
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
		if (is_rts_frame(frame_control)) {
			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
			__set_bit(ENTRY_TXD_ACK, &txdesc->flags);
		} else {
			__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
			__clear_bit(ENTRY_TXD_ACK, &txdesc->flags);
		}
		if (control->rts_cts_rate_idx >= 0)
			rate =
			    ieee80211_get_rts_cts_rate(rt2x00dev->hw, control);
	}

	/*
	 * Determine retry information.
	 */
	txdesc->retry_limit = control->retry_limit;
	if (control->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
		__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);

	/*
	 * Check if more fragments are pending
	 */
	if (ieee80211_get_morefrag(hdr)) {
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
		__set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
	}

	/*
	 * Beacons and probe responses require the tsf timestamp
	 * to be inserted into the frame.
	 */
	if (txdesc->queue == QID_BEACON || is_probe_resp(frame_control))
		__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);

	/*
	 * Determine with what IFS priority this frame should be send.
	 * Set ifs to IFS_SIFS when the this is not the first fragment,
	 * or this fragment came after RTS/CTS.
	 */
	if (test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
		txdesc->ifs = IFS_SIFS;
	} else if (control->flags & IEEE80211_TXCTL_FIRST_FRAGMENT) {
		__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
		txdesc->ifs = IFS_BACKOFF;
	} else {
		txdesc->ifs = IFS_SIFS;
	}

	/*
	 * PLCP setup
	 * Length calculation depends on OFDM/CCK rate.
	 */
	hwrate = rt2x00_get_rate(rate->hw_value);
	txdesc->signal = hwrate->plcp;
	txdesc->service = 0x04;

	if (hwrate->flags & DEV_RATE_OFDM) {
		__set_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags);

		txdesc->length_high = (data_length >> 6) & 0x3f;
		txdesc->length_low = data_length & 0x3f;
	} else {
		/*
		 * Convert length to microseconds.
		 */
		residual = get_duration_res(data_length, hwrate->bitrate);
		duration = get_duration(data_length, hwrate->bitrate);

		if (residual != 0) {
			duration++;

			/*
			 * Check if we need to set the Length Extension
			 */
			if (hwrate->bitrate == 110 && residual <= 30)
				txdesc->service |= 0x80;
		}

		txdesc->length_high = (duration >> 8) & 0xff;
		txdesc->length_low = duration & 0xff;

		/*
		 * When preamble is enabled we should set the
		 * preamble bit for the signal.
		 */
		if (rt2x00_get_rate_preamble(rate->hw_value))
			txdesc->signal |= 0x08;
	}
}
EXPORT_SYMBOL_GPL(rt2x00queue_create_tx_descriptor);

void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
				     struct txentry_desc *txdesc)
{
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);

	rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, entry->skb, txdesc);

	/*
	 * All processing on the frame has been completed, this means
	 * it is now ready to be dumped to userspace through debugfs.
	 */
	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TX, entry->skb);

	/*
	 * We are done writing the frame to the queue entry,
	 * if this entry is a RTS of CTS-to-self frame we are done,
	 * otherwise we need to kick the queue.
	 */
	if (rt2x00dev->ops->lib->kick_tx_queue &&
	    !(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
		rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev,
						   entry->queue->qid);
}
EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);

struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
					 const enum data_queue_qid queue)
{
	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);

	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
		return &rt2x00dev->tx[queue];

	if (!rt2x00dev->bcn)
		return NULL;

	if (queue == QID_BEACON)
		return &rt2x00dev->bcn[0];
	else if (queue == QID_ATIM && atim)
		return &rt2x00dev->bcn[1];

	return NULL;
}
EXPORT_SYMBOL_GPL(rt2x00queue_get_queue);

struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
					  enum queue_index index)
{
	struct queue_entry *entry;
	unsigned long irqflags;

	if (unlikely(index >= Q_INDEX_MAX)) {
		ERROR(queue->rt2x00dev,
		      "Entry requested from invalid index type (%d)\n", index);
		return NULL;
	}

	spin_lock_irqsave(&queue->lock, irqflags);

	entry = &queue->entries[queue->index[index]];

	spin_unlock_irqrestore(&queue->lock, irqflags);

	return entry;
}
EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);

void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
{
	unsigned long irqflags;

	if (unlikely(index >= Q_INDEX_MAX)) {
		ERROR(queue->rt2x00dev,
		      "Index change on invalid index type (%d)\n", index);
		return;
	}

	spin_lock_irqsave(&queue->lock, irqflags);

	queue->index[index]++;
	if (queue->index[index] >= queue->limit)
		queue->index[index] = 0;

	if (index == Q_INDEX) {
		queue->length++;
	} else if (index == Q_INDEX_DONE) {
		queue->length--;
		queue->count ++;
	}

	spin_unlock_irqrestore(&queue->lock, irqflags);
}
EXPORT_SYMBOL_GPL(rt2x00queue_index_inc);

static void rt2x00queue_reset(struct data_queue *queue)
{
	unsigned long irqflags;

	spin_lock_irqsave(&queue->lock, irqflags);

	queue->count = 0;
	queue->length = 0;
	memset(queue->index, 0, sizeof(queue->index));

	spin_unlock_irqrestore(&queue->lock, irqflags);
}

void rt2x00queue_init_rx(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue = rt2x00dev->rx;
	unsigned int i;

	rt2x00queue_reset(queue);

	if (!rt2x00dev->ops->lib->init_rxentry)
		return;

	for (i = 0; i < queue->limit; i++)
		rt2x00dev->ops->lib->init_rxentry(rt2x00dev,
						  &queue->entries[i]);
}

void rt2x00queue_init_tx(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue;
	unsigned int i;

	txall_queue_for_each(rt2x00dev, queue) {
		rt2x00queue_reset(queue);

		if (!rt2x00dev->ops->lib->init_txentry)
			continue;

		for (i = 0; i < queue->limit; i++)
			rt2x00dev->ops->lib->init_txentry(rt2x00dev,
							  &queue->entries[i]);
	}
}

static int rt2x00queue_alloc_entries(struct data_queue *queue,
				     const struct data_queue_desc *qdesc)
{
	struct queue_entry *entries;
	unsigned int entry_size;
	unsigned int i;

	rt2x00queue_reset(queue);

	queue->limit = qdesc->entry_num;
	queue->data_size = qdesc->data_size;
	queue->desc_size = qdesc->desc_size;

	/*
	 * Allocate all queue entries.
	 */
	entry_size = sizeof(*entries) + qdesc->priv_size;
	entries = kzalloc(queue->limit * entry_size, GFP_KERNEL);
	if (!entries)
		return -ENOMEM;

#define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
	( ((char *)(__base)) + ((__limit) * (__esize)) + \
	    ((__index) * (__psize)) )

	for (i = 0; i < queue->limit; i++) {
		entries[i].flags = 0;
		entries[i].queue = queue;
		entries[i].skb = NULL;
		entries[i].entry_idx = i;
		entries[i].priv_data =
		    QUEUE_ENTRY_PRIV_OFFSET(entries, i, queue->limit,
					    sizeof(*entries), qdesc->priv_size);
	}

#undef QUEUE_ENTRY_PRIV_OFFSET

	queue->entries = entries;

	return 0;
}

int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue;
	int status;


	status = rt2x00queue_alloc_entries(rt2x00dev->rx, rt2x00dev->ops->rx);
	if (status)
		goto exit;

	tx_queue_for_each(rt2x00dev, queue) {
		status = rt2x00queue_alloc_entries(queue, rt2x00dev->ops->tx);
		if (status)
			goto exit;
	}

	status = rt2x00queue_alloc_entries(rt2x00dev->bcn, rt2x00dev->ops->bcn);
	if (status)
		goto exit;

	if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
		return 0;

	status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
					   rt2x00dev->ops->atim);
	if (status)
		goto exit;

	return 0;

exit:
	ERROR(rt2x00dev, "Queue entries allocation failed.\n");

	rt2x00queue_uninitialize(rt2x00dev);

	return status;
}

void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue;

	queue_for_each(rt2x00dev, queue) {
		kfree(queue->entries);
		queue->entries = NULL;
	}
}

static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
			     struct data_queue *queue, enum data_queue_qid qid)
{
	spin_lock_init(&queue->lock);

	queue->rt2x00dev = rt2x00dev;
	queue->qid = qid;
	queue->aifs = 2;
	queue->cw_min = 5;
	queue->cw_max = 10;
}

int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue;
	enum data_queue_qid qid;
	unsigned int req_atim =
	    !!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);

	/*
	 * We need the following queues:
	 * RX: 1
	 * TX: ops->tx_queues
	 * Beacon: 1
	 * Atim: 1 (if required)
	 */
	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;

	queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
	if (!queue) {
		ERROR(rt2x00dev, "Queue allocation failed.\n");
		return -ENOMEM;
	}

	/*
	 * Initialize pointers
	 */
	rt2x00dev->rx = queue;
	rt2x00dev->tx = &queue[1];
	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];

	/*
	 * Initialize queue parameters.
	 * RX: qid = QID_RX
	 * TX: qid = QID_AC_BE + index
	 * TX: cw_min: 2^5 = 32.
	 * TX: cw_max: 2^10 = 1024.
	 * BCN & Atim: qid = QID_MGMT
	 */
	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);

	qid = QID_AC_BE;
	tx_queue_for_each(rt2x00dev, queue)
		rt2x00queue_init(rt2x00dev, queue, qid++);

	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_MGMT);
	if (req_atim)
		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_MGMT);

	return 0;
}

void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
{
	kfree(rt2x00dev->rx);
	rt2x00dev->rx = NULL;
	rt2x00dev->tx = NULL;
	rt2x00dev->bcn = NULL;
}
Commit	Line	Data
181d6902 ID	1	/*
	2	Copyright (C) 2004 - 2008 rt2x00 SourceForge Project
	3	<http://rt2x00.serialmonkey.com>
	4
	5	This program is free software; you can redistribute it and/or modify
	6	it under the terms of the GNU General Public License as published by
	7	the Free Software Foundation; either version 2 of the License, or
	8	(at your option) any later version.
	9
	10	This program is distributed in the hope that it will be useful,
	11	but WITHOUT ANY WARRANTY; without even the implied warranty of
	12	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	13	GNU General Public License for more details.
	14
	15	You should have received a copy of the GNU General Public License
	16	along with this program; if not, write to the
	17	Free Software Foundation, Inc.,
	18	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
	19	*/
	20
	21	/*
	22	Module: rt2x00lib
	23	Abstract: rt2x00 queue specific routines.
	24	*/
	25
	26	#include <linux/kernel.h>
	27	#include <linux/module.h>
	28
	29	#include "rt2x00.h"
	30	#include "rt2x00lib.h"
	31
7050ec82 ID	32	void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
	33	struct txentry_desc *txdesc,
	34	struct ieee80211_tx_control *control)
	35	{
2e92e6f2	36	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
7050ec82	37	struct ieee80211_hdr hdr = (struct ieee80211_hdr )entry->skb->data;
2e92e6f2 JB	38	struct ieee80211_rate *rate =
2e92e6f2 JB	39	ieee80211_get_tx_rate(rt2x00dev->hw, control);
7050ec82 ID	40	const struct rt2x00_rate *hwrate;
	41	unsigned int data_length;
	42	unsigned int duration;
	43	unsigned int residual;
	44	u16 frame_control;
	45
	46	memset(txdesc, 0, sizeof(*txdesc));
	47
	48	/*
	49	* Initialize information from queue
	50	*/
	51	txdesc->queue = entry->queue->qid;
	52	txdesc->cw_min = entry->queue->cw_min;
	53	txdesc->cw_max = entry->queue->cw_max;
	54	txdesc->aifs = entry->queue->aifs;
	55
	56	/* Data length should be extended with 4 bytes for CRC */
	57	data_length = entry->skb->len + 4;
	58
	59	/*
	60	* Read required fields from ieee80211 header.
	61	*/
	62	frame_control = le16_to_cpu(hdr->frame_control);
	63
	64	/*
	65	* Check whether this frame is to be acked.
	66	*/
	67	if (!(control->flags & IEEE80211_TXCTL_NO_ACK))
	68	__set_bit(ENTRY_TXD_ACK, &txdesc->flags);
	69
	70	/*
	71	* Check if this is a RTS/CTS frame
	72	*/
	73	if (is_rts_frame(frame_control) \|\| is_cts_frame(frame_control)) {
	74	__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
	75	if (is_rts_frame(frame_control)) {
	76	__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
	77	__set_bit(ENTRY_TXD_ACK, &txdesc->flags);
	78	} else {
	79	__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
	80	__clear_bit(ENTRY_TXD_ACK, &txdesc->flags);
	81	}
2e92e6f2 JB	82	if (control->rts_cts_rate_idx >= 0)
	83	rate =
	84	ieee80211_get_rts_cts_rate(rt2x00dev->hw, control);
7050ec82 ID	85	}
	86
	87	/*
	88	* Determine retry information.
	89	*/
	90	txdesc->retry_limit = control->retry_limit;
	91	if (control->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
	92	__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
	93
	94	/*
	95	* Check if more fragments are pending
	96	*/
	97	if (ieee80211_get_morefrag(hdr)) {
	98	__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
	99	__set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
	100	}
	101
	102	/*
	103	* Beacons and probe responses require the tsf timestamp
	104	* to be inserted into the frame.
	105	*/
	106	if (txdesc->queue == QID_BEACON \|\| is_probe_resp(frame_control))
	107	__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);
	108
	109	/*
	110	* Determine with what IFS priority this frame should be send.
	111	* Set ifs to IFS_SIFS when the this is not the first fragment,
	112	* or this fragment came after RTS/CTS.
	113	*/
	114	if (test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
	115	txdesc->ifs = IFS_SIFS;
	116	} else if (control->flags & IEEE80211_TXCTL_FIRST_FRAGMENT) {
	117	__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
	118	txdesc->ifs = IFS_BACKOFF;
	119	} else {
	120	txdesc->ifs = IFS_SIFS;
	121	}
	122
	123	/*
	124	* PLCP setup
	125	* Length calculation depends on OFDM/CCK rate.
	126	*/
	127	hwrate = rt2x00_get_rate(rate->hw_value);
	128	txdesc->signal = hwrate->plcp;
	129	txdesc->service = 0x04;
	130
	131	if (hwrate->flags & DEV_RATE_OFDM) {
	132	__set_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags);
	133
	134	txdesc->length_high = (data_length >> 6) & 0x3f;
	135	txdesc->length_low = data_length & 0x3f;
	136	} else {
	137	/*
	138	* Convert length to microseconds.
	139	*/
	140	residual = get_duration_res(data_length, hwrate->bitrate);
	141	duration = get_duration(data_length, hwrate->bitrate);
	142
	143	if (residual != 0) {
	144	duration++;
	145
	146	/*
	147	* Check if we need to set the Length Extension
	148	*/
149	if (hwrate->bitrate == 110 && residual <= 30)
150	txdesc->service \|= 0x80;
151	}
152
153	txdesc->length_high = (duration >> 8) & 0xff;
154	txdesc->length_low = duration & 0xff;
155
156	/*
157	* When preamble is enabled we should set the
158	* preamble bit for the signal.
159	*/
160	if (rt2x00_get_rate_preamble(rate->hw_value))
161	txdesc->signal \|= 0x08;
162	}
163	}
164	EXPORT_SYMBOL_GPL(rt2x00queue_create_tx_descriptor);
165
166	void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
167	struct txentry_desc *txdesc)
168	{
169	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
170	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
171
172	rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, entry->skb, txdesc);
173
174	/*
175	* All processing on the frame has been completed, this means
176	* it is now ready to be dumped to userspace through debugfs.
177	*/
178	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TX, entry->skb);
179
180	/*
181	* We are done writing the frame to the queue entry,
182	* if this entry is a RTS of CTS-to-self frame we are done,
183	* otherwise we need to kick the queue.
184	*/
185	if (rt2x00dev->ops->lib->kick_tx_queue &&
186	!(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
187	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev,
188	entry->queue->qid);
189	}
190	EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);
191
181d6902	192	struct data_queue rt2x00queue_get_queue(struct rt2x00_dev rt2x00dev,
e58c6aca	193	const enum data_queue_qid queue)
181d6902 ID	194	{
	195	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
	196
61448f88	197	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
181d6902 ID	198	return &rt2x00dev->tx[queue];
	199
	200	if (!rt2x00dev->bcn)
	201	return NULL;
	202
e58c6aca	203	if (queue == QID_BEACON)
181d6902	204	return &rt2x00dev->bcn[0];
e58c6aca	205	else if (queue == QID_ATIM && atim)
181d6902 ID	206	return &rt2x00dev->bcn[1];
	207
	208	return NULL;
	209	}
	210	EXPORT_SYMBOL_GPL(rt2x00queue_get_queue);
	211
	212	struct queue_entry rt2x00queue_get_entry(struct data_queue queue,
	213	enum queue_index index)
	214	{
	215	struct queue_entry *entry;
5f46c4d0	216	unsigned long irqflags;
181d6902 ID	217
	218	if (unlikely(index >= Q_INDEX_MAX)) {
	219	ERROR(queue->rt2x00dev,
	220	"Entry requested from invalid index type (%d)\n", index);
	221	return NULL;
	222	}
	223
5f46c4d0	224	spin_lock_irqsave(&queue->lock, irqflags);
181d6902 ID	225
	226	entry = &queue->entries[queue->index[index]];
	227
5f46c4d0	228	spin_unlock_irqrestore(&queue->lock, irqflags);
181d6902 ID	229
	230	return entry;
	231	}
	232	EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);
	233
	234	void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
	235	{
5f46c4d0 ID	236	unsigned long irqflags;
5f46c4d0 ID	237
181d6902 ID	238	if (unlikely(index >= Q_INDEX_MAX)) {
	239	ERROR(queue->rt2x00dev,
	240	"Index change on invalid index type (%d)\n", index);
	241	return;
	242	}
	243
5f46c4d0	244	spin_lock_irqsave(&queue->lock, irqflags);
181d6902 ID	245
	246	queue->index[index]++;
	247	if (queue->index[index] >= queue->limit)
	248	queue->index[index] = 0;
	249
10b6b801 ID	250	if (index == Q_INDEX) {
	251	queue->length++;
	252	} else if (index == Q_INDEX_DONE) {
	253	queue->length--;
	254	queue->count ++;
	255	}
181d6902	256
5f46c4d0	257	spin_unlock_irqrestore(&queue->lock, irqflags);
181d6902 ID	258	}
	259	EXPORT_SYMBOL_GPL(rt2x00queue_index_inc);
	260
	261	static void rt2x00queue_reset(struct data_queue *queue)
	262	{
5f46c4d0 ID	263	unsigned long irqflags;
	264
	265	spin_lock_irqsave(&queue->lock, irqflags);
181d6902 ID	266
	267	queue->count = 0;
	268	queue->length = 0;
	269	memset(queue->index, 0, sizeof(queue->index));
	270
5f46c4d0	271	spin_unlock_irqrestore(&queue->lock, irqflags);
181d6902 ID	272	}
	273
	274	void rt2x00queue_init_rx(struct rt2x00_dev *rt2x00dev)
	275	{
	276	struct data_queue *queue = rt2x00dev->rx;
	277	unsigned int i;
	278
	279	rt2x00queue_reset(queue);
	280
	281	if (!rt2x00dev->ops->lib->init_rxentry)
	282	return;
	283
	284	for (i = 0; i < queue->limit; i++)
	285	rt2x00dev->ops->lib->init_rxentry(rt2x00dev,
	286	&queue->entries[i]);
	287	}
	288
	289	void rt2x00queue_init_tx(struct rt2x00_dev *rt2x00dev)
	290	{
	291	struct data_queue *queue;
	292	unsigned int i;
	293
	294	txall_queue_for_each(rt2x00dev, queue) {
	295	rt2x00queue_reset(queue);
	296
	297	if (!rt2x00dev->ops->lib->init_txentry)
	298	continue;
	299
	300	for (i = 0; i < queue->limit; i++)
	301	rt2x00dev->ops->lib->init_txentry(rt2x00dev,
	302	&queue->entries[i]);
	303	}
	304	}
	305
	306	static int rt2x00queue_alloc_entries(struct data_queue *queue,
	307	const struct data_queue_desc *qdesc)
	308	{
	309	struct queue_entry *entries;
	310	unsigned int entry_size;
	311	unsigned int i;
	312
	313	rt2x00queue_reset(queue);
	314
	315	queue->limit = qdesc->entry_num;
	316	queue->data_size = qdesc->data_size;
	317	queue->desc_size = qdesc->desc_size;
	318
	319	/*
	320	* Allocate all queue entries.
	321	*/
	322	entry_size = sizeof(*entries) + qdesc->priv_size;
	323	entries = kzalloc(queue->limit * entry_size, GFP_KERNEL);
	324	if (!entries)
	325	return -ENOMEM;
	326
	327	#define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
231be4e9 AB	328	( ((char )(__base)) + ((__limit) (__esize)) + \
231be4e9 AB	329	((__index) * (__psize)) )
181d6902 ID	330
	331	for (i = 0; i < queue->limit; i++) {
	332	entries[i].flags = 0;
	333	entries[i].queue = queue;
	334	entries[i].skb = NULL;
	335	entries[i].entry_idx = i;
	336	entries[i].priv_data =
	337	QUEUE_ENTRY_PRIV_OFFSET(entries, i, queue->limit,
	338	sizeof(*entries), qdesc->priv_size);
	339	}
	340
	341	#undef QUEUE_ENTRY_PRIV_OFFSET
	342
	343	queue->entries = entries;
	344
	345	return 0;
	346	}
	347
	348	int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
	349	{
	350	struct data_queue *queue;
	351	int status;
	352
	353
	354	status = rt2x00queue_alloc_entries(rt2x00dev->rx, rt2x00dev->ops->rx);
	355	if (status)
	356	goto exit;
	357
	358	tx_queue_for_each(rt2x00dev, queue) {
	359	status = rt2x00queue_alloc_entries(queue, rt2x00dev->ops->tx);
	360	if (status)
	361	goto exit;
	362	}
	363
	364	status = rt2x00queue_alloc_entries(rt2x00dev->bcn, rt2x00dev->ops->bcn);
	365	if (status)
	366	goto exit;
	367
	368	if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
	369	return 0;
	370
	371	status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
	372	rt2x00dev->ops->atim);
	373	if (status)
	374	goto exit;
	375
	376	return 0;
	377
	378	exit:
	379	ERROR(rt2x00dev, "Queue entries allocation failed.\n");
	380
	381	rt2x00queue_uninitialize(rt2x00dev);
	382
	383	return status;
	384	}
	385
	386	void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
	387	{
	388	struct data_queue *queue;
	389
	390	queue_for_each(rt2x00dev, queue) {
	391	kfree(queue->entries);
	392	queue->entries = NULL;
	393	}
394	}
395
8f539276 ID	396	static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
	397	struct data_queue *queue, enum data_queue_qid qid)
	398	{
	399	spin_lock_init(&queue->lock);
	400
	401	queue->rt2x00dev = rt2x00dev;
	402	queue->qid = qid;
	403	queue->aifs = 2;
	404	queue->cw_min = 5;
	405	queue->cw_max = 10;
	406	}
	407
181d6902 ID	408	int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
	409	{
	410	struct data_queue *queue;
	411	enum data_queue_qid qid;
	412	unsigned int req_atim =
	413	!!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
	414
	415	/*
	416	* We need the following queues:
	417	* RX: 1
61448f88	418	* TX: ops->tx_queues
181d6902 ID	419	* Beacon: 1
	420	* Atim: 1 (if required)
	421	*/
61448f88	422	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
181d6902 ID	423
	424	queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
	425	if (!queue) {
	426	ERROR(rt2x00dev, "Queue allocation failed.\n");
	427	return -ENOMEM;
	428	}
	429
	430	/*
	431	* Initialize pointers
	432	*/
	433	rt2x00dev->rx = queue;
	434	rt2x00dev->tx = &queue[1];
61448f88	435	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
181d6902 ID	436
	437	/*
	438	* Initialize queue parameters.
	439	* RX: qid = QID_RX
	440	* TX: qid = QID_AC_BE + index
	441	* TX: cw_min: 2^5 = 32.
	442	* TX: cw_max: 2^10 = 1024.
	443	* BCN & Atim: qid = QID_MGMT
	444	*/
8f539276	445	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
181d6902	446
8f539276 ID	447	qid = QID_AC_BE;
	448	tx_queue_for_each(rt2x00dev, queue)
	449	rt2x00queue_init(rt2x00dev, queue, qid++);
181d6902	450
8f539276	451	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_MGMT);
181d6902	452	if (req_atim)
8f539276	453	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_MGMT);
181d6902 ID	454
	455	return 0;
	456	}
	457
	458	void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
	459	{
	460	kfree(rt2x00dev->rx);
	461	rt2x00dev->rx = NULL;
	462	rt2x00dev->tx = NULL;
	463	rt2x00dev->bcn = NULL;
	464	}