mwifiex: add cfg80211 testmode support

This patch adds cfg80211 testmode support so that userspace tools can
download necessary commands to firmware during manufacturing mode tests.

Signed-off-by: Xinming Hu <huxm@marvell.com>
Signed-off-by: Amitkumar Karwar <akarwar@marvell.com>
Signed-off-by: Kalle Valo <kvalo@codeaurora.org>

diff --git a/drivers/net/wireless/marvell/mwifiex/cfg80211.c b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
index 876d420e936ec4cb309d919569fee87ec78b63c2..0a03d3f90b566fcc9e447272765d6a7b17778344 100644 (file)
--- 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,