9 files changed, 71 insertions, 17 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
index d03f5534afee..2399328e8de7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -293,7 +293,7 @@ static void iwl3945_tx_queue_reclaim(struct iwl_priv *priv,
         if (iwl_queue_space(q) > q->low_mark && (txq_id >= 0) &&
                         (txq_id != IWL_CMD_QUEUE_NUM) &&
                         priv->mac80211_registered)
-                ieee80211_wake_queue(priv->hw, txq_id);
+                iwl_wake_queue(priv, txq_id);
 }
 
 /**
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index bd0140be774e..847a6220c5e6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -2178,10 +2178,9 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
                             (iwl_queue_space(&txq->q) > txq->q.low_mark) &&
                             (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)) {
                                 if (agg->state == IWL_AGG_OFF)
-                                        ieee80211_wake_queue(priv->hw, txq_id);
+                                        iwl_wake_queue(priv, txq_id);
                                 else
-                                        ieee80211_wake_queue(priv->hw,
-                                                             txq->swq_id);
+                                        iwl_wake_queue(priv, txq->swq_id);
                         }
                 }
         } else {
@@ -2205,7 +2204,7 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
 
                 if (priv->mac80211_registered &&
                     (iwl_queue_space(&txq->q) > txq->q.low_mark))
-                        ieee80211_wake_queue(priv->hw, txq_id);
+                        iwl_wake_queue(priv, txq_id);
         }
 
         if (qc && likely(sta_id != IWL_INVALID_STATION))
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index a3d9a95a9b37..e5ca2511a81a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -1295,10 +1295,9 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
                             (iwl_queue_space(&txq->q) > txq->q.low_mark) &&
                             (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)) {
                                 if (agg->state == IWL_AGG_OFF)
-                                        ieee80211_wake_queue(priv->hw, txq_id);
+                                        iwl_wake_queue(priv, txq_id);
                                 else
-                                        ieee80211_wake_queue(priv->hw,
-                                                             txq->swq_id);
+                                        iwl_wake_queue(priv, txq->swq_id);
                         }
                 }
         } else {
@@ -1324,7 +1323,7 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
 
                 if (priv->mac80211_registered &&
                     (iwl_queue_space(&txq->q) > txq->q.low_mark))
-                        ieee80211_wake_queue(priv->hw, txq_id);
+                        iwl_wake_queue(priv, txq_id);
         }
 
         if (ieee80211_is_data_qos(tx_resp->frame_ctrl))
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 4b1298c2b0da..c54fb93e9d72 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -1309,9 +1309,6 @@ int iwl_setup_mac(struct iwl_priv *priv)
 
         /* Default value; 4 EDCA QOS priorities */
         hw->queues = 4;
-        /* queues to support 11n aggregation */
-        if (priv->cfg->sku & IWL_SKU_N)
-                hw->ampdu_queues = priv->cfg->mod_params->num_of_ampdu_queues;
 
         hw->conf.beacon_int = 100;
         hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 0baae8022824..ec9a13846edd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -996,6 +996,12 @@ struct iwl_priv {
         u8 key_mapping_key;
         unsigned long ucode_key_table;
 
+        /* queue refcounts */
+#define IWL_MAX_HW_QUEUES       32
+        unsigned long queue_stopped[BITS_TO_LONGS(IWL_MAX_HW_QUEUES)];
+        /* for each AC */
+        atomic_t queue_stop_count[4];
+
         /* Indication if ieee80211_ops->open has been called */
         u8 is_open;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-helpers.h b/drivers/net/wireless/iwlwifi/iwl-helpers.h
index fb64d297dd4e..a1328c3c81ae 100644
--- a/drivers/net/wireless/iwlwifi/iwl-helpers.h
+++ b/drivers/net/wireless/iwlwifi/iwl-helpers.h
@@ -93,4 +93,56 @@ static inline int iwl_alloc_fw_desc(struct pci_dev *pci_dev,
         return (desc->v_addr != NULL) ? 0 : -ENOMEM;
 }
 
+/*
+ * we have 8 bits used like this:
+ *
+ * 7 6 5 4 3 2 1 0
+ * | | | | | | | |
+ * | | | | | | +-+-------- AC queue (0-3)
+ * | | | | | |
+ * | +-+-+-+-+------------ HW A-MPDU queue
+ * |
+ * +---------------------- indicates agg queue
+ */
+static inline u8 iwl_virtual_agg_queue_num(u8 ac, u8 hwq)
+{
+        BUG_ON(ac > 3);   /* only have 2 bits */
+        BUG_ON(hwq > 31); /* only have 5 bits */
+
+        return 0x80 | (hwq << 2) | ac;
+}
+
+static inline void iwl_wake_queue(struct iwl_priv *priv, u8 queue)
+{
+        u8 ac = queue;
+        u8 hwq = queue;
+
+        if (queue & 0x80) {
+                ac = queue & 3;
+                hwq = (queue >> 2) & 0x1f;
+        }
+
+        if (test_and_clear_bit(hwq, priv->queue_stopped))
+                if (atomic_dec_return(&priv->queue_stop_count[ac]) <= 0)
+                        ieee80211_wake_queue(priv->hw, ac);
+}
+
+static inline void iwl_stop_queue(struct iwl_priv *priv, u8 queue)
+{
+        u8 ac = queue;
+        u8 hwq = queue;
+
+        if (queue & 0x80) {
+                ac = queue & 3;
+                hwq = (queue >> 2) & 0x1f;
+        }
+
+        if (!test_and_set_bit(hwq, priv->queue_stopped))
+                if (atomic_inc_return(&priv->queue_stop_count[ac]) > 0)
+                        ieee80211_stop_queue(priv->hw, ac);
+}
+
+#define ieee80211_stop_queue DO_NOT_USE_ieee80211_stop_queue
+#define ieee80211_wake_queue DO_NOT_USE_ieee80211_wake_queue
+
 #endif                          /* __iwl_helpers_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index b13862a598ef..1f117a49c569 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -763,8 +763,10 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                 hdr->seq_ctrl |= cpu_to_le16(seq_number);
                 seq_number += 0x10;
                 /* aggregation is on for this <sta,tid> */
-                if (info->flags & IEEE80211_TX_CTL_AMPDU)
+                if (info->flags & IEEE80211_TX_CTL_AMPDU) {
                         txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
+                        swq_id = iwl_virtual_agg_queue_num(swq_id, txq_id);
+                }
                 priv->stations[sta_id].tid[tid].tfds_in_queue++;
         }
 
@@ -895,7 +897,7 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                         iwl_txq_update_write_ptr(priv, txq);
                         spin_unlock_irqrestore(&priv->lock, flags);
                 } else {
-                        ieee80211_stop_queue(priv->hw, txq->swq_id);
+                        iwl_stop_queue(priv, txq->swq_id);
                 }
         }
 
@@ -1433,7 +1435,7 @@ void iwl_rx_reply_compressed_ba(struct iwl_priv *priv,
                 if ((iwl_queue_space(&txq->q) > txq->q.low_mark) &&
                     priv->mac80211_registered &&
                     (agg->state != IWL_EMPTYING_HW_QUEUE_DELBA))
-                        ieee80211_wake_queue(priv->hw, txq->swq_id);
+                        iwl_wake_queue(priv, txq->swq_id);
 
                 iwl_txq_check_empty(priv, sta_id, tid, scd_flow);
         }
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index ede29b6c4dc8..a71b08ca7c71 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -1168,7 +1168,7 @@ static int iwl3945_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                         spin_unlock_irqrestore(&priv->lock, flags);
                 }
 
-                ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
+                iwl_stop_queue(priv, skb_get_queue_mapping(skb));
         }
 
         return 0;
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 551161024756..d4fdc8b7d7d8 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -933,7 +933,6 @@ static int __init init_mac80211_hwsim(void)
                         BIT(NL80211_IFTYPE_STATION) |
                         BIT(NL80211_IFTYPE_AP) |
                         BIT(NL80211_IFTYPE_MESH_POINT);
-                hw->ampdu_queues = 1;
 
                 hw->flags = IEEE80211_HW_MFP_CAPABLE;