1 files changed, 43 insertions, 14 deletions

diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/mwl8k.c
index 5b5cf9357dbf..d8eff68978f1 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/mwl8k.c
@@ -256,10 +256,6 @@ static const struct ieee80211_rate mwl8k_rates[] = {
 	{ .bitrate = 720, .hw_value = 144, },
 };
 
-static const u8 mwl8k_rateids[12] = {
-	2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108,
-};
-
 /* Set or get info from Firmware */
 #define MWL8K_CMD_SET			0x0001
 #define MWL8K_CMD_GET			0x0000
@@ -2076,8 +2072,25 @@ struct mwl8k_cmd_update_set_aid {
 	__u8	supp_rates[14];
 } __attribute__((packed));
 
+static void legacy_rate_mask_to_array(u8 *rates, u32 mask)
+{
+	int i;
+	int j;
+
+	/*
+	 * Clear nonstandard rates 4 and 13.
+	 */
+	mask &= 0x1fef;
+
+	for (i = 0, j = 0; i < 14; i++) {
+		if (mask & (1 << i))
+			rates[j++] = mwl8k_rates[i].hw_value;
+	}
+}
+
 static int
-mwl8k_cmd_set_aid(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+mwl8k_cmd_set_aid(struct ieee80211_hw *hw,
+		  struct ieee80211_vif *vif, u32 legacy_rate_mask)
 {
 	struct mwl8k_cmd_update_set_aid *cmd;
 	u16 prot_mode;
@@ -2090,7 +2103,6 @@ mwl8k_cmd_set_aid(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	cmd->header.code = cpu_to_le16(MWL8K_CMD_SET_AID);
 	cmd->header.length = cpu_to_le16(sizeof(*cmd));
 	cmd->aid = cpu_to_le16(vif->bss_conf.aid);
-
 	memcpy(cmd->bssid, vif->bss_conf.bssid, ETH_ALEN);
 
 	if (vif->bss_conf.use_cts_prot) {
@@ -2111,7 +2123,7 @@ mwl8k_cmd_set_aid(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	}
 	cmd->protection_mode = cpu_to_le16(prot_mode);
 
-	memcpy(cmd->supp_rates, mwl8k_rateids, sizeof(mwl8k_rateids));
+	legacy_rate_mask_to_array(cmd->supp_rates, legacy_rate_mask);
 
 	rc = mwl8k_post_cmd(hw, &cmd->header);
 	kfree(cmd);
@@ -2132,7 +2144,8 @@ struct mwl8k_cmd_set_rate {
 } __attribute__((packed));
 
 static int
-mwl8k_cmd_set_rate(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+mwl8k_cmd_set_rate(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   u32 legacy_rate_mask)
 {
 	struct mwl8k_cmd_set_rate *cmd;
 	int rc;
@@ -2143,7 +2156,7 @@ mwl8k_cmd_set_rate(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 	cmd->header.code = cpu_to_le16(MWL8K_CMD_SET_RATE);
 	cmd->header.length = cpu_to_le16(sizeof(*cmd));
-	memcpy(cmd->legacy_rates, mwl8k_rateids, sizeof(mwl8k_rateids));
+	legacy_rate_mask_to_array(cmd->legacy_rates, legacy_rate_mask);
 
 	rc = mwl8k_post_cmd(hw, &cmd->header);
 	kfree(cmd);
@@ -2644,7 +2657,8 @@ struct mwl8k_cmd_update_stadb {
 #define MWL8K_PEER_TYPE_ACCESSPOINT	2
 
 static int mwl8k_cmd_update_stadb_add(struct ieee80211_hw *hw,
-				      struct ieee80211_vif *vif, u8 *addr)
+				      struct ieee80211_vif *vif,
+				      u8 *addr, u32 legacy_rate_mask)
 {
 	struct mwl8k_cmd_update_stadb *cmd;
 	struct peer_capability_info *p;
@@ -2662,7 +2676,7 @@ static int mwl8k_cmd_update_stadb_add(struct ieee80211_hw *hw,
 	p = &cmd->peer_info;
 	p->peer_type = MWL8K_PEER_TYPE_ACCESSPOINT;
 	p->basic_caps = cpu_to_le16(vif->bss_conf.assoc_capability);
-	memcpy(p->legacy_rates, mwl8k_rateids, sizeof(mwl8k_rateids));
+	legacy_rate_mask_to_array(p->legacy_rates, legacy_rate_mask);
 	p->interop = 1;
 	p->amsdu_enabled = 0;
 
@@ -2956,8 +2970,20 @@ static void mwl8k_bss_info_changed(struct ieee80211_hw *hw,
 		return;
 
 	if (vif->bss_conf.assoc) {
+		struct ieee80211_sta *ap;
+		u32 legacy_rate_mask;
+
+		rcu_read_lock();
+		ap = ieee80211_find_sta(vif, vif->bss_conf.bssid);
+		if (ap != NULL)
+			legacy_rate_mask = ap->supp_rates[IEEE80211_BAND_2GHZ];
+		rcu_read_unlock();
+
+		if (ap == NULL)
+			goto out;
+
 		/* Install rates */
-		rc = mwl8k_cmd_set_rate(hw, vif);
+		rc = mwl8k_cmd_set_rate(hw, vif, legacy_rate_mask);
 		if (rc)
 			goto out;
 
@@ -2979,7 +3005,7 @@ static void mwl8k_bss_info_changed(struct ieee80211_hw *hw,
 			goto out;
 
 		/* Set AID */
-		rc = mwl8k_cmd_set_aid(hw, vif);
+		rc = mwl8k_cmd_set_aid(hw, vif, legacy_rate_mask);
 		if (rc)
 			goto out;
 
@@ -3139,6 +3165,7 @@ struct mwl8k_sta_notify_item
 	struct ieee80211_vif *vif;
 	enum sta_notify_cmd cmd;
 	u8 addr[ETH_ALEN];
+	u32 legacy_rate_mask;
 };
 
 static void mwl8k_sta_notify_worker(struct work_struct *work)
@@ -3160,7 +3187,8 @@ static void mwl8k_sta_notify_worker(struct work_struct *work)
 		if (s->cmd == STA_NOTIFY_ADD) {
 			int rc;
 
-			rc = mwl8k_cmd_update_stadb_add(hw, s->vif, s->addr);
+			rc = mwl8k_cmd_update_stadb_add(hw, s->vif,
+						s->addr, s->legacy_rate_mask);
 			if (rc >= 0) {
 				struct ieee80211_sta *sta;
 
@@ -3196,6 +3224,7 @@ mwl8k_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		s->vif = vif;
 		s->cmd = cmd;
 		memcpy(s->addr, sta->addr, ETH_ALEN);
+		s->legacy_rate_mask = sta->supp_rates[IEEE80211_BAND_2GHZ];
 
 		spin_lock(&priv->sta_notify_list_lock);
 		list_add_tail(&s->list, &priv->sta_notify_list);