rt2x00queue.c 25.3 KB
Newer Older
I
Ivo van Doorn 已提交
1
/*
2 3
	Copyright (C) 2010 Willow Garage <http://www.willowgarage.com>
	Copyright (C) 2004 - 2010 Ivo van Doorn <IvDoorn@gmail.com>
4
	Copyright (C) 2004 - 2009 Gertjan van Wingerde <gwingerde@gmail.com>
I
Ivo van Doorn 已提交
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
	<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.
 */

28
#include <linux/slab.h>
I
Ivo van Doorn 已提交
29 30
#include <linux/kernel.h>
#include <linux/module.h>
31
#include <linux/dma-mapping.h>
I
Ivo van Doorn 已提交
32 33 34 35

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

36
struct sk_buff *rt2x00queue_alloc_rxskb(struct queue_entry *entry)
37
{
38
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
39 40
	struct sk_buff *skb;
	struct skb_frame_desc *skbdesc;
I
Ivo van Doorn 已提交
41 42 43
	unsigned int frame_size;
	unsigned int head_size = 0;
	unsigned int tail_size = 0;
44 45 46 47 48

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

	/*
52 53 54
	 * 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.
55
	 */
I
Ivo van Doorn 已提交
56 57 58 59 60
	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
61
	 * and 8 bytes for ICV data as tailroon.
I
Ivo van Doorn 已提交
62 63 64
	 */
	if (test_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags)) {
		head_size += 8;
65
		tail_size += 8;
I
Ivo van Doorn 已提交
66
	}
67 68 69 70

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

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

82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
	/*
	 * 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;
	}

97 98
	return skb;
}
99

100
void rt2x00queue_map_txskb(struct queue_entry *entry)
101
{
102 103
	struct device *dev = entry->queue->rt2x00dev->dev;
	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
104

105
	skbdesc->skb_dma =
106
	    dma_map_single(dev, entry->skb->data, entry->skb->len, DMA_TO_DEVICE);
107 108 109 110
	skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
}
EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);

111
void rt2x00queue_unmap_skb(struct queue_entry *entry)
112
{
113 114
	struct device *dev = entry->queue->rt2x00dev->dev;
	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
115 116

	if (skbdesc->flags & SKBDESC_DMA_MAPPED_RX) {
117
		dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
118 119
				 DMA_FROM_DEVICE);
		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_RX;
H
Helmut Schaa 已提交
120
	} else if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
121
		dma_unmap_single(dev, skbdesc->skb_dma, entry->skb->len,
122 123 124 125
				 DMA_TO_DEVICE);
		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
	}
}
126
EXPORT_SYMBOL_GPL(rt2x00queue_unmap_skb);
127

128
void rt2x00queue_free_skb(struct queue_entry *entry)
129
{
130
	if (!entry->skb)
131 132
		return;

133 134 135
	rt2x00queue_unmap_skb(entry);
	dev_kfree_skb_any(entry->skb);
	entry->skb = NULL;
136
}
137

138
void rt2x00queue_align_frame(struct sk_buff *skb)
139 140
{
	unsigned int frame_length = skb->len;
141
	unsigned int align = ALIGN_SIZE(skb, 0);
142 143 144 145

	if (!align)
		return;

146 147 148 149 150
	skb_push(skb, align);
	memmove(skb->data, skb->data + align, frame_length);
	skb_trim(skb, frame_length);
}

151
void rt2x00queue_align_payload(struct sk_buff *skb, unsigned int header_length)
152 153
{
	unsigned int frame_length = skb->len;
154
	unsigned int align = ALIGN_SIZE(skb, header_length);
155 156 157 158 159 160 161 162 163 164 165

	if (!align)
		return;

	skb_push(skb, align);
	memmove(skb->data, skb->data + align, frame_length);
	skb_trim(skb, frame_length);
}

void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length)
{
166
	unsigned int payload_length = skb->len - header_length;
167 168
	unsigned int header_align = ALIGN_SIZE(skb, 0);
	unsigned int payload_align = ALIGN_SIZE(skb, header_length);
169
	unsigned int l2pad = payload_length ? L2PAD_SIZE(header_length) : 0;
170

171 172 173 174 175 176 177 178 179 180
	/*
	 * Adjust the header alignment if the payload needs to be moved more
	 * than the header.
	 */
	if (payload_align > header_align)
		header_align += 4;

	/* There is nothing to do if no alignment is needed */
	if (!header_align)
		return;
181

182 183 184 185 186 187 188 189 190 191
	/* Reserve the amount of space needed in front of the frame */
	skb_push(skb, header_align);

	/*
	 * Move the header.
	 */
	memmove(skb->data, skb->data + header_align, header_length);

	/* Move the payload, if present and if required */
	if (payload_length && payload_align)
192
		memmove(skb->data + header_length + l2pad,
193
			skb->data + header_length + l2pad + payload_align,
194 195 196 197
			payload_length);

	/* Trim the skb to the correct size */
	skb_trim(skb, header_length + l2pad + payload_length);
198 199
}

200 201
void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length)
{
202
	unsigned int l2pad = L2PAD_SIZE(header_length);
203

204
	if (!l2pad)
205 206 207 208 209 210
		return;

	memmove(skb->data + l2pad, skb->data, header_length);
	skb_pull(skb, l2pad);
}

211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
static void rt2x00queue_create_tx_descriptor_seq(struct queue_entry *entry,
						 struct txentry_desc *txdesc)
{
	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
	struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
	unsigned long irqflags;

	if (!(tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) ||
	    unlikely(!tx_info->control.vif))
		return;

	/*
	 * 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.
	 */
	spin_lock_irqsave(&intf->seqlock, irqflags);

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

static void rt2x00queue_create_tx_descriptor_plcp(struct queue_entry *entry,
						  struct txentry_desc *txdesc,
						  const struct rt2x00_rate *hwrate)
{
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
	struct ieee80211_tx_rate *txrate = &tx_info->control.rates[0];
	unsigned int data_length;
	unsigned int duration;
	unsigned int residual;

	/* Data length + CRC + Crypto overhead (IV/EIV/ICV/MIC) */
	data_length = entry->skb->len + 4;
	data_length += rt2x00crypto_tx_overhead(rt2x00dev, entry->skb);

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

	if (hwrate->flags & DEV_RATE_OFDM) {
		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 (txrate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
			txdesc->signal |= 0x08;
	}
}

301 302
static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
					     struct txentry_desc *txdesc)
303
{
304
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
305
	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
306
	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
307
	struct ieee80211_rate *rate =
308
	    ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
309 310 311 312
	const struct rt2x00_rate *hwrate;

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

313
	/*
314
	 * Header and frame information.
315
	 */
316
	txdesc->length = entry->skb->len;
317 318
	txdesc->header_length = ieee80211_get_hdrlen_from_skb(entry->skb);

319 320 321
	/*
	 * Check whether this frame is to be acked.
	 */
322
	if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
323 324 325 326 327
		__set_bit(ENTRY_TXD_ACK, &txdesc->flags);

	/*
	 * Check if this is a RTS/CTS frame
	 */
328 329
	if (ieee80211_is_rts(hdr->frame_control) ||
	    ieee80211_is_cts(hdr->frame_control)) {
330
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
331
		if (ieee80211_is_rts(hdr->frame_control))
332
			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
333
		else
334
			__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
335
		if (tx_info->control.rts_cts_rate_idx >= 0)
336
			rate =
337
			    ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
338 339 340 341 342
	}

	/*
	 * Determine retry information.
	 */
343
	txdesc->retry_limit = tx_info->control.rates[0].count - 1;
I
Ivo van Doorn 已提交
344
	if (txdesc->retry_limit >= rt2x00dev->long_retry)
345 346 347 348 349
		__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);

	/*
	 * Check if more fragments are pending
	 */
350
	if (ieee80211_has_morefrags(hdr->frame_control)) {
351 352 353 354
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
		__set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
	}

355 356 357 358 359 360
	/*
	 * Check if more frames (!= fragments) are pending
	 */
	if (tx_info->flags & IEEE80211_TX_CTL_MORE_FRAMES)
		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);

361 362
	/*
	 * Beacons and probe responses require the tsf timestamp
363 364 365
	 * to be inserted into the frame, except for a frame that has been injected
	 * through a monitor interface. This latter is needed for testing a
	 * monitor interface.
366
	 */
367 368 369
	if ((ieee80211_is_beacon(hdr->frame_control) ||
	    ieee80211_is_probe_resp(hdr->frame_control)) &&
	    (!(tx_info->flags & IEEE80211_TX_CTL_INJECTED)))
370 371 372 373 374 375 376
		__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.
	 */
377 378
	if ((tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) &&
	    !test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
379 380
		__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
		txdesc->ifs = IFS_BACKOFF;
381
	} else
382 383
		txdesc->ifs = IFS_SIFS;

384 385 386
	/*
	 * Determine rate modulation.
	 */
387
	hwrate = rt2x00_get_rate(rate->hw_value);
388
	txdesc->rate_mode = RATE_MODE_CCK;
389
	if (hwrate->flags & DEV_RATE_OFDM)
390
		txdesc->rate_mode = RATE_MODE_OFDM;
391

392 393 394 395
	/*
	 * Apply TX descriptor handling by components
	 */
	rt2x00crypto_create_tx_descriptor(entry, txdesc);
396
	rt2x00ht_create_tx_descriptor(entry, txdesc, hwrate);
397 398
	rt2x00queue_create_tx_descriptor_seq(entry, txdesc);
	rt2x00queue_create_tx_descriptor_plcp(entry, txdesc, hwrate);
399 400
}

401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426
static int rt2x00queue_write_tx_data(struct queue_entry *entry,
				     struct txentry_desc *txdesc)
{
	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;

	/*
	 * This should not happen, we already checked the entry
	 * was ours. When the hardware disagrees there has been
	 * a queue corruption!
	 */
	if (unlikely(rt2x00dev->ops->lib->get_entry_state &&
		     rt2x00dev->ops->lib->get_entry_state(entry))) {
		ERROR(rt2x00dev,
		      "Corrupt queue %d, accessing entry which is not ours.\n"
		      "Please file bug report to %s.\n",
		      entry->queue->qid, DRV_PROJECT);
		return -EINVAL;
	}

	/*
	 * Add the requested extra tx headroom in front of the skb.
	 */
	skb_push(entry->skb, rt2x00dev->ops->extra_tx_headroom);
	memset(entry->skb->data, 0, rt2x00dev->ops->extra_tx_headroom);

	/*
427
	 * Call the driver's write_tx_data function, if it exists.
428
	 */
429 430
	if (rt2x00dev->ops->lib->write_tx_data)
		rt2x00dev->ops->lib->write_tx_data(entry, txdesc);
431 432 433 434 435

	/*
	 * Map the skb to DMA.
	 */
	if (test_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags))
436
		rt2x00queue_map_txskb(entry);
437 438 439 440

	return 0;
}

441 442
static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
					    struct txentry_desc *txdesc)
443
{
444
	struct data_queue *queue = entry->queue;
445

446
	queue->rt2x00dev->ops->lib->write_tx_desc(entry, txdesc);
447 448 449 450 451

	/*
	 * All processing on the frame has been completed, this means
	 * it is now ready to be dumped to userspace through debugfs.
	 */
452
	rt2x00debug_dump_frame(queue->rt2x00dev, DUMP_FRAME_TX, entry->skb);
453 454 455 456 457 458 459
}

static void rt2x00queue_kick_tx_queue(struct queue_entry *entry,
				      struct txentry_desc *txdesc)
{
	struct data_queue *queue = entry->queue;
	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
460 461

	/*
462
	 * Check if we need to kick the queue, there are however a few rules
463
	 *	1) Don't kick unless this is the last in frame in a burst.
464 465 466
	 *	   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.
467
	 *	2) Rule 1 can be broken when the available entries
468
	 *	   in the queue are less then a certain threshold.
469
	 */
470 471
	if (rt2x00queue_threshold(queue) ||
	    !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
472
		rt2x00dev->ops->lib->kick_tx_queue(queue);
473 474
}

475 476
int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
			       bool local)
477
{
478
	struct ieee80211_tx_info *tx_info;
479 480
	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
	struct txentry_desc txdesc;
I
Ivo van Doorn 已提交
481
	struct skb_frame_desc *skbdesc;
482
	u8 rate_idx, rate_flags;
483 484

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

487 488
	if (unlikely(test_and_set_bit(ENTRY_OWNER_DEVICE_DATA,
				      &entry->flags))) {
489 490 491 492 493 494 495 496 497 498 499 500 501 502 503
		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);

I
Ivo van Doorn 已提交
504
	/*
505
	 * All information is retrieved from the skb->cb array,
I
Ivo van Doorn 已提交
506
	 * now we should claim ownership of the driver part of that
507
	 * array, preserving the bitrate index and flags.
I
Ivo van Doorn 已提交
508
	 */
509 510 511
	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 已提交
512
	skbdesc = get_skb_frame_desc(skb);
I
Ivo van Doorn 已提交
513 514
	memset(skbdesc, 0, sizeof(*skbdesc));
	skbdesc->entry = entry;
515 516
	skbdesc->tx_rate_idx = rate_idx;
	skbdesc->tx_rate_flags = rate_flags;
I
Ivo van Doorn 已提交
517

518 519 520
	if (local)
		skbdesc->flags |= SKBDESC_NOT_MAC80211;

I
Ivo van Doorn 已提交
521 522 523
	/*
	 * When hardware encryption is supported, and this frame
	 * is to be encrypted, we should strip the IV/EIV data from
D
Daniel Mack 已提交
524
	 * the frame so we can provide it to the driver separately.
I
Ivo van Doorn 已提交
525 526
	 */
	if (test_bit(ENTRY_TXD_ENCRYPT, &txdesc.flags) &&
527
	    !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc.flags)) {
528
		if (test_bit(DRIVER_REQUIRE_COPY_IV, &queue->rt2x00dev->flags))
529
			rt2x00crypto_tx_copy_iv(skb, &txdesc);
530
		else
531
			rt2x00crypto_tx_remove_iv(skb, &txdesc);
532
	}
I
Ivo van Doorn 已提交
533

534 535 536 537 538 539 540 541
	/*
	 * When DMA allocation is required we should guarentee to the
	 * driver that the DMA is aligned to a 4-byte boundary.
	 * However some drivers require L2 padding to pad the payload
	 * rather then the header. This could be a requirement for
	 * PCI and USB devices, while header alignment only is valid
	 * for PCI devices.
	 */
542
	if (test_bit(DRIVER_REQUIRE_L2PAD, &queue->rt2x00dev->flags))
543
		rt2x00queue_insert_l2pad(entry->skb, txdesc.header_length);
544
	else if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
545
		rt2x00queue_align_frame(entry->skb);
546

I
Ivo van Doorn 已提交
547 548
	/*
	 * It could be possible that the queue was corrupted and this
I
Ivo van Doorn 已提交
549 550
	 * call failed. Since we always return NETDEV_TX_OK to mac80211,
	 * this frame will simply be dropped.
I
Ivo van Doorn 已提交
551
	 */
552
	if (unlikely(rt2x00queue_write_tx_data(entry, &txdesc))) {
553
		clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
I
Ivo van Doorn 已提交
554
		entry->skb = NULL;
I
Ivo van Doorn 已提交
555
		return -EIO;
556 557
	}

558
	set_bit(ENTRY_DATA_PENDING, &entry->flags);
559 560 561

	rt2x00queue_index_inc(queue, Q_INDEX);
	rt2x00queue_write_tx_descriptor(entry, &txdesc);
562
	rt2x00queue_kick_tx_queue(entry, &txdesc);
563 564 565 566

	return 0;
}

567
int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
568 569
			      struct ieee80211_vif *vif,
			      const bool enable_beacon)
570 571 572 573 574 575 576 577
{
	struct rt2x00_intf *intf = vif_to_intf(vif);
	struct skb_frame_desc *skbdesc;
	struct txentry_desc txdesc;

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

578 579 580 581 582
	mutex_lock(&intf->beacon_skb_mutex);

	/*
	 * Clean up the beacon skb.
	 */
583
	rt2x00queue_free_skb(intf->beacon);
584

585
	if (!enable_beacon) {
586
		rt2x00dev->ops->lib->kill_tx_queue(intf->beacon->queue);
587
		mutex_unlock(&intf->beacon_skb_mutex);
588 589 590
		return 0;
	}

591
	intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
592 593
	if (!intf->beacon->skb) {
		mutex_unlock(&intf->beacon_skb_mutex);
594
		return -ENOMEM;
595
	}
596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611

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

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

	/*
612
	 * Send beacon to hardware and enable beacon genaration..
613
	 */
614
	rt2x00dev->ops->lib->write_beacon(intf->beacon, &txdesc);
615

616 617
	mutex_unlock(&intf->beacon_skb_mutex);

618 619 620
	return 0;
}

621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643
void rt2x00queue_for_each_entry(struct data_queue *queue,
				enum queue_index start,
				enum queue_index end,
				void (*fn)(struct queue_entry *entry))
{
	unsigned long irqflags;
	unsigned int index_start;
	unsigned int index_end;
	unsigned int i;

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

	/*
	 * Only protect the range we are going to loop over,
	 * if during our loop a extra entry is set to pending
	 * it should not be kicked during this run, since it
	 * is part of another TX operation.
	 */
644
	spin_lock_irqsave(&queue->index_lock, irqflags);
645 646
	index_start = queue->index[start];
	index_end = queue->index[end];
647
	spin_unlock_irqrestore(&queue->index_lock, irqflags);
648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665

	/*
	 * Start from the TX done pointer, this guarentees that we will
	 * send out all frames in the correct order.
	 */
	if (index_start < index_end) {
		for (i = index_start; i < index_end; i++)
			fn(&queue->entries[i]);
	} else {
		for (i = index_start; i < queue->limit; i++)
			fn(&queue->entries[i]);

		for (i = 0; i < index_end; i++)
			fn(&queue->entries[i]);
	}
}
EXPORT_SYMBOL_GPL(rt2x00queue_for_each_entry);

I
Ivo van Doorn 已提交
666
struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
667
					 const enum data_queue_qid queue)
I
Ivo van Doorn 已提交
668 669 670
{
	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);

671 672 673
	if (queue == QID_RX)
		return rt2x00dev->rx;

674
	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
I
Ivo van Doorn 已提交
675 676 677 678 679
		return &rt2x00dev->tx[queue];

	if (!rt2x00dev->bcn)
		return NULL;

680
	if (queue == QID_BEACON)
I
Ivo van Doorn 已提交
681
		return &rt2x00dev->bcn[0];
682
	else if (queue == QID_ATIM && atim)
I
Ivo van Doorn 已提交
683 684 685 686 687 688 689 690 691 692
		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;
693
	unsigned long irqflags;
I
Ivo van Doorn 已提交
694 695 696 697 698 699 700

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

701
	spin_lock_irqsave(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
702 703 704

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

705
	spin_unlock_irqrestore(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
706 707 708 709 710 711 712

	return entry;
}
EXPORT_SYMBOL_GPL(rt2x00queue_get_entry);

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

I
Ivo van Doorn 已提交
715 716 717 718 719 720
	if (unlikely(index >= Q_INDEX_MAX)) {
		ERROR(queue->rt2x00dev,
		      "Index change on invalid index type (%d)\n", index);
		return;
	}

721
	spin_lock_irqsave(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
722 723 724 725 726

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

727 728
	queue->last_action[index] = jiffies;

I
Ivo van Doorn 已提交
729 730 731 732
	if (index == Q_INDEX) {
		queue->length++;
	} else if (index == Q_INDEX_DONE) {
		queue->length--;
J
John Daiker 已提交
733
		queue->count++;
I
Ivo van Doorn 已提交
734
	}
I
Ivo van Doorn 已提交
735

736
	spin_unlock_irqrestore(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
737 738 739 740
}

static void rt2x00queue_reset(struct data_queue *queue)
{
741
	unsigned long irqflags;
742
	unsigned int i;
743

744
	spin_lock_irqsave(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
745 746 747

	queue->count = 0;
	queue->length = 0;
748 749 750 751 752

	for (i = 0; i < Q_INDEX_MAX; i++) {
		queue->index[i] = 0;
		queue->last_action[i] = jiffies;
	}
I
Ivo van Doorn 已提交
753

754
	spin_unlock_irqrestore(&queue->index_lock, irqflags);
I
Ivo van Doorn 已提交
755 756
}

757 758 759 760 761
void rt2x00queue_stop_queues(struct rt2x00_dev *rt2x00dev)
{
	struct data_queue *queue;

	txall_queue_for_each(rt2x00dev, queue)
762
		rt2x00dev->ops->lib->kill_tx_queue(queue);
763 764
}

765
void rt2x00queue_init_queues(struct rt2x00_dev *rt2x00dev)
I
Ivo van Doorn 已提交
766 767 768 769
{
	struct data_queue *queue;
	unsigned int i;

770
	queue_for_each(rt2x00dev, queue) {
I
Ivo van Doorn 已提交
771 772
		rt2x00queue_reset(queue);

773
		for (i = 0; i < queue->limit; i++) {
774
			rt2x00dev->ops->lib->clear_entry(&queue->entries[i]);
775 776
			if (queue->qid == QID_RX)
				rt2x00queue_index_inc(queue, Q_INDEX);
777
		}
I
Ivo van Doorn 已提交
778 779 780 781 782 783 784 785 786 787 788 789 790
	}
}

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;
791
	queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
I
Ivo van Doorn 已提交
792 793 794 795 796 797 798
	queue->data_size = qdesc->data_size;
	queue->desc_size = qdesc->desc_size;

	/*
	 * Allocate all queue entries.
	 */
	entry_size = sizeof(*entries) + qdesc->priv_size;
799
	entries = kcalloc(queue->limit, entry_size, GFP_KERNEL);
I
Ivo van Doorn 已提交
800 801 802 803
	if (!entries)
		return -ENOMEM;

#define QUEUE_ENTRY_PRIV_OFFSET(__base, __index, __limit, __esize, __psize) \
804 805
	(((char *)(__base)) + ((__limit) * (__esize)) + \
	    ((__index) * (__psize)))
I
Ivo van Doorn 已提交
806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822 823

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

824
static void rt2x00queue_free_skbs(struct data_queue *queue)
825 826 827 828 829 830 831
{
	unsigned int i;

	if (!queue->entries)
		return;

	for (i = 0; i < queue->limit; i++) {
832
		rt2x00queue_free_skb(&queue->entries[i]);
833 834 835
	}
}

836
static int rt2x00queue_alloc_rxskbs(struct data_queue *queue)
837 838 839 840 841
{
	unsigned int i;
	struct sk_buff *skb;

	for (i = 0; i < queue->limit; i++) {
842
		skb = rt2x00queue_alloc_rxskb(&queue->entries[i]);
843
		if (!skb)
844
			return -ENOMEM;
845 846 847 848 849 850
		queue->entries[i].skb = skb;
	}

	return 0;
}

I
Ivo van Doorn 已提交
851 852 853 854 855 856 857 858 859 860 861 862 863 864 865 866 867 868 869
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;

870 871 872 873 874 875
	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 已提交
876

877
	status = rt2x00queue_alloc_rxskbs(rt2x00dev->rx);
I
Ivo van Doorn 已提交
878 879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894
	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;

895
	rt2x00queue_free_skbs(rt2x00dev->rx);
896

I
Ivo van Doorn 已提交
897 898 899 900 901 902
	queue_for_each(rt2x00dev, queue) {
		kfree(queue->entries);
		queue->entries = NULL;
	}
}

903 904 905
static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
			     struct data_queue *queue, enum data_queue_qid qid)
{
906
	spin_lock_init(&queue->index_lock);
907 908 909

	queue->rt2x00dev = rt2x00dev;
	queue->qid = qid;
910
	queue->txop = 0;
911 912 913 914 915
	queue->aifs = 2;
	queue->cw_min = 5;
	queue->cw_max = 10;
}

I
Ivo van Doorn 已提交
916 917 918 919 920 921 922 923 924 925
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
926
	 * TX: ops->tx_queues
I
Ivo van Doorn 已提交
927 928 929
	 * Beacon: 1
	 * Atim: 1 (if required)
	 */
930
	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
I
Ivo van Doorn 已提交
931

932
	queue = kcalloc(rt2x00dev->data_queues, sizeof(*queue), GFP_KERNEL);
I
Ivo van Doorn 已提交
933 934 935 936 937 938 939 940 941 942
	if (!queue) {
		ERROR(rt2x00dev, "Queue allocation failed.\n");
		return -ENOMEM;
	}

	/*
	 * Initialize pointers
	 */
	rt2x00dev->rx = queue;
	rt2x00dev->tx = &queue[1];
943
	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
I
Ivo van Doorn 已提交
944 945 946 947 948 949 950

	/*
	 * 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 已提交
951 952
	 * BCN: qid = QID_BEACON
	 * ATIM: qid = QID_ATIM
I
Ivo van Doorn 已提交
953
	 */
954
	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
I
Ivo van Doorn 已提交
955

956 957 958
	qid = QID_AC_BE;
	tx_queue_for_each(rt2x00dev, queue)
		rt2x00queue_init(rt2x00dev, queue, qid++);
I
Ivo van Doorn 已提交
959

I
Ivo van Doorn 已提交
960
	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
I
Ivo van Doorn 已提交
961
	if (req_atim)
I
Ivo van Doorn 已提交
962
		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
I
Ivo van Doorn 已提交
963 964 965 966 967 968 969 970 971 972 973

	return 0;
}

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