4 files changed, 58 insertions, 5 deletions

diff --git a/drivers/net/wireless/iwlwifi/dvm/agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 76b762e48703..e575b9b0cda8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -178,7 +178,7 @@ int iwlagn_hw_valid_rtc_data_addr(u32 addr);
 /* lib */
 int iwlagn_send_tx_power(struct iwl_priv *priv);
 void iwlagn_temperature(struct iwl_priv *priv);
-int iwlagn_txfifo_flush(struct iwl_priv *priv);
+int iwlagn_txfifo_flush(struct iwl_priv *priv, u32 scd_q_msk);
 void iwlagn_dev_txfifo_flush(struct iwl_priv *priv);
 int iwlagn_send_beacon_cmd(struct iwl_priv *priv);
 int iwl_send_statistics_request(struct iwl_priv *priv,
@@ -212,6 +212,8 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
                         struct ieee80211_sta *sta, u16 tid, u8 buf_size);
 int iwlagn_tx_agg_stop(struct iwl_priv *priv, struct ieee80211_vif *vif,
                        struct ieee80211_sta *sta, u16 tid);
+int iwlagn_tx_agg_flush(struct iwl_priv *priv, struct ieee80211_vif *vif,
+                        struct ieee80211_sta *sta, u16 tid);
 int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
                                    struct iwl_rx_cmd_buffer *rxb,
                                    struct iwl_device_cmd *cmd);
diff --git a/drivers/net/wireless/iwlwifi/dvm/lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index f2aeb1a8aa01..29ff93f9656e 100644
--- a/drivers/net/wireless/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -136,7 +136,7 @@ int iwlagn_manage_ibss_station(struct iwl_priv *priv,
  *  1. acquire mutex before calling
  *  2. make sure rf is on and not in exit state
  */
-int iwlagn_txfifo_flush(struct iwl_priv *priv)
+int iwlagn_txfifo_flush(struct iwl_priv *priv, u32 scd_q_msk)
 {
         struct iwl_txfifo_flush_cmd flush_cmd;
         struct iwl_host_cmd cmd = {
@@ -162,6 +162,9 @@ int iwlagn_txfifo_flush(struct iwl_priv *priv)
         if (priv->nvm_data->sku_cap_11n_enable)
                 flush_cmd.queue_control |= IWL_AGG_TX_QUEUE_MSK;
 
+        if (scd_q_msk)
+                flush_cmd.queue_control = cpu_to_le32(scd_q_msk);
+
         IWL_DEBUG_INFO(priv, "queue control: 0x%x\n",
                        flush_cmd.queue_control);
         flush_cmd.flush_control = cpu_to_le16(IWL_DROP_ALL);
@@ -173,7 +176,7 @@ void iwlagn_dev_txfifo_flush(struct iwl_priv *priv)
 {
         mutex_lock(&priv->mutex);
         ieee80211_stop_queues(priv->hw);
-        if (iwlagn_txfifo_flush(priv)) {
+        if (iwlagn_txfifo_flush(priv, 0)) {
                 IWL_ERR(priv, "flush request fail\n");
                 goto done;
         }
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index a7294fa4d7e5..fc3879804622 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -777,9 +777,12 @@ static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
                 IWL_DEBUG_HT(priv, "start Tx\n");
                 ret = iwlagn_tx_agg_start(priv, vif, sta, tid, ssn);
                 break;
-        case IEEE80211_AMPDU_TX_STOP_CONT:
         case IEEE80211_AMPDU_TX_STOP_FLUSH:
         case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
+                IWL_DEBUG_HT(priv, "Flush Tx\n");
+                ret = iwlagn_tx_agg_flush(priv, vif, sta, tid);
+                break;
+        case IEEE80211_AMPDU_TX_STOP_CONT:
                 IWL_DEBUG_HT(priv, "stop Tx\n");
                 ret = iwlagn_tx_agg_stop(priv, vif, sta, tid);
                 if ((ret == 0) && (priv->agg_tids_count > 0)) {
@@ -1122,7 +1125,7 @@ static void iwlagn_mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
          */
         if (drop) {
                 IWL_DEBUG_MAC80211(priv, "send flush command\n");
-                if (iwlagn_txfifo_flush(priv)) {
+                if (iwlagn_txfifo_flush(priv, 0)) {
                         IWL_ERR(priv, "flush request fail\n");
                         goto done;
                 }
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index cc1e0c1a6f48..2c9e9897a3f1 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -674,6 +674,51 @@ int iwlagn_tx_agg_start(struct iwl_priv *priv, struct ieee80211_vif *vif,
         return ret;
 }
 
+int iwlagn_tx_agg_flush(struct iwl_priv *priv, struct ieee80211_vif *vif,
+                        struct ieee80211_sta *sta, u16 tid)
+{
+        struct iwl_tid_data *tid_data;
+        enum iwl_agg_state agg_state;
+        int sta_id, txq_id;
+        sta_id = iwl_sta_id(sta);
+
+        /*
+         * First set the agg state to OFF to avoid calling
+         * ieee80211_stop_tx_ba_cb in iwlagn_check_ratid_empty.
+         */
+        spin_lock_bh(&priv->sta_lock);
+
+        tid_data = &priv->tid_data[sta_id][tid];
+        txq_id = tid_data->agg.txq_id;
+        agg_state = tid_data->agg.state;
+        IWL_DEBUG_TX_QUEUES(priv, "Flush AGG: sta %d tid %d q %d state %d\n",
+                            sta_id, tid, txq_id, tid_data->agg.state);
+
+        tid_data->agg.state = IWL_AGG_OFF;
+
+        spin_unlock_bh(&priv->sta_lock);
+
+        if (iwlagn_txfifo_flush(priv, BIT(txq_id)))
+                IWL_ERR(priv, "Couldn't flush the AGG queue\n");
+
+        if (test_bit(txq_id, priv->agg_q_alloc)) {
+                /*
+                 * If the transport didn't know that we wanted to start
+                 * agreggation, don't tell it that we want to stop them.
+                 * This can happen when we don't get the addBA response on
+                 * time, or we hadn't time to drain the AC queues.
+                 */
+                if (agg_state == IWL_AGG_ON)
+                        iwl_trans_txq_disable(priv->trans, txq_id);
+                else
+                        IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
+                                            agg_state);
+                iwlagn_dealloc_agg_txq(priv, txq_id);
+        }
+
+        return 0;
+}
+
 int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
                         struct ieee80211_sta *sta, u16 tid, u8 buf_size)
 {