1 files changed, 63 insertions, 0 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index 5f1e7d802cb..95666e565c7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -1435,3 +1435,66 @@ void iwl_free_tfds_in_queue(struct iwl_priv *priv,
                 priv->stations[sta_id].tid[tid].tfds_in_queue = 0;
         }
 }
+
+#define IWL_FLUSH_WAIT_MS       2000
+
+int iwlagn_wait_tx_queue_empty(struct iwl_priv *priv)
+{
+        struct iwl_tx_queue *txq;
+        struct iwl_queue *q;
+        int cnt;
+        unsigned long now = jiffies;
+        int ret = 0;
+
+        /* waiting for all the tx frames complete might take a while */
+        for (cnt = 0; cnt < priv->hw_params.max_txq_num; cnt++) {
+                if (cnt == IWL_CMD_QUEUE_NUM)
+                        continue;
+                txq = &priv->txq[cnt];
+                q = &txq->q;
+                while (q->read_ptr != q->write_ptr && !time_after(jiffies,
+                       now + msecs_to_jiffies(IWL_FLUSH_WAIT_MS)))
+                                msleep(1);
+
+                if (q->read_ptr != q->write_ptr) {
+                        IWL_ERR(priv, "fail to flush all tx fifo queues\n");
+                        ret = -ETIMEDOUT;
+                        break;
+                }
+        }
+        return ret;
+}
+
+#define IWL_TX_QUEUE_MSK        0xfffff
+
+/**
+ * iwlagn_txfifo_flush: send REPLY_TXFIFO_FLUSH command to uCode
+ *
+ * pre-requirements:
+ *  1. acquire mutex before calling
+ *  2. make sure rf is on and not in exit state
+ */
+int iwlagn_txfifo_flush(struct iwl_priv *priv, u16 flush_control)
+{
+        struct iwl_txfifo_flush_cmd flush_cmd;
+        struct iwl_host_cmd cmd = {
+                .id = REPLY_TXFIFO_FLUSH,
+                .len = sizeof(struct iwl_txfifo_flush_cmd),
+                .flags = CMD_SYNC,
+                .data = &flush_cmd,
+        };
+
+        might_sleep();
+
+        memset(&flush_cmd, 0, sizeof(flush_cmd));
+        flush_cmd.fifo_control = IWL_TX_FIFO_VO_MSK | IWL_TX_FIFO_VI_MSK |
+                                 IWL_TX_FIFO_BE_MSK | IWL_TX_FIFO_BK_MSK;
+        if (priv->cfg->sku & IWL_SKU_N)
+                flush_cmd.fifo_control |= IWL_AGG_TX_QUEUE_MSK;
+
+        IWL_DEBUG_INFO(priv, "fifo queue control: 0X%x\n",
+                       flush_cmd.fifo_control);
+        flush_cmd.flush_control = cpu_to_le16(flush_control);
+
+        return iwl_send_cmd(priv, &cmd);
+}