1 files changed, 61 insertions, 35 deletions

diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index 36016363d225..b0fddb7de549 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -35,8 +35,6 @@
 #include "debugfs.h"
 #include "debugfs_netdev.h"
 
-#define SUPP_MCS_SET_LEN 16
-
 /*
  * For seeing transmitted packets on monitor interfaces
  * we have a radiotap header too.
@@ -1068,56 +1066,84 @@ u32 ieee80211_handle_ht(struct ieee80211_local *local, int enable_ht,
         struct ieee80211_supported_band *sband;
         struct ieee80211_ht_info ht_conf;
         struct ieee80211_ht_bss_info ht_bss_conf;
-        int i;
         u32 changed = 0;
+        int i;
+        u8 max_tx_streams = IEEE80211_HT_CAP_MAX_STREAMS;
+        u8 tx_mcs_set_cap;
 
         sband = local->hw.wiphy->bands[conf->channel->band];
 
+        memset(&ht_conf, 0, sizeof(struct ieee80211_ht_info));
+        memset(&ht_bss_conf, 0, sizeof(struct ieee80211_ht_bss_info));
+
         /* HT is not supported */
         if (!sband->ht_info.ht_supported) {
                 conf->flags &= ~IEEE80211_CONF_SUPPORT_HT_MODE;
-                return 0;
+                goto out;
         }
 
-        memset(&ht_conf, 0, sizeof(struct ieee80211_ht_info));
-        memset(&ht_bss_conf, 0, sizeof(struct ieee80211_ht_bss_info));
-
-        if (enable_ht) {
-                if (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE))
+        /* disable HT */
+        if (!enable_ht) {
+                if (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE)
                         changed |= BSS_CHANGED_HT;
+                conf->flags &= ~IEEE80211_CONF_SUPPORT_HT_MODE;
+                conf->ht_conf.ht_supported = 0;
+                goto out;
+        }
 
-                conf->flags |= IEEE80211_CONF_SUPPORT_HT_MODE;
-                ht_conf.ht_supported = 1;
 
-                ht_conf.cap = req_ht_cap->cap & sband->ht_info.cap;
-                ht_conf.cap &= ~(IEEE80211_HT_CAP_MIMO_PS);
-                ht_conf.cap |= sband->ht_info.cap & IEEE80211_HT_CAP_MIMO_PS;
+        if (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE))
+                changed |= BSS_CHANGED_HT;
 
-                for (i = 0; i < SUPP_MCS_SET_LEN; i++)
-                        ht_conf.supp_mcs_set[i] =
-                                        sband->ht_info.supp_mcs_set[i] &
-                                        req_ht_cap->supp_mcs_set[i];
+        conf->flags |= IEEE80211_CONF_SUPPORT_HT_MODE;
+        ht_conf.ht_supported = 1;
 
-                ht_bss_conf.primary_channel = req_bss_cap->primary_channel;
-                ht_bss_conf.bss_cap = req_bss_cap->bss_cap;
-                ht_bss_conf.bss_op_mode = req_bss_cap->bss_op_mode;
+        ht_conf.cap = req_ht_cap->cap & sband->ht_info.cap;
+        ht_conf.cap &= ~(IEEE80211_HT_CAP_MIMO_PS);
+        ht_conf.cap |= sband->ht_info.cap & IEEE80211_HT_CAP_MIMO_PS;
+        ht_bss_conf.primary_channel = req_bss_cap->primary_channel;
+        ht_bss_conf.bss_cap = req_bss_cap->bss_cap;
+        ht_bss_conf.bss_op_mode = req_bss_cap->bss_op_mode;
 
-                ht_conf.ampdu_factor = req_ht_cap->ampdu_factor;
-                ht_conf.ampdu_density = req_ht_cap->ampdu_density;
+        ht_conf.ampdu_factor = req_ht_cap->ampdu_factor;
+        ht_conf.ampdu_density = req_ht_cap->ampdu_density;
 
-                /* if bss configuration changed store the new one */
-                if (memcmp(&conf->ht_conf, &ht_conf, sizeof(ht_conf)) ||
-                    memcmp(&conf->ht_bss_conf, &ht_bss_conf, sizeof(ht_bss_conf))) {
-                        changed |= BSS_CHANGED_HT;
-                        memcpy(&conf->ht_conf, &ht_conf, sizeof(ht_conf));
-                        memcpy(&conf->ht_bss_conf, &ht_bss_conf, sizeof(ht_bss_conf));
-                }
-        } else {
-                if (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE)
-                        changed |= BSS_CHANGED_HT;
-                conf->flags &= ~IEEE80211_CONF_SUPPORT_HT_MODE;
-        }
+        /* Bits 96-100 */
+        tx_mcs_set_cap = sband->ht_info.supp_mcs_set[12];
 
+        /* configure suppoerted Tx MCS according to requested MCS
+         * (based in most cases on Rx capabilities of peer) and self
+         * Tx MCS capabilities (as defined by low level driver HW
+         * Tx capabilities) */
+        if (!(tx_mcs_set_cap & IEEE80211_HT_CAP_MCS_TX_DEFINED))
+                goto check_changed;
+
+        /* Counting from 0 therfore + 1 */
+        if (tx_mcs_set_cap & IEEE80211_HT_CAP_MCS_TX_RX_DIFF)
+                max_tx_streams = ((tx_mcs_set_cap &
+                                IEEE80211_HT_CAP_MCS_TX_STREAMS) >> 2) + 1;
+
+        for (i = 0; i < max_tx_streams; i++)
+                ht_conf.supp_mcs_set[i] =
+                        sband->ht_info.supp_mcs_set[i] &
+                                        req_ht_cap->supp_mcs_set[i];
+
+        if (tx_mcs_set_cap & IEEE80211_HT_CAP_MCS_TX_UEQM)
+                for (i = IEEE80211_SUPP_MCS_SET_UEQM;
+                     i < IEEE80211_SUPP_MCS_SET_LEN; i++)
+                        ht_conf.supp_mcs_set[i] =
+                                sband->ht_info.supp_mcs_set[i] &
+                                        req_ht_cap->supp_mcs_set[i];
+
+check_changed:
+        /* if bss configuration changed store the new one */
+        if (memcmp(&conf->ht_conf, &ht_conf, sizeof(ht_conf)) ||
+            memcmp(&conf->ht_bss_conf, &ht_bss_conf, sizeof(ht_bss_conf))) {
+                changed |= BSS_CHANGED_HT;
+                memcpy(&conf->ht_conf, &ht_conf, sizeof(ht_conf));
+                memcpy(&conf->ht_bss_conf, &ht_bss_conf, sizeof(ht_bss_conf));
+        }
+out:
         return changed;
 }