1 files changed, 141 insertions, 19 deletions

diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 04681a46eda8..b4f40fe84a01 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -53,6 +53,7 @@ enum nl80211_multicast_groups {
         NL80211_MCGRP_SCAN,
         NL80211_MCGRP_REGULATORY,
         NL80211_MCGRP_MLME,
+        NL80211_MCGRP_VENDOR,
         NL80211_MCGRP_TESTMODE /* keep last - ifdef! */
 };
 
@@ -61,6 +62,7 @@ static const struct genl_multicast_group nl80211_mcgrps[] = {
         [NL80211_MCGRP_SCAN] = { .name = "scan", },
         [NL80211_MCGRP_REGULATORY] = { .name = "regulatory", },
         [NL80211_MCGRP_MLME] = { .name = "mlme", },
+        [NL80211_MCGRP_VENDOR] = { .name = "vendor", },
 #ifdef CONFIG_NL80211_TESTMODE
         [NL80211_MCGRP_TESTMODE] = { .name = "testmode", }
 #endif
@@ -380,6 +382,8 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = {
         [NL80211_ATTR_VENDOR_ID] = { .type = NLA_U32 },
         [NL80211_ATTR_VENDOR_SUBCMD] = { .type = NLA_U32 },
         [NL80211_ATTR_VENDOR_DATA] = { .type = NLA_BINARY },
+        [NL80211_ATTR_QOS_MAP] = { .type = NLA_BINARY,
+                                   .len = IEEE80211_QOS_MAP_LEN_MAX },
 };
 
 /* policy for the key attributes */
@@ -1188,7 +1192,6 @@ 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;
@@ -1455,6 +1458,7 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *dev,
                         if (dev->wiphy.flags & WIPHY_FLAG_HAS_CHANNEL_SWITCH)
                                 CMD(channel_switch, CHANNEL_SWITCH);
                 }
+                CMD(set_qos_map, SET_QOS_MAP);
 
 #ifdef CONFIG_NL80211_TESTMODE
                 CMD(testmode_cmd, TESTMODE);
@@ -1587,16 +1591,38 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *dev,
                 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;
+                if (dev->wiphy.n_vendor_commands) {
+                        const struct nl80211_vendor_cmd_info *info;
+                        struct nlattr *nested;
+
+                        nested = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+                        if (!nested)
+                                goto nla_put_failure;
+
+                        for (i = 0; i < dev->wiphy.n_vendor_commands; i++) {
+                                info = &dev->wiphy.vendor_commands[i].info;
+                                if (nla_put(msg, i + 1, sizeof(*info), info))
+                                        goto nla_put_failure;
+                        }
+                        nla_nest_end(msg, nested);
+                }
+
+                if (dev->wiphy.n_vendor_events) {
+                        const struct nl80211_vendor_cmd_info *info;
+                        struct nlattr *nested;
 
-                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))
+                        nested = nla_nest_start(msg,
+                                                NL80211_ATTR_VENDOR_EVENTS);
+                        if (!nested)
                                 goto nla_put_failure;
-                nla_nest_end(msg, nl_vendor_cmds);
+
+                        for (i = 0; i < dev->wiphy.n_vendor_events; i++) {
+                                info = &dev->wiphy.vendor_events[i];
+                                if (nla_put(msg, i + 1, sizeof(*info), info))
+                                        goto nla_put_failure;
+                        }
+                        nla_nest_end(msg, nested);
+                }
 
                 /* done */
                 state->split_start = 0;
@@ -6726,7 +6752,9 @@ 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)
+                            enum nl80211_attrs attr,
+                            const struct nl80211_vendor_cmd_info *info,
+                            gfp_t gfp)
 {
         struct sk_buff *skb;
         void *hdr;
@@ -6744,6 +6772,16 @@ __cfg80211_alloc_vendor_skb(struct cfg80211_registered_device *rdev,
 
         if (nla_put_u32(skb, NL80211_ATTR_WIPHY, rdev->wiphy_idx))
                 goto nla_put_failure;
+
+        if (info) {
+                if (nla_put_u32(skb, NL80211_ATTR_VENDOR_ID,
+                                info->vendor_id))
+                        goto nla_put_failure;
+                if (nla_put_u32(skb, NL80211_ATTR_VENDOR_SUBCMD,
+                                info->subcmd))
+                        goto nla_put_failure;
+        }
+
         data = nla_nest_start(skb, attr);
 
         ((void **)skb->cb)[0] = rdev;
@@ -6884,29 +6922,54 @@ static int nl80211_testmode_dump(struct sk_buff *skb,
         return err;
 }
 
-struct sk_buff *cfg80211_testmode_alloc_event_skb(struct wiphy *wiphy,
-                                                  int approxlen, gfp_t gfp)
+struct sk_buff *__cfg80211_alloc_event_skb(struct wiphy *wiphy,
+                                           enum nl80211_commands cmd,
+                                           enum nl80211_attrs attr,
+                                           int vendor_event_idx,
+                                           int approxlen, gfp_t gfp)
 {
         struct cfg80211_registered_device *rdev = wiphy_to_dev(wiphy);
+        const struct nl80211_vendor_cmd_info *info;
+
+        switch (cmd) {
+        case NL80211_CMD_TESTMODE:
+                if (WARN_ON(vendor_event_idx != -1))
+                        return NULL;
+                info = NULL;
+                break;
+        case NL80211_CMD_VENDOR:
+                if (WARN_ON(vendor_event_idx < 0 ||
+                            vendor_event_idx >= wiphy->n_vendor_events))
+                        return NULL;
+                info = &wiphy->vendor_events[vendor_event_idx];
+                break;
+        default:
+                WARN_ON(1);
+                return NULL;
+        }
 
         return __cfg80211_alloc_vendor_skb(rdev, approxlen, 0, 0,
-                                           NL80211_CMD_TESTMODE,
-                                           NL80211_ATTR_TESTDATA, gfp);
+                                           cmd, attr, info, gfp);
 }
-EXPORT_SYMBOL(cfg80211_testmode_alloc_event_skb);
+EXPORT_SYMBOL(__cfg80211_alloc_event_skb);
 
-void cfg80211_testmode_event(struct sk_buff *skb, gfp_t gfp)
+void __cfg80211_send_event_skb(struct sk_buff *skb, gfp_t gfp)
 {
         struct cfg80211_registered_device *rdev = ((void **)skb->cb)[0];
         void *hdr = ((void **)skb->cb)[1];
         struct nlattr *data = ((void **)skb->cb)[2];
+        enum nl80211_multicast_groups mcgrp = NL80211_MCGRP_TESTMODE;
 
         nla_nest_end(skb, data);
         genlmsg_end(skb, hdr);
+
+        if (data->nla_type == NL80211_ATTR_VENDOR_DATA)
+                mcgrp = NL80211_MCGRP_VENDOR;
+
         genlmsg_multicast_netns(&nl80211_fam, wiphy_net(&rdev->wiphy), skb, 0,
-                                NL80211_MCGRP_TESTMODE, gfp);
+                                mcgrp, gfp);
 }
-EXPORT_SYMBOL(cfg80211_testmode_event);
+EXPORT_SYMBOL(__cfg80211_send_event_skb);
 #endif
 
 static int nl80211_connect(struct sk_buff *skb, struct genl_info *info)
@@ -9039,7 +9102,7 @@ struct sk_buff *__cfg80211_alloc_reply_skb(struct wiphy *wiphy,
         return __cfg80211_alloc_vendor_skb(rdev, approxlen,
                                            rdev->cur_cmd_info->snd_portid,
                                            rdev->cur_cmd_info->snd_seq,
-                                           cmd, attr, GFP_KERNEL);
+                                           cmd, attr, NULL, GFP_KERNEL);
 }
 EXPORT_SYMBOL(__cfg80211_alloc_reply_skb);
 
@@ -9061,6 +9124,57 @@ int cfg80211_vendor_cmd_reply(struct sk_buff *skb)
 EXPORT_SYMBOL_GPL(cfg80211_vendor_cmd_reply);
 
 
+static int nl80211_set_qos_map(struct sk_buff *skb,
+                               struct genl_info *info)
+{
+        struct cfg80211_registered_device *rdev = info->user_ptr[0];
+        struct cfg80211_qos_map *qos_map = NULL;
+        struct net_device *dev = info->user_ptr[1];
+        u8 *pos, len, num_des, des_len, des;
+        int ret;
+
+        if (!rdev->ops->set_qos_map)
+                return -EOPNOTSUPP;
+
+        if (info->attrs[NL80211_ATTR_QOS_MAP]) {
+                pos = nla_data(info->attrs[NL80211_ATTR_QOS_MAP]);
+                len = nla_len(info->attrs[NL80211_ATTR_QOS_MAP]);
+
+                if (len % 2 || len < IEEE80211_QOS_MAP_LEN_MIN ||
+                    len > IEEE80211_QOS_MAP_LEN_MAX)
+                        return -EINVAL;
+
+                qos_map = kzalloc(sizeof(struct cfg80211_qos_map), GFP_KERNEL);
+                if (!qos_map)
+                        return -ENOMEM;
+
+                num_des = (len - IEEE80211_QOS_MAP_LEN_MIN) >> 1;
+                if (num_des) {
+                        des_len = num_des *
+                                sizeof(struct cfg80211_dscp_exception);
+                        memcpy(qos_map->dscp_exception, pos, des_len);
+                        qos_map->num_des = num_des;
+                        for (des = 0; des < num_des; des++) {
+                                if (qos_map->dscp_exception[des].up > 7) {
+                                        kfree(qos_map);
+                                        return -EINVAL;
+                                }
+                        }
+                        pos += des_len;
+                }
+                memcpy(qos_map->up, pos, IEEE80211_QOS_MAP_LEN_MIN);
+        }
+
+        wdev_lock(dev->ieee80211_ptr);
+        ret = nl80211_key_allowed(dev->ieee80211_ptr);
+        if (!ret)
+                ret = rdev_set_qos_map(rdev, dev, qos_map);
+        wdev_unlock(dev->ieee80211_ptr);
+
+        kfree(qos_map);
+        return ret;
+}
+
 #define NL80211_FLAG_NEED_WIPHY         0x01
 #define NL80211_FLAG_NEED_NETDEV        0x02
 #define NL80211_FLAG_NEED_RTNL          0x04
@@ -9793,6 +9907,14 @@ static const struct genl_ops nl80211_ops[] = {
                 .internal_flags = NL80211_FLAG_NEED_WIPHY |
                                   NL80211_FLAG_NEED_RTNL,
         },
+        {
+                .cmd = NL80211_CMD_SET_QOS_MAP,
+                .doit = nl80211_set_qos_map,
+                .policy = nl80211_policy,
+                .flags = GENL_ADMIN_PERM,
+                .internal_flags = NL80211_FLAG_NEED_NETDEV_UP |
+                                  NL80211_FLAG_NEED_RTNL,
+        },
 };
 
 /* notification functions */