nl80211: vendor command support

Add support for vendor-specific commands to nl80211. This is
intended to be used for really vendor-specific functionality
that can't be implemented in a generic fashion for any reason.
It's *NOT* intended to be used for any normal/generic feature
or any optimisations that could be implemented across drivers.

Currently, only vendor commands (with replies) are supported,
no dump operations or vendor-specific notifications.

Also add a function wdev_to_ieee80211_vif() to mac80211 which
is needed for mac80211-based drivers wanting to implement any
vendor commands.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 169 insertions, 68 deletions

diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index bdcf256e3628..6989989de092 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -358,6 +358,9 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = {
         [NL80211_ATTR_STA_SUPPORTED_OPER_CLASSES] = { .type = NLA_BINARY },
         [NL80211_ATTR_HANDLE_DFS] = { .type = NLA_FLAG },
         [NL80211_ATTR_OPMODE_NOTIF] = { .type = NLA_U8 },
+        [NL80211_ATTR_VENDOR_ID] = { .type = NLA_U32 },
+        [NL80211_ATTR_VENDOR_SUBCMD] = { .type = NLA_U32 },
+        [NL80211_ATTR_VENDOR_DATA] = { .type = NLA_BINARY },
 };
 
 /* policy for the key attributes */
@@ -1166,6 +1169,7 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *dev,
         struct nlattr *nl_bands, *nl_band;
         struct nlattr *nl_freqs, *nl_freq;
         struct nlattr *nl_cmds;
+        struct nlattr *nl_vendor_cmds;
         enum ieee80211_band band;
         struct ieee80211_channel *chan;
         int i;
@@ -1561,6 +1565,19 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *dev,
                     (nla_put_flag(msg, NL80211_ATTR_SUPPORT_5_MHZ) ||
                      nla_put_flag(msg, NL80211_ATTR_SUPPORT_10_MHZ)))
                         goto nla_put_failure;
+                state->split_start++;
+                break;
+        case 11:
+                nl_vendor_cmds = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+                if (!nl_vendor_cmds)
+                        goto nla_put_failure;
+
+                for (i = 0; i < dev->wiphy.n_vendor_commands; i++)
+                        if (nla_put(msg, i + 1,
+                                    sizeof(struct nl80211_vendor_cmd_info),
+                                    &dev->wiphy.vendor_commands[i].info))
+                                goto nla_put_failure;
+                nla_nest_end(msg, nl_vendor_cmds);
 
                 /* done */
                 state->split_start = 0;
@@ -6682,6 +6699,40 @@ static int nl80211_set_mcast_rate(struct sk_buff *skb, struct genl_info *info)
         return err;
 }
 
+static struct sk_buff *
+__cfg80211_alloc_vendor_skb(struct cfg80211_registered_device *rdev,
+                            int approxlen, u32 portid, u32 seq,
+                            enum nl80211_commands cmd,
+                            enum nl80211_attrs attr, 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, portid, seq, 0, cmd);
+        if (!hdr) {
+                kfree_skb(skb);
+                return NULL;
+        }
+
+        if (nla_put_u32(skb, NL80211_ATTR_WIPHY, rdev->wiphy_idx))
+                goto nla_put_failure;
+        data = nla_nest_start(skb, attr);
+
+        ((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;
+}
 
 #ifdef CONFIG_NL80211_TESTMODE
 static struct genl_multicast_group nl80211_testmode_mcgrp = {
@@ -6710,11 +6761,11 @@ static int nl80211_testmode_do(struct sk_buff *skb, struct genl_info *info)
         if (!info->attrs[NL80211_ATTR_TESTDATA])
                 return -EINVAL;
 
-        rdev->testmode_info = info;
+        rdev->cur_cmd_info = info;
         err = rdev_testmode_cmd(rdev, wdev,
                                 nla_data(info->attrs[NL80211_ATTR_TESTDATA]),
                                 nla_len(info->attrs[NL80211_ATTR_TESTDATA]));
-        rdev->testmode_info = NULL;
+        rdev->cur_cmd_info = NULL;
 
         return err;
 }
@@ -6814,77 +6865,14 @@ static int nl80211_testmode_dump(struct sk_buff *skb,
         return err;
 }
 
-static struct sk_buff *
-__cfg80211_testmode_alloc_skb(struct cfg80211_registered_device *rdev,
-                              int approxlen, u32 portid, 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, portid, seq, 0, NL80211_CMD_TESTMODE);
-        if (!hdr) {
-                kfree_skb(skb);
-                return NULL;
-        }
-
-        if (nla_put_u32(skb, NL80211_ATTR_WIPHY, rdev->wiphy_idx))
-                goto nla_put_failure;
-        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_portid,
-                                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);
+        return __cfg80211_alloc_vendor_skb(rdev, approxlen, 0, 0,
+                                           NL80211_CMD_TESTMODE,
+                                           NL80211_ATTR_TESTDATA, gfp);
 }
 EXPORT_SYMBOL(cfg80211_testmode_alloc_event_skb);
 
@@ -8867,6 +8855,111 @@ static int nl80211_crit_protocol_stop(struct sk_buff *skb,
         return 0;
 }
 
+static int nl80211_vendor_cmd(struct sk_buff *skb, struct genl_info *info)
+{
+        struct cfg80211_registered_device *rdev = info->user_ptr[0];
+        struct wireless_dev *wdev =
+                __cfg80211_wdev_from_attrs(genl_info_net(info), info->attrs);
+        int i, err;
+        u32 vid, subcmd;
+
+        if (!rdev->wiphy.vendor_commands)
+                return -EOPNOTSUPP;
+
+        if (IS_ERR(wdev)) {
+                err = PTR_ERR(wdev);
+                if (err != -EINVAL)
+                        return err;
+                wdev = NULL;
+        } else if (wdev->wiphy != &rdev->wiphy) {
+                return -EINVAL;
+        }
+
+        if (!info->attrs[NL80211_ATTR_VENDOR_ID] ||
+            !info->attrs[NL80211_ATTR_VENDOR_SUBCMD])
+                return -EINVAL;
+
+        vid = nla_get_u32(info->attrs[NL80211_ATTR_VENDOR_ID]);
+        subcmd = nla_get_u32(info->attrs[NL80211_ATTR_VENDOR_SUBCMD]);
+        for (i = 0; i < rdev->wiphy.n_vendor_commands; i++) {
+                const struct wiphy_vendor_command *vcmd;
+                void *data = NULL;
+                int len = 0;
+
+                vcmd = &rdev->wiphy.vendor_commands[i];
+
+                if (vcmd->info.vendor_id != vid || vcmd->info.subcmd != subcmd)
+                        continue;
+
+                if (vcmd->flags & (WIPHY_VENDOR_CMD_NEED_WDEV |
+                                   WIPHY_VENDOR_CMD_NEED_NETDEV)) {
+                        if (!wdev)
+                                return -EINVAL;
+                        if (vcmd->flags & WIPHY_VENDOR_CMD_NEED_NETDEV &&
+                            !wdev->netdev)
+                                return -EINVAL;
+
+                        if (vcmd->flags & WIPHY_VENDOR_CMD_NEED_RUNNING) {
+                                if (wdev->netdev &&
+                                    !netif_running(wdev->netdev))
+                                        return -ENETDOWN;
+                                if (!wdev->netdev && !wdev->p2p_started)
+                                        return -ENETDOWN;
+                        }
+                } else {
+                        wdev = NULL;
+                }
+
+                if (info->attrs[NL80211_ATTR_VENDOR_DATA]) {
+                        data = nla_data(info->attrs[NL80211_ATTR_VENDOR_DATA]);
+                        len = nla_len(info->attrs[NL80211_ATTR_VENDOR_DATA]);
+                }
+
+                rdev->cur_cmd_info = info;
+                err = rdev->wiphy.vendor_commands[i].doit(&rdev->wiphy, wdev,
+                                                          data, len);
+                rdev->cur_cmd_info = NULL;
+                return err;
+        }
+
+        return -EOPNOTSUPP;
+}
+
+struct sk_buff *__cfg80211_alloc_reply_skb(struct wiphy *wiphy,
+                                           enum nl80211_commands cmd,
+                                           enum nl80211_attrs attr,
+                                           int approxlen)
+{
+        struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+
+        if (WARN_ON(!rdev->cur_cmd_info))
+                return NULL;
+
+        return __cfg80211_alloc_vendor_skb(rdev, approxlen,
+                                           rdev->cur_cmd_info->snd_portid,
+                                           rdev->cur_cmd_info->snd_seq,
+                                           cmd, attr, GFP_KERNEL);
+}
+EXPORT_SYMBOL(__cfg80211_alloc_reply_skb);
+
+int cfg80211_vendor_cmd_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->cur_cmd_info)) {
+                kfree_skb(skb);
+                return -EINVAL;
+        }
+
+        nla_nest_end(skb, data);
+        genlmsg_end(skb, hdr);
+        return genlmsg_reply(skb, rdev->cur_cmd_info);
+}
+EXPORT_SYMBOL_GPL(cfg80211_vendor_cmd_reply);
+
+
 #define NL80211_FLAG_NEED_WIPHY         0x01
 #define NL80211_FLAG_NEED_NETDEV        0x02
 #define NL80211_FLAG_NEED_RTNL          0x04
@@ -9591,6 +9684,14 @@ static struct genl_ops nl80211_ops[] = {
                 .internal_flags = NL80211_FLAG_NEED_NETDEV_UP |
                                   NL80211_FLAG_NEED_RTNL,
         },
+        {
+                .cmd = NL80211_CMD_VENDOR,
+                .doit = nl80211_vendor_cmd,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+                .internal_flags = NL80211_FLAG_NEED_WIPHY |
+                                  NL80211_FLAG_NEED_RTNL,
+        },
 };
 
 static struct genl_multicast_group nl80211_mlme_mcgrp = {