1 files changed, 397 insertions, 16 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 659e9f44c40c..7f442030f5ad 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -25,24 +25,370 @@
 
 #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;
+
+        /*
+         * 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.
+         */
+        reserved_size = 4;
+
+        /*
+         * 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)
+{
+        if (!skb)
+                return;
+
+        rt2x00queue_unmap_skb(rt2x00dev, skb);
+        dev_kfree_skb_any(skb);
+}
+
+static 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;
+        }
+}
+
+static 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);
+}
+
+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;
+}
+
+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;
+}
+
 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 +442,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 +498,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 +531,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 +580,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 +605,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 +635,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 +652,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 +660,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 +669,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;
 }