1 files changed, 132 insertions, 23 deletions

diff --git a/net/mac80211/tdls.c b/net/mac80211/tdls.c
index c9f9752217ac..fff0d864adfa 100644
--- a/net/mac80211/tdls.c
+++ b/net/mac80211/tdls.c
@@ -136,6 +136,24 @@ ieee80211_tdls_add_supp_channels(struct ieee80211_sub_if_data *sdata,
 	*pos = 2 * subband_cnt;
 }
 
+static void ieee80211_tdls_add_oper_classes(struct ieee80211_sub_if_data *sdata,
+					    struct sk_buff *skb)
+{
+	u8 *pos;
+	u8 op_class;
+
+	if (!ieee80211_chandef_to_operating_class(&sdata->vif.bss_conf.chandef,
+						  &op_class))
+		return;
+
+	pos = skb_put(skb, 4);
+	*pos++ = WLAN_EID_SUPPORTED_REGULATORY_CLASSES;
+	*pos++ = 2; /* len */
+
+	*pos++ = op_class;
+	*pos++ = op_class; /* give current operating class as alternate too */
+}
+
 static void ieee80211_tdls_add_bss_coex_ie(struct sk_buff *skb)
 {
 	u8 *pos = (void *)skb_put(skb, 3);
@@ -193,6 +211,17 @@ static void ieee80211_tdls_add_link_ie(struct ieee80211_sub_if_data *sdata,
 	memcpy(lnkid->resp_sta, rsp_addr, ETH_ALEN);
 }
 
+static void
+ieee80211_tdls_add_aid(struct ieee80211_sub_if_data *sdata, struct sk_buff *skb)
+{
+	struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
+	u8 *pos = (void *)skb_put(skb, 4);
+
+	*pos++ = WLAN_EID_AID;
+	*pos++ = 2; /* len */
+	put_unaligned_le16(ifmgd->aid, pos);
+}
+
 /* translate numbering in the WMM parameter IE to the mac80211 notation */
 static enum ieee80211_ac_numbers ieee80211_ac_from_wmm(int ac)
 {
@@ -271,21 +300,11 @@ ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 	struct ieee80211_local *local = sdata->local;
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_sta_ht_cap ht_cap;
+	struct ieee80211_sta_vht_cap vht_cap;
 	struct sta_info *sta = NULL;
 	size_t offset = 0, noffset;
 	u8 *pos;
 
-	rcu_read_lock();
-
-	/* we should have the peer STA if we're already responding */
-	if (action_code == WLAN_TDLS_SETUP_RESPONSE) {
-		sta = sta_info_get(sdata, peer);
-		if (WARN_ON_ONCE(!sta)) {
-			rcu_read_unlock();
-			return;
-		}
-	}
-
 	ieee80211_add_srates_ie(sdata, skb, false, band);
 	ieee80211_add_ext_srates_ie(sdata, skb, false, band);
 	ieee80211_tdls_add_supp_channels(sdata, skb);
@@ -338,6 +357,19 @@ ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 		offset = noffset;
 	}
 
+	rcu_read_lock();
+
+	/* we should have the peer STA if we're already responding */
+	if (action_code == WLAN_TDLS_SETUP_RESPONSE) {
+		sta = sta_info_get(sdata, peer);
+		if (WARN_ON_ONCE(!sta)) {
+			rcu_read_unlock();
+			return;
+		}
+	}
+
+	ieee80211_tdls_add_oper_classes(sdata, skb);
+
 	/*
 	 * with TDLS we can switch channels, and HT-caps are not necessarily
 	 * the same on all bands. The specification limits the setup to a
@@ -346,7 +378,9 @@ ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 	sband = local->hw.wiphy->bands[band];
 	memcpy(&ht_cap, &sband->ht_cap, sizeof(ht_cap));
 
-	if (action_code == WLAN_TDLS_SETUP_REQUEST && ht_cap.ht_supported) {
+	if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
+	     action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
+	    ht_cap.ht_supported) {
 		ieee80211_apply_htcap_overrides(sdata, &ht_cap);
 
 		/* disable SMPS in TDLS initiator */
@@ -368,12 +402,63 @@ ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 		ieee80211_ie_build_ht_cap(pos, &ht_cap, ht_cap.cap);
 	}
 
-	rcu_read_unlock();
-
 	if (ht_cap.ht_supported &&
 	    (ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 		ieee80211_tdls_add_bss_coex_ie(skb);
 
+	ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
+
+	/* add any custom IEs that go before VHT capabilities */
+	if (extra_ies_len) {
+		static const u8 before_vht_cap[] = {
+			WLAN_EID_SUPP_RATES,
+			WLAN_EID_COUNTRY,
+			WLAN_EID_EXT_SUPP_RATES,
+			WLAN_EID_SUPPORTED_CHANNELS,
+			WLAN_EID_RSN,
+			WLAN_EID_EXT_CAPABILITY,
+			WLAN_EID_QOS_CAPA,
+			WLAN_EID_FAST_BSS_TRANSITION,
+			WLAN_EID_TIMEOUT_INTERVAL,
+			WLAN_EID_SUPPORTED_REGULATORY_CLASSES,
+			WLAN_EID_MULTI_BAND,
+		};
+		noffset = ieee80211_ie_split(extra_ies, extra_ies_len,
+					     before_vht_cap,
+					     ARRAY_SIZE(before_vht_cap),
+					     offset);
+		pos = skb_put(skb, noffset - offset);
+		memcpy(pos, extra_ies + offset, noffset - offset);
+		offset = noffset;
+	}
+
+	/* build the VHT-cap similarly to the HT-cap */
+	memcpy(&vht_cap, &sband->vht_cap, sizeof(vht_cap));
+	if ((action_code == WLAN_TDLS_SETUP_REQUEST ||
+	     action_code == WLAN_PUB_ACTION_TDLS_DISCOVER_RES) &&
+	    vht_cap.vht_supported) {
+		ieee80211_apply_vhtcap_overrides(sdata, &vht_cap);
+
+		/* the AID is present only when VHT is implemented */
+		if (action_code == WLAN_TDLS_SETUP_REQUEST)
+			ieee80211_tdls_add_aid(sdata, skb);
+
+		pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
+		ieee80211_ie_build_vht_cap(pos, &vht_cap, vht_cap.cap);
+	} else if (action_code == WLAN_TDLS_SETUP_RESPONSE &&
+		   vht_cap.vht_supported && sta->sta.vht_cap.vht_supported) {
+		/* the peer caps are already intersected with our own */
+		memcpy(&vht_cap, &sta->sta.vht_cap, sizeof(vht_cap));
+
+		/* the AID is present only when VHT is implemented */
+		ieee80211_tdls_add_aid(sdata, skb);
+
+		pos = skb_put(skb, sizeof(struct ieee80211_vht_cap) + 2);
+		ieee80211_ie_build_vht_cap(pos, &vht_cap, vht_cap.cap);
+	}
+
+	rcu_read_unlock();
+
 	/* add any remaining IEs */
 	if (extra_ies_len) {
 		noffset = extra_ies_len;
@@ -381,7 +466,6 @@ ieee80211_tdls_add_setup_start_ies(struct ieee80211_sub_if_data *sdata,
 		memcpy(pos, extra_ies + offset, noffset - offset);
 	}
 
-	ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 }
 
 static void
@@ -394,6 +478,7 @@ ieee80211_tdls_add_setup_cfm_ies(struct ieee80211_sub_if_data *sdata,
 	struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
 	size_t offset = 0, noffset;
 	struct sta_info *sta, *ap_sta;
+	enum ieee80211_band band = ieee80211_get_sdata_band(sdata);
 	u8 *pos;
 
 	rcu_read_lock();
@@ -453,6 +538,21 @@ ieee80211_tdls_add_setup_cfm_ies(struct ieee80211_sub_if_data *sdata,
 		}
 	}
 
+	ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
+
+	/* only include VHT-operation if not on the 2.4GHz band */
+	if (band != IEEE80211_BAND_2GHZ && !ap_sta->sta.vht_cap.vht_supported &&
+	    sta->sta.vht_cap.vht_supported) {
+		struct ieee80211_chanctx_conf *chanctx_conf =
+				rcu_dereference(sdata->vif.chanctx_conf);
+		if (!WARN_ON(!chanctx_conf)) {
+			pos = skb_put(skb, 2 +
+				      sizeof(struct ieee80211_vht_operation));
+			ieee80211_ie_build_vht_oper(pos, &sta->sta.vht_cap,
+						    &chanctx_conf->def);
+		}
+	}
+
 	rcu_read_unlock();
 
 	/* add any remaining IEs */
@@ -461,8 +561,6 @@ ieee80211_tdls_add_setup_cfm_ies(struct ieee80211_sub_if_data *sdata,
 		pos = skb_put(skb, noffset - offset);
 		memcpy(pos, extra_ies + offset, noffset - offset);
 	}
-
-	ieee80211_tdls_add_link_ie(sdata, skb, peer, initiator);
 }
 
 static void
@@ -708,8 +806,12 @@ ieee80211_tdls_build_mgmt_packet_data(struct ieee80211_sub_if_data *sdata,
 			       26 + /* max(WMM-info, WMM-param) */
 			       2 + max(sizeof(struct ieee80211_ht_cap),
 				       sizeof(struct ieee80211_ht_operation)) +
+			       2 + max(sizeof(struct ieee80211_vht_cap),
+				       sizeof(struct ieee80211_vht_operation)) +
 			       50 + /* supported channels */
 			       3 + /* 40/20 BSS coex */
+			       4 + /* AID */
+			       4 + /* oper classes */
 			       extra_ies_len +
 			       sizeof(struct ieee80211_tdls_lnkie));
 	if (!skb)
@@ -907,7 +1009,7 @@ ieee80211_tdls_mgmt_setup(struct wiphy *wiphy, struct net_device *dev,
 	if (!is_zero_ether_addr(sdata->u.mgd.tdls_peer) &&
 	    !ether_addr_equal(sdata->u.mgd.tdls_peer, peer)) {
 		ret = -EBUSY;
-		goto exit;
+		goto out_unlock;
 	}
 
 	/*
@@ -922,27 +1024,34 @@ ieee80211_tdls_mgmt_setup(struct wiphy *wiphy, struct net_device *dev,
 		if (!sta_info_get(sdata, peer)) {
 			rcu_read_unlock();
 			ret = -ENOLINK;
-			goto exit;
+			goto out_unlock;
 		}
 		rcu_read_unlock();
 	}
 
 	ieee80211_flush_queues(local, sdata, false);
+	memcpy(sdata->u.mgd.tdls_peer, peer, ETH_ALEN);
+	mutex_unlock(&local->mtx);
 
+	/* we cannot take the mutex while preparing the setup packet */
 	ret = ieee80211_tdls_prep_mgmt_packet(wiphy, dev, peer, action_code,
 					      dialog_token, status_code,
 					      peer_capability, initiator,
 					      extra_ies, extra_ies_len, 0,
 					      NULL);
-	if (ret < 0)
-		goto exit;
+	if (ret < 0) {
+		mutex_lock(&local->mtx);
+		eth_zero_addr(sdata->u.mgd.tdls_peer);
+		mutex_unlock(&local->mtx);
+		return ret;
+	}
 
-	memcpy(sdata->u.mgd.tdls_peer, peer, ETH_ALEN);
 	ieee80211_queue_delayed_work(&sdata->local->hw,
 				     &sdata->u.mgd.tdls_peer_del_work,
 				     TDLS_PEER_SETUP_TIMEOUT);
+	return 0;
 
-exit:
+out_unlock:
 	mutex_unlock(&local->mtx);
 	return ret;
 }