1 files changed, 101 insertions, 40 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 953dc4f2c6af..b704e5b183d0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -60,15 +60,15 @@ void rt2x00lib_config_intf(struct rt2x00_dev *rt2x00dev,
          * Note that when NULL is passed as address we will send
          * 00:00:00:00:00 to the device to clear the address.
          * This will prevent the device being confused when it wants
-         * to ACK frames or consideres itself associated.
+         * to ACK frames or considers itself associated.
          */
-        memset(&conf.mac, 0, sizeof(conf.mac));
+        memset(conf.mac, 0, sizeof(conf.mac));
         if (mac)
-                memcpy(&conf.mac, mac, ETH_ALEN);
+                memcpy(conf.mac, mac, ETH_ALEN);
 
-        memset(&conf.bssid, 0, sizeof(conf.bssid));
+        memset(conf.bssid, 0, sizeof(conf.bssid));
         if (bssid)
-                memcpy(&conf.bssid, bssid, ETH_ALEN);
+                memcpy(conf.bssid, bssid, ETH_ALEN);
 
         flags |= CONFIG_UPDATE_TYPE;
         if (mac || (!rt2x00dev->intf_ap_count && !rt2x00dev->intf_sta_count))
@@ -81,7 +81,8 @@ void rt2x00lib_config_intf(struct rt2x00_dev *rt2x00dev,
 
 void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
                           struct rt2x00_intf *intf,
-                          struct ieee80211_bss_conf *bss_conf)
+                          struct ieee80211_bss_conf *bss_conf,
+                          u32 changed)
 {
         struct rt2x00lib_erp erp;
 
@@ -99,19 +100,17 @@ void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
         erp.basic_rates = bss_conf->basic_rates;
         erp.beacon_int = bss_conf->beacon_int;
 
+        /* Update the AID, this is needed for dynamic PS support */
+        rt2x00dev->aid = bss_conf->assoc ? bss_conf->aid : 0;
+        rt2x00dev->last_beacon = bss_conf->timestamp;
+
         /* Update global beacon interval time, this is needed for PS support */
         rt2x00dev->beacon_int = bss_conf->beacon_int;
 
-        rt2x00dev->ops->lib->config_erp(rt2x00dev, &erp);
-}
+        if (changed & BSS_CHANGED_HT)
+                erp.ht_opmode = bss_conf->ht_operation_mode;
 
-static inline
-enum antenna rt2x00lib_config_antenna_check(enum antenna current_ant,
-                                            enum antenna default_ant)
-{
-        if (current_ant != ANTENNA_SW_DIVERSITY)
-                return current_ant;
-        return (default_ant != ANTENNA_SW_DIVERSITY) ? default_ant : ANTENNA_B;
+        rt2x00dev->ops->lib->config_erp(rt2x00dev, &erp, changed);
 }
 
 void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
@@ -122,35 +121,43 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
         struct antenna_setup *active = &rt2x00dev->link.ant.active;
 
         /*
-         * Failsafe: Make sure we are not sending the
-         * ANTENNA_SW_DIVERSITY state to the driver.
-         * If that happens, fallback to hardware defaults,
-         * or our own default.
-         * If diversity handling is active for a particular antenna,
-         * we shouldn't overwrite that antenna.
-         * The calls to rt2x00lib_config_antenna_check()
-         * might have caused that we restore back to the already
-         * active setting. If that has happened we can quit.
+         * When the caller tries to send the SW diversity,
+         * we must update the ANTENNA_RX_DIVERSITY flag to
+         * enable the antenna diversity in the link tuner.
+         *
+         * Secondly, we must guarentee we never send the
+         * software antenna diversity command to the driver.
          */
-        if (!(ant->flags & ANTENNA_RX_DIVERSITY))
-                config.rx = rt2x00lib_config_antenna_check(config.rx, def->rx);
-        else
+        if (!(ant->flags & ANTENNA_RX_DIVERSITY)) {
+                if (config.rx == ANTENNA_SW_DIVERSITY) {
+                        ant->flags |= ANTENNA_RX_DIVERSITY;
+
+                        if (def->rx == ANTENNA_SW_DIVERSITY)
+                                config.rx = ANTENNA_B;
+                        else
+                                config.rx = def->rx;
+                }
+        } else if (config.rx == ANTENNA_SW_DIVERSITY)
                 config.rx = active->rx;
 
-        if (!(ant->flags & ANTENNA_TX_DIVERSITY))
-                config.tx = rt2x00lib_config_antenna_check(config.tx, def->tx);
-        else
-                config.tx = active->tx;
+        if (!(ant->flags & ANTENNA_TX_DIVERSITY)) {
+                if (config.tx == ANTENNA_SW_DIVERSITY) {
+                        ant->flags |= ANTENNA_TX_DIVERSITY;
 
-        if (config.rx == active->rx && config.tx == active->tx)
-                return;
+                        if (def->tx == ANTENNA_SW_DIVERSITY)
+                                config.tx = ANTENNA_B;
+                        else
+                                config.tx = def->tx;
+                }
+        } else if (config.tx == ANTENNA_SW_DIVERSITY)
+                config.tx = active->tx;
 
         /*
          * Antenna setup changes require the RX to be disabled,
          * else the changes will be ignored by the device.
          */
         if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF_LINK);
+                rt2x00queue_stop_queue(rt2x00dev->rx);
 
         /*
          * Write new antenna setup to device and reset the link tuner.
@@ -164,7 +171,35 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
         memcpy(active, &config, sizeof(config));
 
         if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON_LINK);
+                rt2x00queue_start_queue(rt2x00dev->rx);
+}
+
+static 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;
 }
 
 void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
@@ -173,6 +208,9 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 {
         struct rt2x00lib_conf libconf;
         u16 hw_value;
+        u16 autowake_timeout;
+        u16 beacon_int;
+        u16 beacon_diff;
 
         memset(&libconf, 0, sizeof(libconf));
 
@@ -180,10 +218,10 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 
         if (ieee80211_flags & IEEE80211_CONF_CHANGE_CHANNEL) {
                 if (conf_is_ht40(conf)) {
-                        __set_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
+                        set_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
                         hw_value = rt2x00ht_center_channel(rt2x00dev, conf);
                 } else {
-                        __clear_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
+                        clear_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
                         hw_value = conf->channel->hw_value;
                 }
 
@@ -196,6 +234,10 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
                        sizeof(libconf.channel));
         }
 
+        if (test_bit(REQUIRE_PS_AUTOWAKE, &rt2x00dev->cap_flags) &&
+            (ieee80211_flags & IEEE80211_CONF_CHANGE_PS))
+                cancel_delayed_work_sync(&rt2x00dev->autowakeup_work);
+
         /*
          * Start configuration.
          */
@@ -208,11 +250,30 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
         if (ieee80211_flags & IEEE80211_CONF_CHANGE_CHANNEL)
                 rt2x00link_reset_tuner(rt2x00dev, false);
 
+        if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) &&
+            test_bit(REQUIRE_PS_AUTOWAKE, &rt2x00dev->cap_flags) &&
+            (ieee80211_flags & IEEE80211_CONF_CHANGE_PS) &&
+            (conf->flags & IEEE80211_CONF_PS)) {
+                beacon_diff = (long)jiffies - (long)rt2x00dev->last_beacon;
+                beacon_int = msecs_to_jiffies(rt2x00dev->beacon_int);
+
+                if (beacon_diff > beacon_int)
+                        beacon_diff = 0;
+
+                autowake_timeout = (conf->max_sleep_period * beacon_int) - beacon_diff;
+                queue_delayed_work(rt2x00dev->workqueue,
+                                   &rt2x00dev->autowakeup_work,
+                                   autowake_timeout - 15);
+        }
+
+        if (conf->flags & IEEE80211_CONF_PS)
+                set_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
+        else
+                clear_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
+
         rt2x00dev->curr_band = conf->channel->band;
+        rt2x00dev->curr_freq = conf->channel->center_freq;
         rt2x00dev->tx_power = conf->power_level;
         rt2x00dev->short_retry = conf->short_frame_max_tx_count;
         rt2x00dev->long_retry = conf->long_frame_max_tx_count;
-
-        rt2x00dev->rx_status.band = conf->channel->band;
-        rt2x00dev->rx_status.freq = conf->channel->center_freq;
 }