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,