diff --git a/drivers/net/wireless/marvell/mwifiex/cfg80211.c b/drivers/net/wireless/marvell/mwifiex/cfg80211.c index 876d420e936e..0a03d3f90b56 100644 --- a/drivers/net/wireless/marvell/mwifiex/cfg80211.c +++ b/drivers/net/wireless/marvell/mwifiex/cfg80211.c @@ -3919,6 +3919,88 @@ static int mwifiex_cfg80211_get_channel(struct wiphy *wiphy, return ret; } +#ifdef CONFIG_NL80211_TESTMODE + +enum mwifiex_tm_attr { + __MWIFIEX_TM_ATTR_INVALID = 0, + MWIFIEX_TM_ATTR_CMD = 1, + MWIFIEX_TM_ATTR_DATA = 2, + + /* keep last */ + __MWIFIEX_TM_ATTR_AFTER_LAST, + MWIFIEX_TM_ATTR_MAX = __MWIFIEX_TM_ATTR_AFTER_LAST - 1, +}; + +static const struct nla_policy mwifiex_tm_policy[MWIFIEX_TM_ATTR_MAX + 1] = { + [MWIFIEX_TM_ATTR_CMD] = { .type = NLA_U32 }, + [MWIFIEX_TM_ATTR_DATA] = { .type = NLA_BINARY, + .len = MWIFIEX_SIZE_OF_CMD_BUFFER }, +}; + +enum mwifiex_tm_command { + MWIFIEX_TM_CMD_HOSTCMD = 0, +}; + +static int mwifiex_tm_cmd(struct wiphy *wiphy, struct wireless_dev *wdev, + void *data, int len) +{ + struct mwifiex_private *priv = mwifiex_netdev_get_priv(wdev->netdev); + struct mwifiex_ds_misc_cmd *hostcmd; + struct nlattr *tb[MWIFIEX_TM_ATTR_MAX + 1]; + struct mwifiex_adapter *adapter; + struct sk_buff *skb; + int err; + + if (!priv) + return -EINVAL; + adapter = priv->adapter; + + err = nla_parse(tb, MWIFIEX_TM_ATTR_MAX, data, len, + mwifiex_tm_policy); + if (err) + return err; + + if (!tb[MWIFIEX_TM_ATTR_CMD]) + return -EINVAL; + + switch (nla_get_u32(tb[MWIFIEX_TM_ATTR_CMD])) { + case MWIFIEX_TM_CMD_HOSTCMD: + if (!tb[MWIFIEX_TM_ATTR_DATA]) + return -EINVAL; + + hostcmd = kzalloc(sizeof(*hostcmd), GFP_KERNEL); + if (!hostcmd) + return -ENOMEM; + + hostcmd->len = nla_len(tb[MWIFIEX_TM_ATTR_DATA]); + memcpy(hostcmd->cmd, nla_data(tb[MWIFIEX_TM_ATTR_DATA]), + hostcmd->len); + + if (mwifiex_send_cmd(priv, 0, 0, 0, hostcmd, true)) { + dev_err(priv->adapter->dev, "Failed to process hostcmd\n"); + return -EFAULT; + } + + /* process hostcmd response*/ + skb = cfg80211_testmode_alloc_reply_skb(wiphy, hostcmd->len); + if (!skb) + return -ENOMEM; + err = nla_put(skb, MWIFIEX_TM_ATTR_DATA, + hostcmd->len, hostcmd->cmd); + if (err) { + kfree_skb(skb); + return -EMSGSIZE; + } + + err = cfg80211_testmode_reply(skb); + kfree(hostcmd); + return err; + default: + return -EOPNOTSUPP; + } +} +#endif + static int mwifiex_cfg80211_start_radar_detection(struct wiphy *wiphy, struct net_device *dev, @@ -4031,6 +4113,7 @@ static struct cfg80211_ops mwifiex_cfg80211_ops = { .tdls_cancel_channel_switch = mwifiex_cfg80211_tdls_cancel_chan_switch, .add_station = mwifiex_cfg80211_add_station, .change_station = mwifiex_cfg80211_change_station, + CFG80211_TESTMODE_CMD(mwifiex_tm_cmd) .get_channel = mwifiex_cfg80211_get_channel, .start_radar_detection = mwifiex_cfg80211_start_radar_detection, .channel_switch = mwifiex_cfg80211_channel_switch,