3 files changed, 59 insertions, 26 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
index 266490d8a39..a709d05c586 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
@@ -947,7 +947,7 @@ void iwlagn_txq_ctx_reset(struct iwl_priv *priv)
  */
 void iwlagn_txq_ctx_stop(struct iwl_priv *priv)
 {
-        int ch;
+        int ch, txq_id;
         unsigned long flags;
 
         /* Turn off all Tx DMA fifos */
@@ -966,6 +966,16 @@ void iwlagn_txq_ctx_stop(struct iwl_priv *priv)
                             iwl_read_direct32(priv, FH_TSSR_TX_STATUS_REG));
         }
         spin_unlock_irqrestore(&priv->lock, flags);
+
+        if (!priv->txq)
+                return;
+
+        /* Unmap DMA from host system and free skb's */
+        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+                if (txq_id == priv->cmd_queue)
+                        iwl_cmd_queue_unmap(priv);
+                else
+                        iwl_tx_queue_unmap(priv, txq_id);
 }
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index 909b42d5d9c..ce368d8d402 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -510,6 +510,7 @@ void iwl_rx_reply_error(struct iwl_priv *priv,
 * RX
 ******************************************************/
 void iwl_cmd_queue_free(struct iwl_priv *priv);
+void iwl_cmd_queue_unmap(struct iwl_priv *priv);
 int iwl_rx_queue_alloc(struct iwl_priv *priv);
 void iwl_rx_queue_update_write_ptr(struct iwl_priv *priv,
                                   struct iwl_rx_queue *q);
@@ -534,6 +535,7 @@ int iwl_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq,
 void iwl_tx_queue_reset(struct iwl_priv *priv, struct iwl_tx_queue *txq,
                         int slots_num, u32 txq_id);
 void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id);
+void iwl_tx_queue_unmap(struct iwl_priv *priv, int txq_id);
 void iwl_setup_watchdog(struct iwl_priv *priv);
 /*****************************************************
  * TX power
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index 7e607d39da1..277c9175dcf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -86,6 +86,23 @@ void iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 }
 
 /**
+ * iwl_tx_queue_unmap -  Unmap any remaining DMA mappings and free skb's
+ */
+void iwl_tx_queue_unmap(struct iwl_priv *priv, int txq_id)
+{
+        struct iwl_tx_queue *txq = &priv->txq[txq_id];
+        struct iwl_queue *q = &txq->q;
+
+        if (q->n_bd == 0)
+                return;
+
+         while (q->write_ptr != q->read_ptr) {
+                priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+                q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd);
+        }
+}
+
+/**
  * iwl_tx_queue_free - Deallocate DMA queue.
  * @txq: Transmit queue to deallocate.
  *
@@ -96,17 +113,10 @@ void iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id)
 {
         struct iwl_tx_queue *txq = &priv->txq[txq_id];
-        struct iwl_queue *q = &txq->q;
         struct device *dev = &priv->pci_dev->dev;
         int i;
 
-        if (q->n_bd == 0)
-                return;
-
-        /* first, empty all BD's */
-        for (; q->write_ptr != q->read_ptr;
-             q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd))
-                priv->cfg->ops->lib->txq_free_tfd(priv, txq);
+        iwl_tx_queue_unmap(priv, txq_id);
 
         /* De-alloc array of command/tx buffers */
         for (i = 0; i < TFD_TX_CMD_SLOTS; i++)
@@ -132,39 +142,33 @@ void iwl_tx_queue_free(struct iwl_priv *priv, int txq_id)
 }
 
 /**
- * iwl_cmd_queue_free - Deallocate DMA queue.
- * @txq: Transmit queue to deallocate.
- *
- * Empty queue by removing and destroying all BD's.
- * Free all buffers.
- * 0-fill, but do not free "txq" descriptor structure.
+ * iwl_cmd_queue_unmap - Unmap any remaining DMA mappings from command queue
  */
-void iwl_cmd_queue_free(struct iwl_priv *priv)
+void iwl_cmd_queue_unmap(struct iwl_priv *priv)
 {
         struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
         struct iwl_queue *q = &txq->q;
-        struct device *dev = &priv->pci_dev->dev;
         int i;
         bool huge = false;
 
         if (q->n_bd == 0)
                 return;
 
-        for (; q->read_ptr != q->write_ptr;
-             q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+        while (q->read_ptr != q->write_ptr) {
                 /* we have no way to tell if it is a huge cmd ATM */
                 i = get_cmd_index(q, q->read_ptr, 0);
 
-                if (txq->meta[i].flags & CMD_SIZE_HUGE) {
+                if (txq->meta[i].flags & CMD_SIZE_HUGE)
                         huge = true;
-                        continue;
-                }
+                else
+                        pci_unmap_single(priv->pci_dev,
+                                         dma_unmap_addr(&txq->meta[i], mapping),
+                                         dma_unmap_len(&txq->meta[i], len),
+                                         PCI_DMA_BIDIRECTIONAL);
 
-                pci_unmap_single(priv->pci_dev,
-                                 dma_unmap_addr(&txq->meta[i], mapping),
-                                 dma_unmap_len(&txq->meta[i], len),
-                                 PCI_DMA_BIDIRECTIONAL);
+             q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd);
         }
+
         if (huge) {
                 i = q->n_window;
                 pci_unmap_single(priv->pci_dev,
@@ -172,6 +176,23 @@ void iwl_cmd_queue_free(struct iwl_priv *priv)
                                  dma_unmap_len(&txq->meta[i], len),
                                  PCI_DMA_BIDIRECTIONAL);
         }
+}
+
+/**
+ * iwl_cmd_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.
+ * 0-fill, but do not free "txq" descriptor structure.
+ */
+void iwl_cmd_queue_free(struct iwl_priv *priv)
+{
+        struct iwl_tx_queue *txq = &priv->txq[priv->cmd_queue];
+        struct device *dev = &priv->pci_dev->dev;
+        int i;
+
+        iwl_cmd_queue_unmap(priv);
 
         /* De-alloc array of command/tx buffers */
         for (i = 0; i <= TFD_CMD_SLOTS; i++)