1 files changed, 167 insertions, 7 deletions

diff --git a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
index 429d07b651dd..9e7b8933d30c 100644
--- a/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
@@ -957,6 +957,47 @@ static void rt2800_rate_from_status(struct skb_frame_desc *skbdesc,
         skbdesc->tx_rate_flags = flags;
 }
 
+static bool rt2800_txdone_entry_check(struct queue_entry *entry, u32 reg)
+{
+        __le32 *txwi;
+        u32 word;
+        int wcid, ack, pid;
+        int tx_wcid, tx_ack, tx_pid, is_agg;
+
+        /*
+         * 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))
+                return false;
+
+        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);
+        is_agg  = rt2x00_get_field32(reg, TX_STA_FIFO_TX_AGGRE);
+
+        /*
+         * Validate if this TX status report is intended for
+         * this entry by comparing the WCID/ACK/PID fields.
+         */
+        txwi = rt2800_drv_get_txwi(entry);
+
+        word = rt2x00_desc_read(txwi, 1);
+        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 || (!is_agg && pid != tx_pid)) {
+                rt2x00_dbg(entry->queue->rt2x00dev,
+                           "TX status report missed for queue %d entry %d\n",
+                           entry->queue->qid, entry->entry_idx);
+                return false;
+        }
+
+        return true;
+}
+
 void rt2800_txdone_entry(struct queue_entry *entry, u32 status, __le32 *txwi,
                          bool match)
 {
@@ -1059,6 +1100,119 @@ void rt2800_txdone_entry(struct queue_entry *entry, u32 status, __le32 *txwi,
 }
 EXPORT_SYMBOL_GPL(rt2800_txdone_entry);
 
+void rt2800_txdone(struct rt2x00_dev *rt2x00dev)
+{
+        struct data_queue *queue;
+        struct queue_entry *entry;
+        u32 reg;
+        u8 qid;
+        bool match;
+
+        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(rt2x00queue_empty(queue))) {
+                        rt2x00_dbg(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
+                                   qid);
+                        break;
+                }
+
+                entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
+
+                if (unlikely(test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags) ||
+                             !test_bit(ENTRY_DATA_STATUS_PENDING, &entry->flags))) {
+                        rt2x00_warn(rt2x00dev, "Data pending for entry %u in queue %u\n",
+                                    entry->entry_idx, qid);
+                        break;
+                }
+
+                match = rt2800_txdone_entry_check(entry, reg);
+                rt2800_txdone_entry(entry, reg, rt2800_drv_get_txwi(entry), match);
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800_txdone);
+
+static inline bool rt2800_entry_txstatus_timeout(struct rt2x00_dev *rt2x00dev,
+                                                 struct queue_entry *entry)
+{
+        bool ret;
+        unsigned long tout;
+
+        if (!test_bit(ENTRY_DATA_STATUS_PENDING, &entry->flags))
+                return false;
+
+        if (test_bit(DEVICE_STATE_FLUSHING, &rt2x00dev->flags))
+                tout = msecs_to_jiffies(50);
+        else
+                tout = msecs_to_jiffies(2000);
+
+        ret = time_after(jiffies, entry->last_action + tout);
+        if (unlikely(ret))
+                rt2x00_dbg(entry->queue->rt2x00dev,
+                           "TX status timeout for entry %d in queue %d\n",
+                           entry->entry_idx, entry->queue->qid);
+        return ret;
+}
+
+bool rt2800_txstatus_timeout(struct rt2x00_dev *rt2x00dev)
+{
+        struct data_queue *queue;
+        struct queue_entry *entry;
+
+        if (!test_bit(DEVICE_STATE_FLUSHING, &rt2x00dev->flags)) {
+                unsigned long tout = msecs_to_jiffies(1000);
+
+                if (time_before(jiffies, rt2x00dev->last_nostatus_check + tout))
+                        return false;
+        }
+
+        rt2x00dev->last_nostatus_check = jiffies;
+
+        tx_queue_for_each(rt2x00dev, queue) {
+                entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
+                if (rt2800_entry_txstatus_timeout(rt2x00dev, entry))
+                        return true;
+        }
+
+        return false;
+}
+EXPORT_SYMBOL_GPL(rt2800_txstatus_timeout);
+
+void rt2800_txdone_nostatus(struct rt2x00_dev *rt2x00dev)
+{
+        struct data_queue *queue;
+        struct queue_entry *entry;
+
+        /*
+         * Process any trailing TX status reports for IO failures,
+         * we loop until we find the first non-IO error entry. This
+         * can either be a frame which is free, is being uploaded,
+         * or has completed the upload but didn't have an entry
+         * in the TX_STAT_FIFO register yet.
+         */
+        tx_queue_for_each(rt2x00dev, queue) {
+                while (!rt2x00queue_empty(queue)) {
+                        entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
+
+                        if (test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags) ||
+                            !test_bit(ENTRY_DATA_STATUS_PENDING, &entry->flags))
+                                break;
+
+                        if (test_bit(ENTRY_DATA_IO_FAILED, &entry->flags) ||
+                            rt2800_entry_txstatus_timeout(rt2x00dev, entry))
+                                rt2x00lib_txdone_noinfo(entry, TXDONE_FAILURE);
+                        else
+                                break;
+                }
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800_txdone_nostatus);
+
 static unsigned int rt2800_hw_beacon_base(struct rt2x00_dev *rt2x00dev,
                                           unsigned int index)
 {
@@ -1557,12 +1711,13 @@ static void rt2800_set_max_psdu_len(struct rt2x00_dev *rt2x00dev)
         rt2800_register_write(rt2x00dev, MAX_LEN_CFG, reg);
 }
 
-int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
+int rt2800_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
                    struct ieee80211_sta *sta)
 {
-        int wcid;
-        struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
+        struct rt2x00_dev *rt2x00dev = hw->priv;
         struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+        struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
+        int wcid;
 
         /*
          * Limit global maximum TX AMPDU length to smallest value of all
@@ -1608,8 +1763,10 @@ int rt2800_sta_add(struct rt2x00_dev *rt2x00dev, struct ieee80211_vif *vif,
 }
 EXPORT_SYMBOL_GPL(rt2800_sta_add);
 
-int rt2800_sta_remove(struct rt2x00_dev *rt2x00dev, struct ieee80211_sta *sta)
+int rt2800_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                      struct ieee80211_sta *sta)
 {
+        struct rt2x00_dev *rt2x00dev = hw->priv;
         struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
         struct rt2x00_sta *sta_priv = sta_to_rt2x00_sta(sta);
         int wcid = sta_priv->wcid;
@@ -6220,8 +6377,9 @@ static void rt2800_init_bbp_53xx(struct rt2x00_dev *rt2x00dev)
                 rt2800_register_write(rt2x00dev, GPIO_CTRL, reg);
         }
 
-        /* This chip has hardware antenna diversity*/
-        if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R)) {
+        /* These chips have hardware RX antenna diversity */
+        if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R) ||
+            rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5370G)) {
                 rt2800_bbp_write(rt2x00dev, 150, 0); /* Disable Antenna Software OFDM */
                 rt2800_bbp_write(rt2x00dev, 151, 0); /* Disable Antenna Software CCK */
                 rt2800_bbp_write(rt2x00dev, 154, 0); /* Clear previously selected antenna */
@@ -8748,7 +8906,9 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
                 rt2x00dev->default_ant.rx = ANTENNA_A;
         }
 
-        if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R)) {
+        /* These chips have hardware RX antenna diversity */
+        if (rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5390R) ||
+            rt2x00_rt_rev_gte(rt2x00dev, RT5390, REV_RT5370G)) {
                 rt2x00dev->default_ant.tx = ANTENNA_HW_DIVERSITY; /* Unused */
                 rt2x00dev->default_ant.rx = ANTENNA_HW_DIVERSITY; /* Unused */
         }