2 files changed, 128 insertions, 4 deletions

diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index 6c22b23a2845..78f72c34438a 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -260,6 +260,9 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
  * @wd_timeout: queue watchdog timeout (jiffies)
  * @reg_lock: protect hw register access
  * @cmd_in_flight: true when we have a host command in flight
+ * @fw_mon_phys: physical address of the buffer for the firmware monitor
+ * @fw_mon_page: points to the first page of the buffer for the firmware monitor
+ * @fw_mon_size: size of the buffer for the firmware monitor
  */
 struct iwl_trans_pcie {
         struct iwl_rxq rxq;
@@ -312,6 +315,10 @@ struct iwl_trans_pcie {
         /*protect hw register */
         spinlock_t reg_lock;
         bool cmd_in_flight;
+
+        dma_addr_t fw_mon_phys;
+        struct page *fw_mon_page;
+        u32 fw_mon_size;
 };
 
 #define IWL_TRANS_GET_PCIE_TRANS(_iwl_trans) \
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 788085bc65d7..3ffac4888c12 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -76,6 +76,68 @@
 #include "iwl-fw-error-dump.h"
 #include "internal.h"
 
+static void iwl_pcie_free_fw_monitor(struct iwl_trans *trans)
+{
+        struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
+        if (!trans_pcie->fw_mon_page)
+                return;
+
+        dma_unmap_page(trans->dev, trans_pcie->fw_mon_phys,
+                       trans_pcie->fw_mon_size, DMA_FROM_DEVICE);
+        __free_pages(trans_pcie->fw_mon_page,
+                     get_order(trans_pcie->fw_mon_size));
+        trans_pcie->fw_mon_page = NULL;
+        trans_pcie->fw_mon_phys = 0;
+        trans_pcie->fw_mon_size = 0;
+}
+
+static void iwl_pcie_alloc_fw_monitor(struct iwl_trans *trans)
+{
+        struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+        struct page *page;
+        dma_addr_t phys;
+        u32 size;
+        u8 power;
+
+        if (trans_pcie->fw_mon_page) {
+                dma_sync_single_for_device(trans->dev, trans_pcie->fw_mon_phys,
+                                           trans_pcie->fw_mon_size,
+                                           DMA_FROM_DEVICE);
+                return;
+        }
+
+        phys = 0;
+        for (power = 26; power >= 11; power--) {
+                int order;
+
+                size = BIT(power);
+                order = get_order(size);
+                page = alloc_pages(__GFP_COMP | __GFP_NOWARN | __GFP_ZERO,
+                                   order);
+                if (!page)
+                        continue;
+
+                phys = dma_map_page(trans->dev, page, 0, PAGE_SIZE << order,
+                                    DMA_FROM_DEVICE);
+                if (dma_mapping_error(trans->dev, phys)) {
+                        __free_pages(page, order);
+                        continue;
+                }
+                IWL_INFO(trans,
+                         "Allocated 0x%08x bytes (order %d) for firmware monitor.\n",
+                         size, order);
+                break;
+        }
+
+        if (!page)
+                return;
+
+        trans_pcie->fw_mon_page = page;
+        trans_pcie->fw_mon_phys = phys;
+        trans_pcie->fw_mon_size = size;
+}
+
 static u32 iwl_trans_pcie_read_shr(struct iwl_trans *trans, u32 reg)
 {
         iwl_write32(trans, HEEP_CTRL_WRD_PCIEX_CTRL_REG,
@@ -675,6 +737,7 @@ static int iwl_pcie_load_cpu_sections(struct iwl_trans *trans,
 static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
                                 const struct fw_img *image)
 {
+        struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
         int ret = 0;
         int first_ucode_section;
 
@@ -733,6 +796,20 @@ static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
                         return ret;
         }
 
+        /* supported for 7000 only for the moment */
+        if (iwlwifi_mod_params.fw_monitor &&
+            trans->cfg->device_family == IWL_DEVICE_FAMILY_7000) {
+                iwl_pcie_alloc_fw_monitor(trans);
+
+                if (trans_pcie->fw_mon_size) {
+                        iwl_write_prph(trans, MON_BUFF_BASE_ADDR,
+                                       trans_pcie->fw_mon_phys >> 4);
+                        iwl_write_prph(trans, MON_BUFF_END_ADDR,
+                                       (trans_pcie->fw_mon_phys +
+                                        trans_pcie->fw_mon_size) >> 4);
+                }
+        }
+
         /* release CPU reset */
         if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
                 iwl_write_prph(trans, RELEASE_CPU_RESET, RELEASE_CPU_RESET_BIT);
@@ -1126,6 +1203,8 @@ void iwl_trans_pcie_free(struct iwl_trans *trans)
         if (trans_pcie->napi.poll)
                 netif_napi_del(&trans_pcie->napi);
 
+        iwl_pcie_free_fw_monitor(trans);
+
         kfree(trans);
 }
 
@@ -1698,10 +1777,15 @@ static u32 iwl_trans_pcie_dump_data(struct iwl_trans *trans,
         u32 len;
         int i, ptr;
 
+        len = sizeof(*data) +
+                cmdq->q.n_window * (sizeof(*txcmd) + TFD_MAX_PAYLOAD_SIZE);
+
+        if (trans_pcie->fw_mon_page)
+                len += sizeof(*data) + sizeof(struct iwl_fw_error_fw_mon) +
+                        trans_pcie->fw_mon_size;
+
         if (!buf)
-                return sizeof(*data) +
-                       cmdq->q.n_window * (sizeof(*txcmd) +
-                                           TFD_MAX_PAYLOAD_SIZE);
+                return len;
 
         len = 0;
         data = buf;
@@ -1729,7 +1813,40 @@ static u32 iwl_trans_pcie_dump_data(struct iwl_trans *trans,
         spin_unlock_bh(&cmdq->lock);
 
         data->len = cpu_to_le32(len);
-        return sizeof(*data) + len;
+        len += sizeof(*data);
+
+        if (trans_pcie->fw_mon_page) {
+                struct iwl_fw_error_fw_mon *fw_mon_data;
+
+                data = iwl_fw_error_next_data(data);
+                data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_FW_MONITOR);
+                data->len = cpu_to_le32(trans_pcie->fw_mon_size +
+                                        sizeof(*fw_mon_data));
+                fw_mon_data = (void *)data->data;
+                fw_mon_data->fw_mon_wr_ptr =
+                        cpu_to_le32(iwl_read_prph(trans, MON_BUFF_WRPTR));
+                fw_mon_data->fw_mon_cycle_cnt =
+                        cpu_to_le32(iwl_read_prph(trans, MON_BUFF_CYCLE_CNT));
+                fw_mon_data->fw_mon_base_ptr =
+                        cpu_to_le32(iwl_read_prph(trans, MON_BUFF_BASE_ADDR));
+
+                /*
+                 * The firmware is now asserted, it won't write anything to
+                 * the buffer. CPU can take ownership to fetch the data.
+                 * The buffer will be handed back to the device before the
+                 * firmware will be restarted.
+                 */
+                dma_sync_single_for_cpu(trans->dev, trans_pcie->fw_mon_phys,
+                                        trans_pcie->fw_mon_size,
+                                        DMA_FROM_DEVICE);
+                memcpy(fw_mon_data->data, page_address(trans_pcie->fw_mon_page),
+                       trans_pcie->fw_mon_size);
+
+                len += sizeof(*data) + sizeof(*fw_mon_data) +
+                        trans_pcie->fw_mon_size;
+        }
+
+        return len;
 }
 #else
 static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,