rt2x00queue.c 20.5 KB
Newer Older
I
Ivo van Doorn 已提交
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
/*
	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>
28
#include <linux/dma-mapping.h>
I
Ivo van Doorn 已提交
29 30 31 32

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

33 34
struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
					struct queue_entry *entry)
35
{
36 37
	struct sk_buff *skb;
	struct skb_frame_desc *skbdesc;
I
Ivo van Doorn 已提交
38 39 40
	unsigned int frame_size;
	unsigned int head_size = 0;
	unsigned int tail_size = 0;
41 42 43 44 45

	/*
	 * The frame size includes descriptor size, because the
	 * hardware directly receive the frame into the skbuffer.
	 */
46
	frame_size = entry->queue->data_size + entry->queue->desc_size;
47 48

	/*
49 50 51
	 * The payload should be aligned to a 4-byte boundary,
	 * this means we need at least 3 bytes for moving the frame
	 * into the correct offset.
52
	 */
I
Ivo van Doorn 已提交
53 54 55 56 57 58 59 60 61 62 63 64 65
	head_size = 4;

	/*
	 * For IV/EIV/ICV assembly we must make sure there is
	 * at least 8 bytes bytes available in headroom for IV/EIV
	 * and 4 bytes for ICV data as tailroon.
	 */
#ifdef CONFIG_RT2X00_LIB_CRYPTO
	if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags)) {
		head_size += 8;
		tail_size += 4;
	}
#endif /* CONFIG_RT2X00_LIB_CRYPTO */
66 67 68 69

	/*
	 * Allocate skbuffer.
	 */
I
Ivo van Doorn 已提交
70
	skb = dev_alloc_skb(frame_size + head_size + tail_size);
71 72 73
	if (!skb)
		return NULL;

I
Ivo van Doorn 已提交
74 75 76 77 78
	/*
	 * Make sure we not have a frame with the requested bytes
	 * available in the head and tail.
	 */
	skb_reserve(skb, head_size);
79 80
	skb_put(skb, frame_size);

81 82 83 84 85 86 87 88 89 90 91 92 93 94 95
	/*
	 * Populate skbdesc.
	 */
	skbdesc = get_skb_frame_desc(skb);
	memset(skbdesc, 0, sizeof(*skbdesc));
	skbdesc->entry = entry;

	if (test_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags)) {
		skbdesc->skb_dma = dma_map_single(rt2x00dev->dev,
						  skb->data,
						  skb->len,
						  DMA_FROM_DEVICE);
		skbdesc->flags |= SKBDESC_DMA_MAPPED_RX;
	}

96 97
	return skb;
}
98

99
void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
100
{
101 102
	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);

103 104 105 106 107 108 109 110 111 112 113 114 115 116 117
	/*
	 * If device has requested headroom, we should make sure that
	 * is also mapped to the DMA so it can be used for transfering
	 * additional descriptor information to the hardware.
	 */
	skb_push(skb, rt2x00dev->hw->extra_tx_headroom);

	skbdesc->skb_dma =
	    dma_map_single(rt2x00dev->dev, skb->data, skb->len, DMA_TO_DEVICE);

	/*
	 * Restore data pointer to original location again.
	 */
	skb_pull(skb, rt2x00dev->hw->extra_tx_headroom);

118 119 120 121 122 123 124 125 126 127 128 129 130 131 132
	skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
}
EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);

void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
{
	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);

	if (skbdesc->flags & SKBDESC_DMA_MAPPED_RX) {
		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
				 DMA_FROM_DEVICE);
		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_RX;
	}

	if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
133 134 135 136 137 138
		/*
		 * Add headroom to the skb length, it has been removed
		 * by the driver, but it was actually mapped to DMA.
		 */
		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma,
				 skb->len + rt2x00dev->hw->extra_tx_headroom,
139 140 141 142 143 144 145
				 DMA_TO_DEVICE);
		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
	}
}

void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
{
146 147 148
	if (!skb)
		return;

149
	rt2x00queue_unmap_skb(rt2x00dev, skb);
150 151
	dev_kfree_skb_any(skb);
}
152

153 154
static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
					     struct txentry_desc *txdesc)
155
{
156
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
157
	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
158
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
159
	struct ieee80211_rate *rate =
160
	    ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
161 162 163 164
	const struct rt2x00_rate *hwrate;
	unsigned int data_length;
	unsigned int duration;
	unsigned int residual;
165
	unsigned long irqflags;
166 167 168 169 170 171 172 173 174 175 176

	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;

I
Ivo van Doorn 已提交
177
	/* Data length + CRC + IV/EIV/ICV/MMIC (when using encryption) */
178 179 180 181 182
	data_length = entry->skb->len + 4;

	/*
	 * Check whether this frame is to be acked.
	 */
183
	if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
184 185
		__set_bit(ENTRY_TXD_ACK, &txdesc->flags);

I
Ivo van Doorn 已提交
186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
#ifdef CONFIG_RT2X00_LIB_CRYPTO
	if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags) &&
	    !entry->skb->do_not_encrypt) {
		struct ieee80211_key_conf *hw_key = tx_info->control.hw_key;

		__set_bit(ENTRY_TXD_ENCRYPT, &txdesc->flags);

		txdesc->cipher = rt2x00crypto_key_to_cipher(hw_key);

		if (hw_key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
			__set_bit(ENTRY_TXD_ENCRYPT_PAIRWISE, &txdesc->flags);

		txdesc->key_idx = hw_key->hw_key_idx;
		txdesc->iv_offset = ieee80211_get_hdrlen_from_skb(entry->skb);

		/*
		 * Extend frame length to include all encryption overhead
		 * that will be added by the hardware.
		 */
		data_length += rt2x00crypto_tx_overhead(tx_info);

		if (!(hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_IV))
			__set_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc->flags);

		if (!(hw_key->flags & IEEE80211_KEY_FLAG_GENERATE_MMIC))
			__set_bit(ENTRY_TXD_ENCRYPT_MMIC, &txdesc->flags);
	}
#endif /* CONFIG_RT2X00_LIB_CRYPTO */

215 216 217
	/*
	 * Check if this is a RTS/CTS frame
	 */
218 219
	if (ieee80211_is_rts(hdr->frame_control) ||
	    ieee80211_is_cts(hdr->frame_control)) {
220
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
221
		if (ieee80211_is_rts(hdr->frame_control))
222
			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
223
		else
224
			__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
225
		if (tx_info->control.rts_cts_rate_idx >= 0)
226
			rate =
227
			    ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
228 229 230 231 232
	}

	/*
	 * Determine retry information.
	 */
233
	txdesc->retry_limit = tx_info->control.rates[0].count - 1;
I
Ivo van Doorn 已提交
234
	if (txdesc->retry_limit >= rt2x00dev->long_retry)
235 236 237 238 239
		__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);

	/*
	 * Check if more fragments are pending
	 */
240
	if (ieee80211_has_morefrags(hdr->frame_control)) {
241 242 243 244 245 246 247 248
		__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.
	 */
249 250
	if (ieee80211_is_beacon(hdr->frame_control) ||
	    ieee80211_is_probe_resp(hdr->frame_control))
251 252 253 254 255 256 257 258 259
		__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;
260
	} else if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) {
261 262 263 264 265 266
		__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
		txdesc->ifs = IFS_BACKOFF;
	} else {
		txdesc->ifs = IFS_SIFS;
	}

267 268 269 270 271 272 273 274 275 276 277 278 279
	/*
	 * Hardware should insert sequence counter.
	 * FIXME: We insert a software sequence counter first for
	 * hardware that doesn't support hardware sequence counting.
	 *
	 * This is wrong because beacons are not getting sequence
	 * numbers assigned properly.
	 *
	 * A secondary problem exists for drivers that cannot toggle
	 * sequence counting per-frame, since those will override the
	 * sequence counter given by mac80211.
	 */
	if (tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
280 281
		if (likely(tx_info->control.vif)) {
			struct rt2x00_intf *intf;
282

283
			intf = vif_to_intf(tx_info->control.vif);
284

285
			spin_lock_irqsave(&intf->seqlock, irqflags);
286

287 288 289 290 291 292 293 294 295
			if (test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags))
				intf->seqno += 0x10;
			hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
			hdr->seq_ctrl |= cpu_to_le16(intf->seqno);

			spin_unlock_irqrestore(&intf->seqlock, irqflags);

			__set_bit(ENTRY_TXD_GENERATE_SEQ, &txdesc->flags);
		}
296 297
	}

298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314
	/*
	 * 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.
		 */
315 316
		residual = GET_DURATION_RES(data_length, hwrate->bitrate);
		duration = GET_DURATION(data_length, hwrate->bitrate);
317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339

		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;
	}
}

340 341
static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
					    struct txentry_desc *txdesc)
342
{
343 344
	struct data_queue *queue = entry->queue;
	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
345 346 347 348 349 350 351 352 353 354

	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);

	/*
355 356 357 358 359 360 361 362
	 * Check if we need to kick the queue, there are however a few rules
	 *	1) Don't kick beacon queue
	 *	2) Don't kick unless this is the last in frame in a burst.
	 *	   When the burst flag is set, this frame is always followed
	 *	   by another frame which in some way are related to eachother.
	 *	   This is true for fragments, RTS or CTS-to-self frames.
	 *	3) Rule 2 can be broken when the available entries
	 *	   in the queue are less then a certain threshold.
363
	 */
364 365 366 367 368 369
	if (entry->queue->qid == QID_BEACON)
		return;

	if (rt2x00queue_threshold(queue) ||
	    !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
		rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, queue->qid);
370 371
}

372 373
int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
{
374
	struct ieee80211_tx_info *tx_info;
375 376
	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
	struct txentry_desc txdesc;
I
Ivo van Doorn 已提交
377
	struct skb_frame_desc *skbdesc;
378
	unsigned int iv_len = 0;
379
	u8 rate_idx, rate_flags;
380 381

	if (unlikely(rt2x00queue_full(queue)))
I
Ivo van Doorn 已提交
382
		return -ENOBUFS;
383

384
	if (test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399
		ERROR(queue->rt2x00dev,
		      "Arrived at non-free entry in the non-full queue %d.\n"
		      "Please file bug report to %s.\n",
		      queue->qid, DRV_PROJECT);
		return -EINVAL;
	}

	/*
	 * Copy all TX descriptor information into txdesc,
	 * after that we are free to use the skb->cb array
	 * for our information.
	 */
	entry->skb = skb;
	rt2x00queue_create_tx_descriptor(entry, &txdesc);

400 401 402
	if (IEEE80211_SKB_CB(skb)->control.hw_key != NULL)
		iv_len = IEEE80211_SKB_CB(skb)->control.hw_key->iv_len;

I
Ivo van Doorn 已提交
403
	/*
404
	 * All information is retrieved from the skb->cb array,
I
Ivo van Doorn 已提交
405
	 * now we should claim ownership of the driver part of that
406
	 * array, preserving the bitrate index and flags.
I
Ivo van Doorn 已提交
407
	 */
408 409 410
	tx_info = IEEE80211_SKB_CB(skb);
	rate_idx = tx_info->control.rates[0].idx;
	rate_flags = tx_info->control.rates[0].flags;
I
Ivo van Doorn 已提交
411
	skbdesc = get_skb_frame_desc(skb);
I
Ivo van Doorn 已提交
412 413
	memset(skbdesc, 0, sizeof(*skbdesc));
	skbdesc->entry = entry;
414 415
	skbdesc->tx_rate_idx = rate_idx;
	skbdesc->tx_rate_flags = rate_flags;
I
Ivo van Doorn 已提交
416

I
Ivo van Doorn 已提交
417 418 419 420 421 422
	/*
	 * When hardware encryption is supported, and this frame
	 * is to be encrypted, we should strip the IV/EIV data from
	 * the frame so we can provide it to the driver seperately.
	 */
	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
I
Ivo van Doorn 已提交
423
	    !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags))
I
Ivo van Doorn 已提交
424 425 426 427
		rt2x00crypto_tx_remove_iv(skb, iv_len);

	/*
	 * It could be possible that the queue was corrupted and this
I
Ivo van Doorn 已提交
428 429
	 * call failed. Since we always return NETDEV_TX_OK to mac80211,
	 * this frame will simply be dropped.
I
Ivo van Doorn 已提交
430
	 */
431
	if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
432
		clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
I
Ivo van Doorn 已提交
433
		entry->skb = NULL;
I
Ivo van Doorn 已提交
434
		return -EIO;
435 436
	}

I
Ivo van Doorn 已提交
437 438 439
	if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
		rt2x00queue_map_txskb(queue->rt2x00dev, skb);

440
	set_bit(ENTRY_DATA_PENDING, &entry->flags);
441 442 443 444 445 446 447

	rt2x00queue_index_inc(queue, Q_INDEX);
	rt2x00queue_write_tx_descriptor(entry, &txdesc);

	return 0;
}

448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501
int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
			      struct ieee80211_vif *vif)
{
	struct rt2x00_intf *intf = vif_to_intf(vif);
	struct skb_frame_desc *skbdesc;
	struct txentry_desc txdesc;
	__le32 desc[16];

	if (unlikely(!intf->beacon))
		return -ENOBUFS;

	intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
	if (!intf->beacon->skb)
		return -ENOMEM;

	/*
	 * Copy all TX descriptor information into txdesc,
	 * after that we are free to use the skb->cb array
	 * for our information.
	 */
	rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);

	/*
	 * For the descriptor we use a local array from where the
	 * driver can move it to the correct location required for
	 * the hardware.
	 */
	memset(desc, 0, sizeof(desc));

	/*
	 * Fill in skb descriptor
	 */
	skbdesc = get_skb_frame_desc(intf->beacon->skb);
	memset(skbdesc, 0, sizeof(*skbdesc));
	skbdesc->desc = desc;
	skbdesc->desc_len = intf->beacon->queue->desc_size;
	skbdesc->entry = intf->beacon;

	/*
	 * Write TX descriptor into reserved room in front of the beacon.
	 */
	rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);

	/*
	 * Send beacon to hardware.
	 * Also enable beacon generation, which might have been disabled
	 * by the driver during the config_beacon() callback function.
	 */
	rt2x00dev->ops->lib->write_beacon(intf->beacon);
	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, QID_BEACON);

	return 0;
}

I
Ivo van Doorn 已提交
502
struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
503
					 const enum data_queue_qid queue)
I
Ivo van Doorn 已提交
504 505 506
{
	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);

507
	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
I
Ivo van Doorn 已提交
508 509 510 511 512
		return &rt2x00dev->tx[queue];

	if (!rt2x00dev->bcn)
		return NULL;

513
	if (queue == QID_BEACON)
I
Ivo van Doorn 已提交
514
		return &rt2x00dev->bcn[0];
515
	else if (queue == QID_ATIM && atim)
I
Ivo van Doorn 已提交
516 517 518 519 520 521 522 523 524 525
		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;
526
	unsigned long irqflags;
I
Ivo van Doorn 已提交
527 528 529 530 531 532 533

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

534
	spin_lock_irqsave(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
535 536 537

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

538
	spin_unlock_irqrestore(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
539 540 541 542 543 544 545

	return entry;
}
EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);

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

I
Ivo van Doorn 已提交
548 549 550 551 552 553
	if (unlikely(index >= Q_INDEX_MAX)) {
		ERROR(queue->rt2x00dev,
		      "Index change on invalid index type (%d)\n", index);
		return;
	}

554
	spin_lock_irqsave(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
555 556 557 558 559

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

I
Ivo van Doorn 已提交
560 561 562 563
	if (index == Q_INDEX) {
		queue->length++;
	} else if (index == Q_INDEX_DONE) {
		queue->length--;
J
John Daiker 已提交
564
		queue->count++;
I
Ivo van Doorn 已提交
565
	}
I
Ivo van Doorn 已提交
566

567
	spin_unlock_irqrestore(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
568 569 570 571
}

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

	spin_lock_irqsave(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
575 576 577 578 579

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

580
	spin_unlock_irqrestore(&queue->lock, irqflags);
I
Ivo van Doorn 已提交
581 582
}

583
void rt2x00queue_init_queues(struct rt2x00_dev *rt2x00dev)
I
Ivo van Doorn 已提交
584 585 586 587
{
	struct data_queue *queue;
	unsigned int i;

588
	queue_for_each(rt2x00dev, queue) {
I
Ivo van Doorn 已提交
589 590
		rt2x00queue_reset(queue);

591 592 593
		for (i = 0; i < queue->limit; i++) {
			queue->entries[i].flags = 0;

594
			rt2x00dev->ops->lib->clear_entry(&queue->entries[i]);
595
		}
I
Ivo van Doorn 已提交
596 597 598 599 600 601 602 603 604 605 606 607 608
	}
}

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;
609
	queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
I
Ivo van Doorn 已提交
610 611 612 613 614 615 616 617 618 619 620 621
	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) \
622 623
	( ((char *)(__base)) + ((__limit) * (__esize)) + \
	    ((__index) * (__psize)) )
I
Ivo van Doorn 已提交
624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641

	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;
}

642 643
static void rt2x00queue_free_skbs(struct rt2x00_dev *rt2x00dev,
				  struct data_queue *queue)
644 645 646 647 648 649 650 651
{
	unsigned int i;

	if (!queue->entries)
		return;

	for (i = 0; i < queue->limit; i++) {
		if (queue->entries[i].skb)
652
			rt2x00queue_free_skb(rt2x00dev, queue->entries[i].skb);
653 654 655
	}
}

656 657
static int rt2x00queue_alloc_rxskbs(struct rt2x00_dev *rt2x00dev,
				    struct data_queue *queue)
658 659 660 661 662
{
	unsigned int i;
	struct sk_buff *skb;

	for (i = 0; i < queue->limit; i++) {
663
		skb = rt2x00queue_alloc_rxskb(rt2x00dev, &queue->entries[i]);
664
		if (!skb)
665
			return -ENOMEM;
666 667 668 669 670 671
		queue->entries[i].skb = skb;
	}

	return 0;
}

I
Ivo van Doorn 已提交
672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690
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;

691 692 693 694 695 696
	if (test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags)) {
		status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
						   rt2x00dev->ops->atim);
		if (status)
			goto exit;
	}
I
Ivo van Doorn 已提交
697

698
	status = rt2x00queue_alloc_rxskbs(rt2x00dev, rt2x00dev->rx);
I
Ivo van Doorn 已提交
699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715
	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;

716
	rt2x00queue_free_skbs(rt2x00dev, rt2x00dev->rx);
717

I
Ivo van Doorn 已提交
718 719 720 721 722 723
	queue_for_each(rt2x00dev, queue) {
		kfree(queue->entries);
		queue->entries = NULL;
	}
}

724 725 726 727 728 729 730
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;
731
	queue->txop = 0;
732 733 734 735 736
	queue->aifs = 2;
	queue->cw_min = 5;
	queue->cw_max = 10;
}

I
Ivo van Doorn 已提交
737 738 739 740 741 742 743 744 745 746
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
747
	 * TX: ops->tx_queues
I
Ivo van Doorn 已提交
748 749 750
	 * Beacon: 1
	 * Atim: 1 (if required)
	 */
751
	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
I
Ivo van Doorn 已提交
752 753 754 755 756 757 758 759 760 761 762 763

	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];
764
	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
I
Ivo van Doorn 已提交
765 766 767 768 769 770 771

	/*
	 * 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.
I
Ivo van Doorn 已提交
772 773
	 * BCN: qid = QID_BEACON
	 * ATIM: qid = QID_ATIM
I
Ivo van Doorn 已提交
774
	 */
775
	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
I
Ivo van Doorn 已提交
776

777 778 779
	qid = QID_AC_BE;
	tx_queue_for_each(rt2x00dev, queue)
		rt2x00queue_init(rt2x00dev, queue, qid++);
I
Ivo van Doorn 已提交
780

I
Ivo van Doorn 已提交
781
	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
I
Ivo van Doorn 已提交
782
	if (req_atim)
I
Ivo van Doorn 已提交
783
		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
I
Ivo van Doorn 已提交
784 785 786 787 788 789 790 791 792 793 794

	return 0;
}

void rt2x00queue_free(struct rt2x00_dev *rt2x00dev)
{
	kfree(rt2x00dev->rx);
	rt2x00dev->rx = NULL;
	rt2x00dev->tx = NULL;
	rt2x00dev->bcn = NULL;
}