nl80211/cfg80211: add match filtering for sched_scan

[GitHub/mt8127/android_kernel_alcatel_ttab.git] / net / wireless / nl80211.c

diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 0cda46ab35e5949ac23843071a4a769c546d3b77..f4cfd3abfbfd28af654c5a6c10d15f727b7aea6e 100644 (file)
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -190,6 +190,7 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = {
        [NL80211_ATTR_IE_ASSOC_RESP] = { .type = NLA_BINARY,
                                         .len = IEEE80211_MAX_DATA_LEN },
        [NL80211_ATTR_ROAM_SUPPORT] = { .type = NLA_FLAG },
+       [NL80211_ATTR_SCHED_SCAN_MATCH] = { .type = NLA_NESTED },
 };
 
 /* policy for the key attributes */
@@ -232,6 +233,12 @@ nl80211_rekey_policy[NUM_NL80211_REKEY_DATA] = {
        [NL80211_REKEY_DATA_REPLAY_CTR] = { .len = NL80211_REPLAY_CTR_LEN },
 };
 
+static const struct nla_policy
+nl80211_match_policy[NL80211_SCHED_SCAN_MATCH_ATTR_MAX + 1] = {
+       [NL80211_ATTR_SCHED_SCAN_MATCH_SSID] = { .type = NLA_BINARY,
+                                                .len = IEEE80211_MAX_SSID_LEN },
+};
+
 /* ifidx get helper */
 static int nl80211_get_ifidx(struct netlink_callback *cb)
 {
@@ -715,6 +722,8 @@ static int nl80211_send_wiphy(struct sk_buff *msg, u32 pid, u32 seq, int flags,
                    dev->wiphy.max_scan_ie_len);
        NLA_PUT_U16(msg, NL80211_ATTR_MAX_SCHED_SCAN_IE_LEN,
                    dev->wiphy.max_sched_scan_ie_len);
+       NLA_PUT_U8(msg, NL80211_ATTR_MAX_MATCH_SETS,
+                  dev->wiphy.max_match_sets);
 
        if (dev->wiphy.flags & WIPHY_FLAG_IBSS_RSN)
                NLA_PUT_FLAG(msg, NL80211_ATTR_SUPPORT_IBSS_RSN);
@@ -3632,10 +3641,11 @@ static int nl80211_start_sched_scan(struct sk_buff *skb,
        struct net_device *dev = info->user_ptr[1];
        struct nlattr *attr;
        struct wiphy *wiphy;
-       int err, tmp, n_ssids = 0, n_channels, i;
+       int err, tmp, n_ssids = 0, n_match_sets = 0, n_channels, i;
        u32 interval;
        enum ieee80211_band band;
        size_t ie_len;
+       struct nlattr *tb[NL80211_SCHED_SCAN_MATCH_ATTR_MAX + 1];
 
        if (!(rdev->wiphy.flags & WIPHY_FLAG_SUPPORTS_SCHED_SCAN) ||
            !rdev->ops->sched_scan_start)
@@ -3674,6 +3684,15 @@ static int nl80211_start_sched_scan(struct sk_buff *skb,
        if (n_ssids > wiphy->max_sched_scan_ssids)
                return -EINVAL;
 
+       if (info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH])
+               nla_for_each_nested(attr,
+                                   info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH],
+                                   tmp)
+                       n_match_sets++;
+
+       if (n_match_sets > wiphy->max_match_sets)
+               return -EINVAL;
+
        if (info->attrs[NL80211_ATTR_IE])
                ie_len = nla_len(info->attrs[NL80211_ATTR_IE]);
        else
@@ -3691,6 +3710,7 @@ static int nl80211_start_sched_scan(struct sk_buff *skb,
 
        request = kzalloc(sizeof(*request)
                        + sizeof(*request->ssids) * n_ssids
+                       + sizeof(*request->match_sets) * n_match_sets
                        + sizeof(*request->channels) * n_channels
                        + ie_len, GFP_KERNEL);
        if (!request) {
@@ -3708,6 +3728,18 @@ static int nl80211_start_sched_scan(struct sk_buff *skb,
                        request->ie = (void *)(request->channels + n_channels);
        }
 
+       if (n_match_sets) {
+               if (request->ie)
+                       request->match_sets = (void *)(request->ie + ie_len);
+               else if (request->ssids)
+                       request->match_sets =
+                               (void *)(request->ssids + n_ssids);
+               else
+                       request->match_sets =
+                               (void *)(request->channels + n_channels);
+       }
+       request->n_match_sets = n_match_sets;
+
        i = 0;
        if (info->attrs[NL80211_ATTR_SCAN_FREQUENCIES]) {
                /* user specified, bail out if channel not found */
@@ -3772,6 +3804,31 @@ static int nl80211_start_sched_scan(struct sk_buff *skb,
                }
        }
 
+       i = 0;
+       if (info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH]) {
+               nla_for_each_nested(attr,
+                                   info->attrs[NL80211_ATTR_SCHED_SCAN_MATCH],
+                                   tmp) {
+                       struct nlattr *ssid;
+
+                       nla_parse(tb, NL80211_SCHED_SCAN_MATCH_ATTR_MAX,
+                                 nla_data(attr), nla_len(attr),
+                                 nl80211_match_policy);
+                       ssid = tb[NL80211_ATTR_SCHED_SCAN_MATCH_SSID];
+                       if (ssid) {
+                               if (nla_len(ssid) > IEEE80211_MAX_SSID_LEN) {
+                                       err = -EINVAL;
+                                       goto out_free;
+                               }
+                               memcpy(request->match_sets[i].ssid.ssid,
+                                      nla_data(ssid), nla_len(ssid));
+                               request->match_sets[i].ssid.ssid_len =
+                                       nla_len(ssid);
+                       }
+                       i++;
+               }
+       }
+
        if (info->attrs[NL80211_ATTR_IE]) {
                request->ie_len = nla_len(info->attrs[NL80211_ATTR_IE]);
                memcpy((void *)request->ie,