nl80211: add feature for BSS selection support

Introducing a new feature that the driver can use to
indicate the driver/firmware supports configuration of BSS
selection criteria upon CONNECT command. This can be useful
when multiple BSS-es are found belonging to the same ESS,
ie. Infra-BSS with same SSID. The criteria can then be used to
offload selection of a preferred BSS.

Reviewed-by: Hante Meuleman <meuleman@broadcom.com>
Reviewed-by: Franky (Zhenhui) Lin <frankyl@broadcom.com>
Reviewed-by: Pieter-Paul Giesberts <pieterpg@broadcom.com>
Reviewed-by: Lei Zhang <leizh@broadcom.com>
Signed-off-by: Arend van Spriel <arend@broadcom.com>
[move wiphy support check into parse_bss_select()]
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

2 files changed, 118 insertions, 0 deletions

diff --git a/net/wireless/core.c b/net/wireless/core.c
index 9f1c4aa851ef..5327e4b974fa 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -626,6 +626,13 @@ int wiphy_register(struct wiphy *wiphy)
                      !rdev->ops->set_mac_acl)))
                 return -EINVAL;
 
+        /* assure only valid behaviours are flagged by driver
+         * hence subtract 2 as bit 0 is invalid.
+         */
+        if (WARN_ON(wiphy->bss_select_support &&
+                    (wiphy->bss_select_support & ~(BIT(__NL80211_BSS_SELECT_ATTR_AFTER_LAST) - 2))))
+                return -EINVAL;
+
         if (wiphy->addresses)
                 memcpy(wiphy->perm_addr, wiphy->addresses[0].addr, ETH_ALEN);
 
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 1b43f7839eeb..d6c6449c0389 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -402,6 +402,7 @@ static const struct nla_policy nl80211_policy[NUM_NL80211_ATTR] = {
         [NL80211_ATTR_SCHED_SCAN_DELAY] = { .type = NLA_U32 },
         [NL80211_ATTR_REG_INDOOR] = { .type = NLA_FLAG },
         [NL80211_ATTR_PBSS] = { .type = NLA_FLAG },
+        [NL80211_ATTR_BSS_SELECT] = { .type = NLA_NESTED },
 };
 
 /* policy for the key attributes */
@@ -486,6 +487,15 @@ nl80211_plan_policy[NL80211_SCHED_SCAN_PLAN_MAX + 1] = {
         [NL80211_SCHED_SCAN_PLAN_ITERATIONS] = { .type = NLA_U32 },
 };
 
+static const struct nla_policy
+nl80211_bss_select_policy[NL80211_BSS_SELECT_ATTR_MAX + 1] = {
+        [NL80211_BSS_SELECT_ATTR_RSSI] = { .type = NLA_FLAG },
+        [NL80211_BSS_SELECT_ATTR_BAND_PREF] = { .type = NLA_U32 },
+        [NL80211_BSS_SELECT_ATTR_RSSI_ADJUST] = {
+                .len = sizeof(struct nl80211_bss_select_rssi_adjust)
+        },
+};
+
 static int nl80211_prepare_wdev_dump(struct sk_buff *skb,
                                      struct netlink_callback *cb,
                                      struct cfg80211_registered_device **rdev,
@@ -1731,6 +1741,25 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *rdev,
                             rdev->wiphy.ext_features))
                         goto nla_put_failure;
 
+                if (rdev->wiphy.bss_select_support) {
+                        struct nlattr *nested;
+                        u32 bss_select_support = rdev->wiphy.bss_select_support;
+
+                        nested = nla_nest_start(msg, NL80211_ATTR_BSS_SELECT);
+                        if (!nested)
+                                goto nla_put_failure;
+
+                        i = 0;
+                        while (bss_select_support) {
+                                if ((bss_select_support & 1) &&
+                                    nla_put_flag(msg, i))
+                                        goto nla_put_failure;
+                                i++;
+                                bss_select_support >>= 1;
+                        }
+                        nla_nest_end(msg, nested);
+                }
+
                 /* done */
                 state->split_start = 0;
                 break;
@@ -5758,6 +5787,73 @@ static int validate_scan_freqs(struct nlattr *freqs)
         return n_channels;
 }
 
+static bool is_band_valid(struct wiphy *wiphy, enum ieee80211_band b)
+{
+        return b < IEEE80211_NUM_BANDS && wiphy->bands[b];
+}
+
+static int parse_bss_select(struct nlattr *nla, struct wiphy *wiphy,
+                            struct cfg80211_bss_selection *bss_select)
+{
+        struct nlattr *attr[NL80211_BSS_SELECT_ATTR_MAX + 1];
+        struct nlattr *nest;
+        int err;
+        bool found = false;
+        int i;
+
+        /* only process one nested attribute */
+        nest = nla_data(nla);
+        if (!nla_ok(nest, nla_len(nest)))
+                return -EINVAL;
+
+        err = nla_parse(attr, NL80211_BSS_SELECT_ATTR_MAX, nla_data(nest),
+                        nla_len(nest), nl80211_bss_select_policy);
+        if (err)
+                return err;
+
+        /* only one attribute may be given */
+        for (i = 0; i <= NL80211_BSS_SELECT_ATTR_MAX; i++) {
+                if (attr[i]) {
+                        if (found)
+                                return -EINVAL;
+                        found = true;
+                }
+        }
+
+        bss_select->behaviour = __NL80211_BSS_SELECT_ATTR_INVALID;
+
+        if (attr[NL80211_BSS_SELECT_ATTR_RSSI])
+                bss_select->behaviour = NL80211_BSS_SELECT_ATTR_RSSI;
+
+        if (attr[NL80211_BSS_SELECT_ATTR_BAND_PREF]) {
+                bss_select->behaviour = NL80211_BSS_SELECT_ATTR_BAND_PREF;
+                bss_select->param.band_pref =
+                        nla_get_u32(attr[NL80211_BSS_SELECT_ATTR_BAND_PREF]);
+                if (!is_band_valid(wiphy, bss_select->param.band_pref))
+                        return -EINVAL;
+        }
+
+        if (attr[NL80211_BSS_SELECT_ATTR_RSSI_ADJUST]) {
+                struct nl80211_bss_select_rssi_adjust *adj_param;
+
+                adj_param = nla_data(attr[NL80211_BSS_SELECT_ATTR_RSSI_ADJUST]);
+                bss_select->behaviour = NL80211_BSS_SELECT_ATTR_RSSI_ADJUST;
+                bss_select->param.adjust.band = adj_param->band;
+                bss_select->param.adjust.delta = adj_param->delta;
+                if (!is_band_valid(wiphy, bss_select->param.adjust.band))
+                        return -EINVAL;
+        }
+
+        /* user-space did not provide behaviour attribute */
+        if (bss_select->behaviour == __NL80211_BSS_SELECT_ATTR_INVALID)
+                return -EINVAL;
+
+        if (!(wiphy->bss_select_support & BIT(bss_select->behaviour)))
+                return -EINVAL;
+
+        return 0;
+}
+
 static int nl80211_parse_random_mac(struct nlattr **attrs,
                                     u8 *mac_addr, u8 *mac_addr_mask)
 {
@@ -8001,6 +8097,21 @@ static int nl80211_connect(struct sk_buff *skb, struct genl_info *info)
                 return -EOPNOTSUPP;
         }
 
+        if (info->attrs[NL80211_ATTR_BSS_SELECT]) {
+                /* bss selection makes no sense if bssid is set */
+                if (connect.bssid) {
+                        kzfree(connkeys);
+                        return -EINVAL;
+                }
+
+                err = parse_bss_select(info->attrs[NL80211_ATTR_BSS_SELECT],
+                                       wiphy, &connect.bss_select);
+                if (err) {
+                        kzfree(connkeys);
+                        return err;
+                }
+        }
+
         wdev_lock(dev->ieee80211_ptr);
         err = cfg80211_connect(rdev, dev, &connect, connkeys, NULL);
         wdev_unlock(dev->ieee80211_ptr);