4 files changed, 46 insertions, 8 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 9aa480117172..bebbda5ffaab 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2531,11 +2531,8 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
         else
                 spec->ht.ht_supported = false;
 
-        /*
-         * Don't set IEEE80211_HT_CAP_SUP_WIDTH_20_40 for now as it causes
-         * reception problems with HT40 capable 11n APs
-         */
         spec->ht.cap =
+            IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
             IEEE80211_HT_CAP_GRN_FLD |
             IEEE80211_HT_CAP_SGI_20 |
             IEEE80211_HT_CAP_SGI_40 |
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 098315a271ca..8dbd634dae27 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -170,23 +170,27 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
                       unsigned int ieee80211_flags)
 {
         struct rt2x00lib_conf libconf;
+        u16 hw_value;
 
         memset(&libconf, 0, sizeof(libconf));
 
         libconf.conf = conf;
 
         if (ieee80211_flags & IEEE80211_CONF_CHANGE_CHANNEL) {
-                if (conf_is_ht40(conf))
+                if (conf_is_ht40(conf)) {
                         __set_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
-                else
+                        hw_value = rt2x00ht_center_channel(rt2x00dev, conf);
+                } else {
                         __clear_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
+                        hw_value = conf->channel->hw_value;
+                }
 
                 memcpy(&libconf.rf,
-                       &rt2x00dev->spec.channels[conf->channel->hw_value],
+                       &rt2x00dev->spec.channels[hw_value],
                        sizeof(libconf.rf));
 
                 memcpy(&libconf.channel,
-                       &rt2x00dev->spec.channels_info[conf->channel->hw_value],
+                       &rt2x00dev->spec.channels_info[hw_value],
                        sizeof(libconf.channel));
         }
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00ht.c b/drivers/net/wireless/rt2x00/rt2x00ht.c
index 5a407602ce3e..c4b749da4302 100644
--- a/drivers/net/wireless/rt2x00/rt2x00ht.c
+++ b/drivers/net/wireless/rt2x00/rt2x00ht.c
@@ -84,3 +84,31 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
         else
                 txdesc->txop = TXOP_HTTXOP;
 }
+
+u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+                            struct ieee80211_conf *conf)
+{
+        struct hw_mode_spec *spec = &rt2x00dev->spec;
+        int center_channel;
+        u16 i;
+
+        /*
+         * Initialize center channel to current channel.
+         */
+        center_channel = spec->channels[conf->channel->hw_value].channel;
+
+        /*
+         * Adjust center channel to HT40+ and HT40- operation.
+         */
+        if (conf_is_ht40_plus(conf))
+                center_channel += 2;
+        else if (conf_is_ht40_minus(conf))
+                center_channel -= (center_channel == 14) ? 1 : 2;
+
+        for (i = 0; i < spec->num_channels; i++)
+                if (spec->channels[i].channel == center_channel)
+                        return i;
+
+        WARN_ON(1);
+        return conf->channel->hw_value;
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 822affc9b4ca..ed27de1de57b 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -367,12 +367,21 @@ static inline void rt2x00crypto_rx_insert_iv(struct sk_buff *skb,
 void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
                                    struct txentry_desc *txdesc,
                                    const struct rt2x00_rate *hwrate);
+
+u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+                            struct ieee80211_conf *conf);
 #else
 static inline void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
                                                  struct txentry_desc *txdesc,
                                                  const struct rt2x00_rate *hwrate)
 {
 }
+
+static inline u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+                                          struct ieee80211_conf *conf)
+{
+        return conf->channel->hw_value;
+}
 #endif /* CONFIG_RT2X00_LIB_HT */
 
 /*