1 files changed, 82 insertions, 1 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index ba82c972703a..6e9229830a29 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -457,6 +457,87 @@ static int rt2800usb_get_tx_data_len(struct queue_entry *entry)
 /*
  * TX control handlers
  */
+static bool rt2800usb_txdone_entry_check(struct queue_entry *entry, u32 reg)
+{
+        __le32 *txwi;
+        u32 word;
+        int wcid, ack, pid;
+        int tx_wcid, tx_ack, tx_pid;
+
+        wcid    = rt2x00_get_field32(reg, TX_STA_FIFO_WCID);
+        ack     = rt2x00_get_field32(reg, TX_STA_FIFO_TX_ACK_REQUIRED);
+        pid     = rt2x00_get_field32(reg, TX_STA_FIFO_PID_TYPE);
+
+        /*
+         * This frames has returned with an IO error,
+         * so the status report is not intended for this
+         * frame.
+         */
+        if (test_bit(ENTRY_DATA_IO_FAILED, &entry->flags)) {
+                rt2x00lib_txdone_noinfo(entry, TXDONE_FAILURE);
+                return false;
+        }
+
+        /*
+         * Validate if this TX status report is intended for
+         * this entry by comparing the WCID/ACK/PID fields.
+         */
+        txwi = rt2800usb_get_txwi(entry);
+
+        rt2x00_desc_read(txwi, 1, &word);
+        tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
+        tx_ack  = rt2x00_get_field32(word, TXWI_W1_ACK);
+        tx_pid  = rt2x00_get_field32(word, TXWI_W1_PACKETID);
+
+        if ((wcid != tx_wcid) || (ack != tx_ack) || (pid != tx_pid)) {
+                WARNING(entry->queue->rt2x00dev,
+                        "TX status report missed for queue %d entry %d\n",
+                entry->queue->qid, entry->entry_idx);
+                rt2x00lib_txdone_noinfo(entry, TXDONE_UNKNOWN);
+                return false;
+        }
+
+        return true;
+}
+
+static void rt2800usb_txdone(struct rt2x00_dev *rt2x00dev)
+{
+        struct data_queue *queue;
+        struct queue_entry *entry;
+        u32 reg;
+        u8 qid;
+
+        while (kfifo_get(&rt2x00dev->txstatus_fifo, &reg)) {
+
+                /* TX_STA_FIFO_PID_QUEUE is a 2-bit field, thus
+                 * qid is guaranteed to be one of the TX QIDs
+                 */
+                qid = rt2x00_get_field32(reg, TX_STA_FIFO_PID_QUEUE);
+                queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
+                if (unlikely(!queue)) {
+                        WARNING(rt2x00dev, "Got TX status for an unavailable "
+                                           "queue %u, dropping\n", qid);
+                        continue;
+                }
+
+                /*
+                 * Inside each queue, we process each entry in a chronological
+                 * order. We first check that the queue is not empty.
+                 */
+                entry = NULL;
+                while (!rt2x00queue_empty(queue)) {
+                        entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
+                        if (rt2800usb_txdone_entry_check(entry, reg))
+                                break;
+                }
+
+                if (!entry || rt2x00queue_empty(queue))
+                        break;
+
+                rt2800_txdone_entry(entry, reg);
+        }
+}
+
 static void rt2800usb_work_txdone(struct work_struct *work)
 {
         struct rt2x00_dev *rt2x00dev =
@@ -464,7 +545,7 @@ static void rt2800usb_work_txdone(struct work_struct *work)
         struct data_queue *queue;
         struct queue_entry *entry;
 
-        rt2800_txdone(rt2x00dev);
+        rt2800usb_txdone(rt2x00dev);
 
         /*
          * Process any trailing TX status reports for IO failures,