1 files changed, 37 insertions, 68 deletions

diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index c90f338b229c..3888ad3b052f 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -4091,93 +4091,62 @@ int ieee80211_parse_bitrates(enum nl80211_chan_width width,
 	return count;
 }
 
-int ieee80211_add_srates_ie(struct ieee80211_sub_if_data *sdata,
-			    struct sk_buff *skb, bool need_basic,
-			    enum nl80211_band band)
+int ieee80211_put_srates_elem(struct sk_buff *skb,
+			      const struct ieee80211_supported_band *sband,
+			      u32 basic_rates, u32 rate_flags, u32 masked_rates,
+			      u8 element_id)
 {
-	struct ieee80211_local *local = sdata->local;
-	struct ieee80211_supported_band *sband;
-	int rate;
-	u8 i, rates, *pos;
-	u32 basic_rates = sdata->vif.bss_conf.basic_rates;
-	u32 rate_flags;
+	u8 i, rates, skip;
 
-	rate_flags =
-		ieee80211_chandef_rate_flags(&sdata->vif.bss_conf.chanreq.oper);
-	sband = local->hw.wiphy->bands[band];
 	rates = 0;
 	for (i = 0; i < sband->n_bitrates; i++) {
 		if ((rate_flags & sband->bitrates[i].flags) != rate_flags)
 			continue;
+		if (masked_rates & BIT(i))
+			continue;
 		rates++;
 	}
-	if (rates > 8)
-		rates = 8;
 
-	if (skb_tailroom(skb) < rates + 2)
-		return -ENOMEM;
-
-	pos = skb_put(skb, rates + 2);
-	*pos++ = WLAN_EID_SUPP_RATES;
-	*pos++ = rates;
-	for (i = 0; i < rates; i++) {
-		u8 basic = 0;
-		if ((rate_flags & sband->bitrates[i].flags) != rate_flags)
-			continue;
-
-		if (need_basic && basic_rates & BIT(i))
-			basic = 0x80;
-		rate = DIV_ROUND_UP(sband->bitrates[i].bitrate, 5);
-		*pos++ = basic | (u8) rate;
+	if (element_id == WLAN_EID_SUPP_RATES) {
+		rates = min_t(u8, rates, 8);
+		skip = 0;
+	} else {
+		skip = 8;
+		if (rates <= skip)
+			return 0;
+		rates -= skip;
 	}
 
-	return 0;
-}
+	if (skb_tailroom(skb) < rates + 2)
+		return -ENOBUFS;
 
-int ieee80211_add_ext_srates_ie(struct ieee80211_sub_if_data *sdata,
-				struct sk_buff *skb, bool need_basic,
-				enum nl80211_band band)
-{
-	struct ieee80211_local *local = sdata->local;
-	struct ieee80211_supported_band *sband;
-	int rate;
-	u8 i, exrates, *pos;
-	u32 basic_rates = sdata->vif.bss_conf.basic_rates;
-	u32 rate_flags;
+	skb_put_u8(skb, element_id);
+	skb_put_u8(skb, rates);
+
+	for (i = 0; i < sband->n_bitrates && rates; i++) {
+		int rate;
+		u8 basic;
 
-	rate_flags =
-		ieee80211_chandef_rate_flags(&sdata->vif.bss_conf.chanreq.oper);
-	sband = local->hw.wiphy->bands[band];
-	exrates = 0;
-	for (i = 0; i < sband->n_bitrates; i++) {
 		if ((rate_flags & sband->bitrates[i].flags) != rate_flags)
 			continue;
-		exrates++;
-	}
+		if (masked_rates & BIT(i))
+			continue;
 
-	if (exrates > 8)
-		exrates -= 8;
-	else
-		exrates = 0;
+		if (skip > 0) {
+			skip--;
+			continue;
+		}
 
-	if (skb_tailroom(skb) < exrates + 2)
-		return -ENOMEM;
+		basic = basic_rates & BIT(i) ? 0x80 : 0;
 
-	if (exrates) {
-		pos = skb_put(skb, exrates + 2);
-		*pos++ = WLAN_EID_EXT_SUPP_RATES;
-		*pos++ = exrates;
-		for (i = 8; i < sband->n_bitrates; i++) {
-			u8 basic = 0;
-			if ((rate_flags & sband->bitrates[i].flags)
-			    != rate_flags)
-				continue;
-			if (need_basic && basic_rates & BIT(i))
-				basic = 0x80;
-			rate = DIV_ROUND_UP(sband->bitrates[i].bitrate, 5);
-			*pos++ = basic | (u8) rate;
-		}
+		rate = DIV_ROUND_UP(sband->bitrates[i].bitrate, 5);
+		skb_put_u8(skb, basic | (u8)rate);
+		rates--;
 	}
+
+	WARN(rates > 0, "rates confused: rates:%d, element:%d\n",
+	     rates, element_id);
+
 	return 0;
 }