cfg80211: Add support for QoS mapping

This allows QoS mapping from external networks to be implemented as
defined in IEEE Std 802.11-2012, 10.24.9. APs can use this to advertise
DSCP ranges and exceptions for mapping frames to a specific UP over
Wi-Fi.

The payload of the QoS Map Set element (IEEE Std 802.11-2012, 8.4.2.97)
is sent to the driver through the new NL80211_ATTR_QOS_MAP attribute to
configure the local behavior either on the AP (based on local
configuration) or on a station (based on information received from the
AP).

Signed-off-by: Kyeyoon Park <kyeyoonp@qca.qualcomm.com>
Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

1 files changed, 62 insertions, 0 deletions

diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 8a7ff041349b..b4f40fe84a01 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -382,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 */
@@ -1456,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);
@@ -9121,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
@@ -9853,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 */