1 files changed, 341 insertions, 16 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 659e9f44c40..8e86611791f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -25,24 +25,314 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/dma-mapping.h>
 
 #include "rt2x00.h"
 #include "rt2x00lib.h"
 
+struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
+					struct queue_entry *entry)
+{
+	unsigned int frame_size;
+	unsigned int reserved_size;
+	struct sk_buff *skb;
+	struct skb_frame_desc *skbdesc;
+
+	/*
+	 * The frame size includes descriptor size, because the
+	 * hardware directly receive the frame into the skbuffer.
+	 */
+	frame_size = entry->queue->data_size + entry->queue->desc_size;
+
+	/*
+	 * Reserve a few bytes extra headroom to allow drivers some moving
+	 * space (e.g. for alignment), while keeping the skb aligned.
+	 */
+	reserved_size = 8;
+
+	/*
+	 * Allocate skbuffer.
+	 */
+	skb = dev_alloc_skb(frame_size + reserved_size);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, reserved_size);
+	skb_put(skb, frame_size);
+
+	/*
+	 * 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;
+	}
+
+	return skb;
+}
+
+void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
+{
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+
+	skbdesc->skb_dma = dma_map_single(rt2x00dev->dev, skb->data, skb->len,
+					  DMA_TO_DEVICE);
+	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) {
+		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
+				 DMA_TO_DEVICE);
+		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
+	}
+}
+
+void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
+{
+	rt2x00queue_unmap_skb(rt2x00dev, skb);
+	dev_kfree_skb_any(skb);
+}
+
+void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
+				      struct txentry_desc *txdesc)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
+	struct ieee80211_rate *rate =
+	    ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
+	const struct rt2x00_rate *hwrate;
+	unsigned int data_length;
+	unsigned int duration;
+	unsigned int residual;
+
+	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;
+
+	/*
+	 * Check whether this frame is to be acked.
+	 */
+	if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
+		__set_bit(ENTRY_TXD_ACK, &txdesc->flags);
+
+	/*
+	 * Check if this is a RTS/CTS frame
+	 */
+	if (ieee80211_is_rts(hdr->frame_control) ||
+	    ieee80211_is_cts(hdr->frame_control)) {
+		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
+		if (ieee80211_is_rts(hdr->frame_control))
+			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
+		else
+			__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
+		if (tx_info->control.rts_cts_rate_idx >= 0)
+			rate =
+			    ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
+	}
+
+	/*
+	 * Determine retry information.
+	 */
+	txdesc->retry_limit = tx_info->control.retry_limit;
+	if (tx_info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
+		__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
+
+	/*
+	 * Check if more fragments are pending
+	 */
+	if (ieee80211_has_morefrags(hdr->frame_control)) {
+		__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 (ieee80211_is_beacon(hdr->frame_control) ||
+	    ieee80211_is_probe_resp(hdr->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 (tx_info->flags & IEEE80211_TX_CTL_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 data_queue *queue = entry->queue;
+	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+
+	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);
+
+	/*
+	 * 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.
+	 */
+	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);
+}
+EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);
+
+int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
+{
+	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
+	struct txentry_desc txdesc;
+	struct skb_frame_desc *skbdesc;
+
+	if (unlikely(rt2x00queue_full(queue)))
+		return -EINVAL;
+
+	if (__test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
+		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);
+
+	/*
+	 * skb->cb array is now ours and we are free to use it.
+	 */
+	skbdesc = get_skb_frame_desc(entry->skb);
+	memset(skbdesc, 0, sizeof(*skbdesc));
+	skbdesc->entry = entry;
+
+	if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
+		__clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+		return -EIO;
+	}
+
+	if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
+		rt2x00queue_map_txskb(queue->rt2x00dev, skb);
+
+	__set_bit(ENTRY_DATA_PENDING, &entry->flags);
+
+	rt2x00queue_index_inc(queue, Q_INDEX);
+	rt2x00queue_write_tx_descriptor(entry, &txdesc);
+
+	return 0;
+}
+
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
-					 const unsigned int queue)
+					 const enum data_queue_qid queue)
 {
 	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
 
-	if (queue < rt2x00dev->hw->queues && rt2x00dev->tx)
+	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
 		return &rt2x00dev->tx[queue];
 
 	if (!rt2x00dev->bcn)
 		return NULL;
 
-	if (queue == RT2X00_BCN_QUEUE_BEACON)
+	if (queue == QID_BEACON)
 		return &rt2x00dev->bcn[0];
-	else if (queue == RT2X00_BCN_QUEUE_ATIM && atim)
+	else if (queue == QID_ATIM && atim)
 		return &rt2x00dev->bcn[1];
 
 	return NULL;
@@ -96,7 +386,6 @@ void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
 
 	spin_unlock_irqrestore(&queue->lock, irqflags);
 }
-EXPORT_SYMBOL_GPL(rt2x00queue_index_inc);
 
 static void rt2x00queue_reset(struct data_queue *queue)
 {
@@ -153,6 +442,7 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
 	rt2x00queue_reset(queue);
 
 	queue->limit = qdesc->entry_num;
+	queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
 	queue->data_size = qdesc->data_size;
 	queue->desc_size = qdesc->desc_size;
 
@@ -185,12 +475,41 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
 	return 0;
 }
 
+static void rt2x00queue_free_skbs(struct rt2x00_dev *rt2x00dev,
+				  struct data_queue *queue)
+{
+	unsigned int i;
+
+	if (!queue->entries)
+		return;
+
+	for (i = 0; i < queue->limit; i++) {
+		if (queue->entries[i].skb)
+			rt2x00queue_free_skb(rt2x00dev, queue->entries[i].skb);
+	}
+}
+
+static int rt2x00queue_alloc_rxskbs(struct rt2x00_dev *rt2x00dev,
+				    struct data_queue *queue)
+{
+	unsigned int i;
+	struct sk_buff *skb;
+
+	for (i = 0; i < queue->limit; i++) {
+		skb = rt2x00queue_alloc_rxskb(rt2x00dev, &queue->entries[i]);
+		if (!skb)
+			return -ENOMEM;
+		queue->entries[i].skb = skb;
+	}
+
+	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;
@@ -205,11 +524,14 @@ int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 	if (status)
 		goto exit;
 
-	if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-		return 0;
+	if (test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags)) {
+		status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
+						   rt2x00dev->ops->atim);
+		if (status)
+			goto exit;
+	}
 
-	status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
-					   rt2x00dev->ops->atim);
+	status = rt2x00queue_alloc_rxskbs(rt2x00dev, rt2x00dev->rx);
 	if (status)
 		goto exit;
 
@@ -227,6 +549,8 @@ void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
 
+	rt2x00queue_free_skbs(rt2x00dev, rt2x00dev->rx);
+
 	queue_for_each(rt2x00dev, queue) {
 		kfree(queue->entries);
 		queue->entries = NULL;
@@ -255,11 +579,11 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * We need the following queues:
 	 * RX: 1
-	 * TX: hw->queues
+	 * TX: ops->tx_queues
 	 * Beacon: 1
 	 * Atim: 1 (if required)
 	 */
-	rt2x00dev->data_queues = 2 + rt2x00dev->hw->queues + req_atim;
+	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
 
 	queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
 	if (!queue) {
@@ -272,7 +596,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2x00dev->rx = queue;
 	rt2x00dev->tx = &queue[1];
-	rt2x00dev->bcn = &queue[1 + rt2x00dev->hw->queues];
+	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
 
 	/*
 	 * Initialize queue parameters.
@@ -280,7 +604,8 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	 * TX: qid = QID_AC_BE + index
 	 * TX: cw_min: 2^5 = 32.
 	 * TX: cw_max: 2^10 = 1024.
-	 * BCN & Atim: qid = QID_MGMT
+	 * BCN: qid = QID_BEACON
+	 * ATIM: qid = QID_ATIM
 	 */
 	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
 
@@ -288,9 +613,9 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	tx_queue_for_each(rt2x00dev, queue)
 		rt2x00queue_init(rt2x00dev, queue, qid++);
 
-	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_MGMT);
+	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
 	if (req_atim)
-		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_MGMT);
+		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
 
 	return 0;
 }