1 files changed, 136 insertions, 0 deletions

diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 01523ba81baf..bb8de268a6bf 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -3416,6 +3416,128 @@ unlock_rtnl:
         return err;
 }
 
+#ifdef CONFIG_NL80211_TESTMODE
+static struct genl_multicast_group nl80211_testmode_mcgrp = {
+        .name = "testmode",
+};
+
+static int nl80211_testmode_do(struct sk_buff *skb, struct genl_info *info)
+{
+        struct cfg80211_registered_device *rdev;
+        int err;
+
+        if (!info->attrs[NL80211_ATTR_TESTDATA])
+                return -EINVAL;
+
+        rtnl_lock();
+
+        rdev = cfg80211_get_dev_from_info(info);
+        if (IS_ERR(rdev)) {
+                err = PTR_ERR(rdev);
+                goto unlock_rtnl;
+        }
+
+        err = -EOPNOTSUPP;
+        if (rdev->ops->testmode_cmd) {
+                rdev->testmode_info = info;
+                err = rdev->ops->testmode_cmd(&rdev->wiphy,
+                                nla_data(info->attrs[NL80211_ATTR_TESTDATA]),
+                                nla_len(info->attrs[NL80211_ATTR_TESTDATA]));
+                rdev->testmode_info = NULL;
+        }
+
+        cfg80211_put_dev(rdev);
+
+ unlock_rtnl:
+        rtnl_unlock();
+        return err;
+}
+
+static struct sk_buff *
+__cfg80211_testmode_alloc_skb(struct cfg80211_registered_device *rdev,
+                              int approxlen, u32 pid, u32 seq, gfp_t gfp)
+{
+        struct sk_buff *skb;
+        void *hdr;
+        struct nlattr *data;
+
+        skb = nlmsg_new(approxlen + 100, gfp);
+        if (!skb)
+                return NULL;
+
+        hdr = nl80211hdr_put(skb, pid, seq, 0, NL80211_CMD_TESTMODE);
+        if (!hdr) {
+                kfree_skb(skb);
+                return NULL;
+        }
+
+        NLA_PUT_U32(skb, NL80211_ATTR_WIPHY, rdev->wiphy_idx);
+        data = nla_nest_start(skb, NL80211_ATTR_TESTDATA);
+
+        ((void **)skb->cb)[0] = rdev;
+        ((void **)skb->cb)[1] = hdr;
+        ((void **)skb->cb)[2] = data;
+
+        return skb;
+
+ nla_put_failure:
+        kfree_skb(skb);
+        return NULL;
+}
+
+struct sk_buff *cfg80211_testmode_alloc_reply_skb(struct wiphy *wiphy,
+                                                  int approxlen)
+{
+        struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+
+        if (WARN_ON(!rdev->testmode_info))
+                return NULL;
+
+        return __cfg80211_testmode_alloc_skb(rdev, approxlen,
+                                rdev->testmode_info->snd_pid,
+                                rdev->testmode_info->snd_seq,
+                                GFP_KERNEL);
+}
+EXPORT_SYMBOL(cfg80211_testmode_alloc_reply_skb);
+
+int cfg80211_testmode_reply(struct sk_buff *skb)
+{
+        struct cfg80211_registered_device *rdev = ((void **)skb->cb)[0];
+        void *hdr = ((void **)skb->cb)[1];
+        struct nlattr *data = ((void **)skb->cb)[2];
+
+        if (WARN_ON(!rdev->testmode_info)) {
+                kfree_skb(skb);
+                return -EINVAL;
+        }
+
+        nla_nest_end(skb, data);
+        genlmsg_end(skb, hdr);
+        return genlmsg_reply(skb, rdev->testmode_info);
+}
+EXPORT_SYMBOL(cfg80211_testmode_reply);
+
+struct sk_buff *cfg80211_testmode_alloc_event_skb(struct wiphy *wiphy,
+                                                  int approxlen, gfp_t gfp)
+{
+        struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+
+        return __cfg80211_testmode_alloc_skb(rdev, approxlen, 0, 0, gfp);
+}
+EXPORT_SYMBOL(cfg80211_testmode_alloc_event_skb);
+
+void cfg80211_testmode_event(struct sk_buff *skb, gfp_t gfp)
+{
+        void *hdr = ((void **)skb->cb)[1];
+        struct nlattr *data = ((void **)skb->cb)[2];
+
+        nla_nest_end(skb, data);
+        genlmsg_end(skb, hdr);
+        genlmsg_multicast(skb, 0, nl80211_testmode_mcgrp.id, gfp);
+}
+EXPORT_SYMBOL(cfg80211_testmode_event);
+#endif
+
 static struct genl_ops nl80211_ops[] = {
         {
                 .cmd = NL80211_CMD_GET_WIPHY,
@@ -3629,6 +3751,14 @@ static struct genl_ops nl80211_ops[] = {
                 .policy = nl80211_policy,
                 .flags = GENL_ADMIN_PERM,
         },
+#ifdef CONFIG_NL80211_TESTMODE
+        {
+                .cmd = NL80211_CMD_TESTMODE,
+                .doit = nl80211_testmode_do,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+        },
+#endif
 };
 static struct genl_multicast_group nl80211_mlme_mcgrp = {
         .name = "mlme",
@@ -4102,6 +4232,12 @@ int nl80211_init(void)
         if (err)
                 goto err_out;
 
+#ifdef CONFIG_NL80211_TESTMODE
+        err = genl_register_mc_group(&nl80211_fam, &nl80211_testmode_mcgrp);
+        if (err)
+                goto err_out;
+#endif
+
         return 0;
  err_out:
         genl_unregister_family(&nl80211_fam);