1 files changed, 113 insertions, 0 deletions
diff --git a/drivers/net/wireless/marvell/mwifiex/sta_cmd.c b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
index c93281f5a47c..dcca71158fc6 100644
--- a/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
@@ -1483,6 +1483,119 @@ int mwifiex_dnld_dt_cfgdata(struct mwifiex_private *priv,
 	return 0;
 }
 
+static int mwifiex_rgpower_table_advance_to_content(u8 **pos, const u8 *data,
+						    const size_t size)
+{
+	while (*pos - data < size) {
+		/* skip spaces, tabs and empty lines */
+		if (**pos == '\r' || **pos == '\n' || **pos == '\0' ||
+		    isspace(**pos)) {
+			(*pos)++;
+			continue;
+		}
+		/* skip line comments */
+		if (**pos == '#') {
+			*pos = strchr(*pos, '\n');
+			if (!*pos)
+				return -EINVAL;
+			(*pos)++;
+			continue;
+		}
+		return 0;
+	}
+	return 0;
+}
+
+int mwifiex_send_rgpower_table(struct mwifiex_private *priv, const u8 *data,
+				const size_t size)
+{
+	int ret = 0;
+	bool start_raw = false;
+	u8 *ptr, *token, *pos = NULL;
+	u8 *_data __free(kfree) = NULL;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct mwifiex_ds_misc_cmd *hostcmd __free(kfree) = NULL;
+
+	hostcmd = kzalloc(sizeof(*hostcmd), GFP_KERNEL);
+	if (!hostcmd)
+		return -ENOMEM;
+
+	_data = kmemdup(data, size, GFP_KERNEL);
+	if (!_data)
+		return -ENOMEM;
+
+	pos = _data;
+	ptr = hostcmd->cmd;
+	while ((pos - _data) < size) {
+		ret = mwifiex_rgpower_table_advance_to_content(&pos, _data, size);
+		if (ret) {
+			mwifiex_dbg(
+				adapter, ERROR,
+				"%s: failed to advance to content in rgpower table\n",
+				__func__);
+			return ret;
+		}
+
+		if (*pos == '}' && start_raw) {
+			hostcmd->len = get_unaligned_le16(&hostcmd->cmd[2]);
+			ret = mwifiex_send_cmd(priv, 0, 0, 0, hostcmd, false);
+			if (ret) {
+				mwifiex_dbg(adapter, ERROR,
+					    "%s: failed to send hostcmd %d\n",
+					    __func__, ret);
+				return ret;
+			}
+
+			memset(hostcmd->cmd, 0, MWIFIEX_SIZE_OF_CMD_BUFFER);
+			ptr = hostcmd->cmd;
+			start_raw = false;
+			pos++;
+			continue;
+		}
+
+		if (!start_raw) {
+			pos = strchr(pos, '=');
+			if (pos) {
+				pos = strchr(pos, '{');
+				if (pos) {
+					start_raw = true;
+					pos++;
+					continue;
+				}
+			}
+			mwifiex_dbg(adapter, ERROR,
+				    "%s: syntax error in hostcmd\n", __func__);
+			return -EINVAL;
+		}
+
+		if (start_raw) {
+			while ((*pos != '\r' && *pos != '\n') &&
+			       (token = strsep((char **)&pos, " "))) {
+				if (ptr - hostcmd->cmd >=
+				    MWIFIEX_SIZE_OF_CMD_BUFFER) {
+					mwifiex_dbg(
+						adapter, ERROR,
+						"%s: hostcmd is larger than %d, aborting\n",
+						__func__, MWIFIEX_SIZE_OF_CMD_BUFFER);
+					return -ENOMEM;
+				}
+
+				ret = kstrtou8(token, 16, ptr);
+				if (ret < 0) {
+					mwifiex_dbg(
+						adapter, ERROR,
+						"%s: failed to parse hostcmd %d token: %s\n",
+						__func__, ret, token);
+					return ret;
+				}
+				ptr++;
+			}
+		}
+	}
+
+	return ret;
+}
+
 /* This function prepares command of set_cfg_data. */
 static int mwifiex_cmd_cfg_data(struct mwifiex_private *priv,
 				struct host_cmd_ds_command *cmd, void *data_buf)