cfg80211: add ability to override VHT capabilities

For testing it's sometimes useful to be able to
override certain VHT capability advertisement,
add the ability to do that in cfg80211.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>

4 files changed, 88 insertions, 8 deletions

diff --git a/net/wireless/core.h b/net/wireless/core.h
index 3aec0e429d8..c2f94f22bb2 100644
--- a/net/wireless/core.h
+++ b/net/wireless/core.h
@@ -335,7 +335,9 @@ int __cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			  const u8 *ie, int ie_len, bool use_mfp,
 			  struct cfg80211_crypto_settings *crypt,
 			  u32 assoc_flags, struct ieee80211_ht_cap *ht_capa,
-			  struct ieee80211_ht_cap *ht_capa_mask);
+			  struct ieee80211_ht_cap *ht_capa_mask,
+			  struct ieee80211_vht_cap *vht_capa,
+			  struct ieee80211_vht_cap *vht_capa_mask);
 int cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			struct net_device *dev, struct ieee80211_channel *chan,
 			const u8 *bssid, const u8 *prev_bssid,
@@ -343,7 +345,9 @@ int cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			const u8 *ie, int ie_len, bool use_mfp,
 			struct cfg80211_crypto_settings *crypt,
 			u32 assoc_flags, struct ieee80211_ht_cap *ht_capa,
-			struct ieee80211_ht_cap *ht_capa_mask);
+			struct ieee80211_ht_cap *ht_capa_mask,
+			struct ieee80211_vht_cap *vht_capa,
+			struct ieee80211_vht_cap *vht_capa_mask);
 int __cfg80211_mlme_deauth(struct cfg80211_registered_device *rdev,
 			   struct net_device *dev, const u8 *bssid,
 			   const u8 *ie, int ie_len, u16 reason,
@@ -375,6 +379,8 @@ int cfg80211_mlme_mgmt_tx(struct cfg80211_registered_device *rdev,
 			  bool no_cck, bool dont_wait_for_ack, u64 *cookie);
 void cfg80211_oper_and_ht_capa(struct ieee80211_ht_cap *ht_capa,
 			       const struct ieee80211_ht_cap *ht_capa_mask);
+void cfg80211_oper_and_vht_capa(struct ieee80211_vht_cap *vht_capa,
+				const struct ieee80211_vht_cap *vht_capa_mask);
 
 /* SME */
 int __cfg80211_connect(struct cfg80211_registered_device *rdev,
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index 5a97ce6d283..c82adfee769 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -343,6 +343,23 @@ void cfg80211_oper_and_ht_capa(struct ieee80211_ht_cap *ht_capa,
 		p1[i] &= p2[i];
 }
 
+/*  Do a logical ht_capa &= ht_capa_mask.  */
+void cfg80211_oper_and_vht_capa(struct ieee80211_vht_cap *vht_capa,
+				const struct ieee80211_vht_cap *vht_capa_mask)
+{
+	int i;
+	u8 *p1, *p2;
+	if (!vht_capa_mask) {
+		memset(vht_capa, 0, sizeof(*vht_capa));
+		return;
+	}
+
+	p1 = (u8*)(vht_capa);
+	p2 = (u8*)(vht_capa_mask);
+	for (i = 0; i < sizeof(*vht_capa); i++)
+		p1[i] &= p2[i];
+}
+
 int __cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			  struct net_device *dev,
 			  struct ieee80211_channel *chan,
@@ -351,7 +368,9 @@ int __cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			  const u8 *ie, int ie_len, bool use_mfp,
 			  struct cfg80211_crypto_settings *crypt,
 			  u32 assoc_flags, struct ieee80211_ht_cap *ht_capa,
-			  struct ieee80211_ht_cap *ht_capa_mask)
+			  struct ieee80211_ht_cap *ht_capa_mask,
+			  struct ieee80211_vht_cap *vht_capa,
+			  struct ieee80211_vht_cap *vht_capa_mask)
 {
 	struct wireless_dev *wdev = dev->ieee80211_ptr;
 	struct cfg80211_assoc_request req;
@@ -388,6 +407,13 @@ int __cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 		       sizeof(req.ht_capa_mask));
 	cfg80211_oper_and_ht_capa(&req.ht_capa_mask,
 				  rdev->wiphy.ht_capa_mod_mask);
+	if (vht_capa)
+		memcpy(&req.vht_capa, vht_capa, sizeof(req.vht_capa));
+	if (vht_capa_mask)
+		memcpy(&req.vht_capa_mask, vht_capa_mask,
+		       sizeof(req.vht_capa_mask));
+	cfg80211_oper_and_vht_capa(&req.vht_capa_mask,
+				   rdev->wiphy.vht_capa_mod_mask);
 
 	req.bss = cfg80211_get_bss(&rdev->wiphy, chan, bssid, ssid, ssid_len,
 				   WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
@@ -422,7 +448,9 @@ int cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 			const u8 *ie, int ie_len, bool use_mfp,
 			struct cfg80211_crypto_settings *crypt,
 			u32 assoc_flags, struct ieee80211_ht_cap *ht_capa,
-			struct ieee80211_ht_cap *ht_capa_mask)
+			struct ieee80211_ht_cap *ht_capa_mask,
+			struct ieee80211_vht_cap *vht_capa,
+			struct ieee80211_vht_cap *vht_capa_mask)
 {
 	struct wireless_dev *wdev = dev->ieee80211_ptr;
 	int err;
@@ -431,7 +459,8 @@ int cfg80211_mlme_assoc(struct cfg80211_registered_device *rdev,
 	wdev_lock(wdev);
 	err = __cfg80211_mlme_assoc(rdev, dev, chan, bssid, prev_bssid,
 				    ssid, ssid_len, ie, ie_len, use_mfp, crypt,
-				    assoc_flags, ht_capa, ht_capa_mask);
+				    assoc_flags, ht_capa, ht_capa_mask,
+				    vht_capa, vht_capa_mask);
 	wdev_unlock(wdev);
 	mutex_unlock(&rdev->devlist_mtx);
 
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index 0e5176784b4..6a5893f5e48 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -371,6 +371,10 @@ static const struct nla_policy nl80211_policy[NL80211_ATTR_MAX+1] = {
 	[NL80211_ATTR_STA_CAPABILITY] = { .type = NLA_U16 },
 	[NL80211_ATTR_STA_EXT_CAPABILITY] = { .type = NLA_BINARY, },
 	[NL80211_ATTR_SPLIT_WIPHY_DUMP] = { .type = NLA_FLAG, },
+	[NL80211_ATTR_DISABLE_VHT] = { .type = NLA_FLAG },
+	[NL80211_ATTR_VHT_CAPABILITY_MASK] = {
+		.len = NL80211_VHT_CAPABILITY_LEN,
+	},
 };
 
 /* policy for the key attributes */
@@ -1522,6 +1526,12 @@ static int nl80211_send_wiphy(struct cfg80211_registered_device *dev,
 			     dev->wiphy.extended_capabilities_mask)))
 			goto nla_put_failure;
 
+		if (dev->wiphy.vht_capa_mod_mask &&
+		    nla_put(msg, NL80211_ATTR_VHT_CAPABILITY_MASK,
+			    sizeof(*dev->wiphy.vht_capa_mod_mask),
+			    dev->wiphy.vht_capa_mod_mask))
+			goto nla_put_failure;
+
 		/* done */
 		*split_start = 0;
 		break;
@@ -5982,6 +5992,8 @@ static int nl80211_associate(struct sk_buff *skb, struct genl_info *info)
 	u32 flags = 0;
 	struct ieee80211_ht_cap *ht_capa = NULL;
 	struct ieee80211_ht_cap *ht_capa_mask = NULL;
+	struct ieee80211_vht_cap *vht_capa = NULL;
+	struct ieee80211_vht_cap *vht_capa_mask = NULL;
 
 	if (!is_valid_ie_attr(info->attrs[NL80211_ATTR_IE]))
 		return -EINVAL;
@@ -6038,12 +6050,25 @@ static int nl80211_associate(struct sk_buff *skb, struct genl_info *info)
 		ht_capa = nla_data(info->attrs[NL80211_ATTR_HT_CAPABILITY]);
 	}
 
+	if (nla_get_flag(info->attrs[NL80211_ATTR_DISABLE_VHT]))
+		flags |= ASSOC_REQ_DISABLE_VHT;
+
+	if (info->attrs[NL80211_ATTR_VHT_CAPABILITY_MASK])
+		vht_capa_mask =
+			nla_data(info->attrs[NL80211_ATTR_VHT_CAPABILITY_MASK]);
+
+	if (info->attrs[NL80211_ATTR_VHT_CAPABILITY]) {
+		if (!vht_capa_mask)
+			return -EINVAL;
+		vht_capa = nla_data(info->attrs[NL80211_ATTR_VHT_CAPABILITY]);
+	}
+
 	err = nl80211_crypto_settings(rdev, info, &crypto, 1);
 	if (!err)
 		err = cfg80211_mlme_assoc(rdev, dev, chan, bssid, prev_bssid,
 					  ssid, ssid_len, ie, ie_len, use_mfp,
-					  &crypto, flags, ht_capa,
-					  ht_capa_mask);
+					  &crypto, flags, ht_capa, ht_capa_mask,
+					  vht_capa, vht_capa_mask);
 
 	return err;
 }
@@ -6623,6 +6648,24 @@ static int nl80211_connect(struct sk_buff *skb, struct genl_info *info)
 		       sizeof(connect.ht_capa));
 	}
 
+	if (nla_get_flag(info->attrs[NL80211_ATTR_DISABLE_VHT]))
+		connect.flags |= ASSOC_REQ_DISABLE_VHT;
+
+	if (info->attrs[NL80211_ATTR_VHT_CAPABILITY_MASK])
+		memcpy(&connect.vht_capa_mask,
+		       nla_data(info->attrs[NL80211_ATTR_VHT_CAPABILITY_MASK]),
+		       sizeof(connect.vht_capa_mask));
+
+	if (info->attrs[NL80211_ATTR_VHT_CAPABILITY]) {
+		if (!info->attrs[NL80211_ATTR_VHT_CAPABILITY_MASK]) {
+			kfree(connkeys);
+			return -EINVAL;
+		}
+		memcpy(&connect.vht_capa,
+		       nla_data(info->attrs[NL80211_ATTR_VHT_CAPABILITY]),
+		       sizeof(connect.vht_capa));
+	}
+
 	err = cfg80211_connect(rdev, dev, &connect, connkeys);
 	if (err)
 		kfree(connkeys);
diff --git a/net/wireless/sme.c b/net/wireless/sme.c
index f432bd3755b..7da118c034f 100644
--- a/net/wireless/sme.c
+++ b/net/wireless/sme.c
@@ -195,7 +195,9 @@ static int cfg80211_conn_do_work(struct wireless_dev *wdev)
 					    params->mfp != NL80211_MFP_NO,
 					    &params->crypto,
 					    params->flags, &params->ht_capa,
-					    &params->ht_capa_mask);
+					    &params->ht_capa_mask,
+					    &params->vht_capa,
+					    &params->vht_capa_mask);
 		if (err)
 			__cfg80211_mlme_deauth(rdev, wdev->netdev, params->bssid,
 					       NULL, 0,