1 files changed, 174 insertions, 3 deletions

diff --git a/drivers/net/wireless/iwlwifi/iwl-sv-open.c b/drivers/net/wireless/iwlwifi/iwl-sv-open.c
index 89b6696622c1..69b7e6bf2d6f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sv-open.c
+++ b/drivers/net/wireless/iwlwifi/iwl-sv-open.c
@@ -97,6 +97,13 @@ struct nla_policy iwl_testmode_gnl_msg_policy[IWL_TM_ATTR_MAX] = {
 
 	[IWL_TM_ATTR_SYNC_RSP] = { .type = NLA_UNSPEC, },
 	[IWL_TM_ATTR_UCODE_RX_PKT] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_EEPROM] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_TRACE_ADDR] = { .type = NLA_UNSPEC, },
+	[IWL_TM_ATTR_TRACE_DATA] = { .type = NLA_UNSPEC, },
+
+	[IWL_TM_ATTR_FIXRATE] = { .type = NLA_U32, },
 };
 
 /*
@@ -167,6 +174,31 @@ nla_put_failure:
 void iwl_testmode_init(struct iwl_priv *priv)
 {
 	priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
+	priv->testmode_trace.trace_enabled = false;
+}
+
+static void iwl_trace_cleanup(struct iwl_priv *priv)
+{
+	struct device *dev = &priv->pci_dev->dev;
+
+	if (priv->testmode_trace.trace_enabled) {
+		if (priv->testmode_trace.cpu_addr &&
+		    priv->testmode_trace.dma_addr)
+			dma_free_coherent(dev,
+					TRACE_TOTAL_SIZE,
+					priv->testmode_trace.cpu_addr,
+					priv->testmode_trace.dma_addr);
+		priv->testmode_trace.trace_enabled = false;
+		priv->testmode_trace.cpu_addr = NULL;
+		priv->testmode_trace.trace_addr = NULL;
+		priv->testmode_trace.dma_addr = 0;
+	}
+}
+
+
+void iwl_testmode_cleanup(struct iwl_priv *priv)
+{
+	iwl_trace_cleanup(priv);
 }
 
 /*
@@ -198,10 +230,11 @@ static int iwl_testmode_ucode(struct ieee80211_hw *hw, struct nlattr **tb)
 	}
 
 	cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
-	cmd.data = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
-	cmd.len = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+	cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+	cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+	cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
 	IWL_INFO(priv, "testmode ucode command ID 0x%x, flags 0x%x,"
-				" len %d\n", cmd.id, cmd.flags, cmd.len);
+				" len %d\n", cmd.id, cmd.flags, cmd.len[0]);
 	/* ok, let's submit the command to ucode */
 	return iwl_send_cmd(priv, &cmd);
 }
@@ -388,6 +421,38 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
 				"Error starting the device: %d\n", status);
 		break;
 
+	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
+		if (priv->eeprom) {
+			skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+				priv->cfg->base_params->eeprom_size + 20);
+			if (!skb) {
+				IWL_DEBUG_INFO(priv,
+				       "Error allocating memory\n");
+				return -ENOMEM;
+			}
+			NLA_PUT_U32(skb, IWL_TM_ATTR_COMMAND,
+				IWL_TM_CMD_DEV2APP_EEPROM_RSP);
+			NLA_PUT(skb, IWL_TM_ATTR_EEPROM,
+				priv->cfg->base_params->eeprom_size,
+				priv->eeprom);
+			status = cfg80211_testmode_reply(skb);
+			if (status < 0)
+				IWL_DEBUG_INFO(priv,
+					       "Error sending msg : %d\n",
+					       status);
+		} else
+			return -EFAULT;
+		break;
+
+	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
+		if (!tb[IWL_TM_ATTR_FIXRATE]) {
+			IWL_DEBUG_INFO(priv,
+				       "Error finding fixrate setting\n");
+			return -ENOMSG;
+		}
+		priv->dbg_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
+		break;
+
 	default:
 		IWL_DEBUG_INFO(priv, "Unknown testmode driver command ID\n");
 		return -ENOSYS;
@@ -399,6 +464,102 @@ nla_put_failure:
 	return -EMSGSIZE;
 }
 
+
+/*
+ * This function handles the user application commands for uCode trace
+ *
+ * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
+ * handlers respectively.
+ *
+ * If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
+ * value of the actual command execution is replied to the user application.
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @tb: gnl message fields from the user space
+ */
+static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
+{
+	struct iwl_priv *priv = hw->priv;
+	struct sk_buff *skb;
+	int status = 0;
+	struct device *dev = &priv->pci_dev->dev;
+
+	switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+		if (priv->testmode_trace.trace_enabled)
+			return -EBUSY;
+
+		priv->testmode_trace.cpu_addr =
+			dma_alloc_coherent(dev,
+					   TRACE_TOTAL_SIZE,
+					   &priv->testmode_trace.dma_addr,
+					   GFP_KERNEL);
+		if (!priv->testmode_trace.cpu_addr)
+			return -ENOMEM;
+		priv->testmode_trace.trace_enabled = true;
+		priv->testmode_trace.trace_addr = (u8 *)PTR_ALIGN(
+			priv->testmode_trace.cpu_addr, 0x100);
+		memset(priv->testmode_trace.trace_addr, 0x03B,
+			TRACE_BUFF_SIZE);
+		skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+			sizeof(priv->testmode_trace.dma_addr) + 20);
+		if (!skb) {
+			IWL_DEBUG_INFO(priv,
+				"Error allocating memory\n");
+			iwl_trace_cleanup(priv);
+			return -ENOMEM;
+		}
+		NLA_PUT(skb, IWL_TM_ATTR_TRACE_ADDR,
+			sizeof(priv->testmode_trace.dma_addr),
+			(u64 *)&priv->testmode_trace.dma_addr);
+		status = cfg80211_testmode_reply(skb);
+		if (status < 0) {
+			IWL_DEBUG_INFO(priv,
+				       "Error sending msg : %d\n",
+				       status);
+		}
+		break;
+
+	case IWL_TM_CMD_APP2DEV_END_TRACE:
+		iwl_trace_cleanup(priv);
+		break;
+
+	case IWL_TM_CMD_APP2DEV_READ_TRACE:
+		if (priv->testmode_trace.trace_enabled &&
+		    priv->testmode_trace.trace_addr) {
+			skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+				20 + TRACE_BUFF_SIZE);
+			if (skb == NULL) {
+				IWL_DEBUG_INFO(priv,
+					"Error allocating memory\n");
+				return -ENOMEM;
+			}
+			NLA_PUT(skb, IWL_TM_ATTR_TRACE_DATA,
+				TRACE_BUFF_SIZE,
+				priv->testmode_trace.trace_addr);
+			status = cfg80211_testmode_reply(skb);
+			if (status < 0) {
+				IWL_DEBUG_INFO(priv,
+				       "Error sending msg : %d\n", status);
+			}
+		} else
+			return -EFAULT;
+		break;
+
+	default:
+		IWL_DEBUG_INFO(priv, "Unknown testmode mem command ID\n");
+		return -ENOSYS;
+	}
+	return status;
+
+nla_put_failure:
+	kfree_skb(skb);
+	if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
+	    IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
+		iwl_trace_cleanup(priv);
+	return -EMSGSIZE;
+}
+
 /* The testmode gnl message handler that takes the gnl message from the
  * user space and parses it per the policy iwl_testmode_gnl_msg_policy, then
  * invoke the corresponding handlers.
@@ -455,9 +616,19 @@ int iwl_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
 	case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
 	case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
 	case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
+	case IWL_TM_CMD_APP2DEV_GET_EEPROM:
+	case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
 		IWL_DEBUG_INFO(priv, "testmode cmd to driver\n");
 		result = iwl_testmode_driver(hw, tb);
 		break;
+
+	case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+	case IWL_TM_CMD_APP2DEV_END_TRACE:
+	case IWL_TM_CMD_APP2DEV_READ_TRACE:
+		IWL_DEBUG_INFO(priv, "testmode uCode trace cmd to driver\n");
+		result = iwl_testmode_trace(hw, tb);
+		break;
+
 	default:
 		IWL_DEBUG_INFO(priv, "Unknown testmode command\n");
 		result = -ENOSYS;