mac80211: Add support for QoS mapping

Implement set_qos_map() handler for mac80211 to enable QoS mapping
functionality.

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, 26 insertions, 0 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index aab3c2f29de3..09d2e58a2ba7 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -3837,6 +3837,31 @@ static void ieee80211_set_wakeup(struct wiphy *wiphy, bool enabled)
 }
 #endif
 
+static int ieee80211_set_qos_map(struct wiphy *wiphy,
+                                 struct net_device *dev,
+                                 struct cfg80211_qos_map *qos_map)
+{
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
+        struct mac80211_qos_map *new_qos_map, *old_qos_map;
+
+        if (qos_map) {
+                new_qos_map = kzalloc(sizeof(*new_qos_map), GFP_KERNEL);
+                if (!new_qos_map)
+                        return -ENOMEM;
+                memcpy(&new_qos_map->qos_map, qos_map, sizeof(*qos_map));
+        } else {
+                /* A NULL qos_map was passed to disable QoS mapping */
+                new_qos_map = NULL;
+        }
+
+        old_qos_map = rtnl_dereference(sdata->qos_map);
+        rcu_assign_pointer(sdata->qos_map, new_qos_map);
+        if (old_qos_map)
+                kfree_rcu(old_qos_map, rcu_head);
+
+        return 0;
+}
+
 struct cfg80211_ops mac80211_config_ops = {
         .add_virtual_intf = ieee80211_add_iface,
         .del_virtual_intf = ieee80211_del_iface,
@@ -3916,4 +3941,5 @@ struct cfg80211_ops mac80211_config_ops = {
         .get_channel = ieee80211_cfg_get_channel,
         .start_radar_detection = ieee80211_start_radar_detection,
         .channel_switch = ieee80211_channel_switch,
+        .set_qos_map = ieee80211_set_qos_map,
 };