#define IEEE80211_HT_CAP_SGI_40                        0x0040
 #define IEEE80211_HT_CAP_DELAY_BA              0x0400
 #define IEEE80211_HT_CAP_MAX_AMSDU             0x0800
+/* 802.11n HT capability AMPDU settings */
 #define IEEE80211_HT_CAP_AMPDU_FACTOR          0x03
 #define IEEE80211_HT_CAP_AMPDU_DENSITY         0x1C
+/* 802.11n HT capability MSC set */
+#define IEEE80211_SUPP_MCS_SET_UEQM            4
+#define IEEE80211_HT_CAP_MAX_STREAMS           4
+#define IEEE80211_SUPP_MCS_SET_LEN             10
+/* maximum streams the spec allows */
+#define IEEE80211_HT_CAP_MCS_TX_DEFINED                0x01
+#define IEEE80211_HT_CAP_MCS_TX_RX_DIFF                0x02
+#define IEEE80211_HT_CAP_MCS_TX_STREAMS                0x0C
+#define IEEE80211_HT_CAP_MCS_TX_UEQM           0x10
 /* 802.11n HT IE masks */
 #define IEEE80211_HT_IE_CHA_SEC_OFFSET         0x03
 #define IEEE80211_HT_IE_CHA_WIDTH              0x04
 #define IEEE80211_HT_IE_NON_HT_STA_PRSNT       0x0010
 
 /* MIMO Power Save Modes */
-#define WLAN_HT_CAP_MIMO_PS_STATIC         0
-#define WLAN_HT_CAP_MIMO_PS_DYNAMIC        1
-#define WLAN_HT_CAP_MIMO_PS_INVALID        2
-#define WLAN_HT_CAP_MIMO_PS_DISABLED       3
+#define WLAN_HT_CAP_MIMO_PS_STATIC     0
+#define WLAN_HT_CAP_MIMO_PS_DYNAMIC    1
+#define WLAN_HT_CAP_MIMO_PS_INVALID    2
+#define WLAN_HT_CAP_MIMO_PS_DISABLED   3
 
 /* Authentication algorithms */
 #define WLAN_AUTH_OPEN 0
 
 #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.
        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;
 }