5 files changed, 51 insertions, 24 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index f85eedbbad68..8fa971ccfddd 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -723,8 +723,7 @@ struct rt2x00_dev {
 
         /*
          * This is the default TX/RX antenna setup as indicated
-         * by the device's EEPROM. When mac80211 sets its
-         * antenna value to 0 we should be using these values.
+         * by the device's EEPROM.
          */
         struct antenna_setup default_ant;
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 3e4eee3ab7d2..1059b4d1868f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -109,15 +109,32 @@ void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
 }
 
 void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
-                              enum antenna rx, enum antenna tx)
+                              struct antenna_setup *ant)
 {
-        struct antenna_setup ant;
-
-        ant.rx = rx;
-        ant.tx = tx;
+        /*
+         * Failsafe: Make sure we are not sending the
+         * ANTENNA_SW_DIVERSITY state to the driver.
+         * If that happes fallback to hardware default,
+         * or our own default.
+         */
+        if (ant->rx == ANTENNA_SW_DIVERSITY) {
+                if (rt2x00dev->default_ant.rx == ANTENNA_SW_DIVERSITY)
+                        ant->rx = ANTENNA_B;
+                else
+                        ant->rx = rt2x00dev->default_ant.rx;
+        }
+        if (ant->tx == ANTENNA_SW_DIVERSITY) {
+                if (rt2x00dev->default_ant.tx == ANTENNA_SW_DIVERSITY)
+                        ant->tx = ANTENNA_B;
+                else
+                        ant->tx = rt2x00dev->default_ant.tx;
+        }
 
-        if (rx == rt2x00dev->link.ant.active.rx &&
-            tx == rt2x00dev->link.ant.active.tx)
+        /*
+         * Only reconfigure when something has changed.
+         */
+        if (ant->rx == rt2x00dev->link.ant.active.rx &&
+            ant->tx == rt2x00dev->link.ant.active.tx)
                 return;
 
         /*
@@ -132,12 +149,12 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
          * The latter is required since we need to recalibrate the
          * noise-sensitivity ratio for the new setup.
          */
-        rt2x00dev->ops->lib->config_ant(rt2x00dev, &ant);
+        rt2x00dev->ops->lib->config_ant(rt2x00dev, ant);
 
         rt2x00lib_reset_link_tuner(rt2x00dev);
         rt2x00_reset_link_ant_rssi(&rt2x00dev->link);
 
-        memcpy(&rt2x00dev->link.ant.active, &ant, sizeof(ant));
+        memcpy(&rt2x00dev->link.ant.active, ant, sizeof(*ant));
 
         if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON_LINK);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index a8fee68a419e..e8ca1cbfeb90 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -176,13 +176,14 @@ void rt2x00lib_toggle_rx(struct rt2x00_dev *rt2x00dev, enum dev_state state)
 
 static void rt2x00lib_evaluate_antenna_sample(struct rt2x00_dev *rt2x00dev)
 {
-        enum antenna rx = rt2x00dev->link.ant.active.rx;
-        enum antenna tx = rt2x00dev->link.ant.active.tx;
+        struct antenna_setup ant;
         int sample_a =
             rt2x00_get_link_ant_rssi_history(&rt2x00dev->link, ANTENNA_A);
         int sample_b =
             rt2x00_get_link_ant_rssi_history(&rt2x00dev->link, ANTENNA_B);
 
+        memcpy(&ant, &rt2x00dev->link.ant.active, sizeof(ant));
+
         /*
          * We are done sampling. Now we should evaluate the results.
          */
@@ -200,21 +201,22 @@ static void rt2x00lib_evaluate_antenna_sample(struct rt2x00_dev *rt2x00dev)
                 return;
 
         if (rt2x00dev->link.ant.flags & ANTENNA_RX_DIVERSITY)
-                rx = (sample_a > sample_b) ? ANTENNA_A : ANTENNA_B;
+                ant.rx = (sample_a > sample_b) ? ANTENNA_A : ANTENNA_B;
 
         if (rt2x00dev->link.ant.flags & ANTENNA_TX_DIVERSITY)
-                tx = (sample_a > sample_b) ? ANTENNA_A : ANTENNA_B;
+                ant.tx = (sample_a > sample_b) ? ANTENNA_A : ANTENNA_B;
 
-        rt2x00lib_config_antenna(rt2x00dev, rx, tx);
+        rt2x00lib_config_antenna(rt2x00dev, &ant);
 }
 
 static void rt2x00lib_evaluate_antenna_eval(struct rt2x00_dev *rt2x00dev)
 {
-        enum antenna rx = rt2x00dev->link.ant.active.rx;
-        enum antenna tx = rt2x00dev->link.ant.active.tx;
+        struct antenna_setup ant;
         int rssi_curr = rt2x00_get_link_ant_rssi(&rt2x00dev->link);
         int rssi_old = rt2x00_update_ant_rssi(&rt2x00dev->link, rssi_curr);
 
+        memcpy(&ant, &rt2x00dev->link.ant.active, sizeof(ant));
+
         /*
          * Legacy driver indicates that we should swap antenna's
          * when the difference in RSSI is greater that 5. This
@@ -230,12 +232,12 @@ static void rt2x00lib_evaluate_antenna_eval(struct rt2x00_dev *rt2x00dev)
         rt2x00dev->link.ant.flags |= ANTENNA_MODE_SAMPLE;
 
         if (rt2x00dev->link.ant.flags & ANTENNA_RX_DIVERSITY)
-                rx = (rx == ANTENNA_A) ? ANTENNA_B : ANTENNA_A;
+                ant.rx = (ant.rx == ANTENNA_A) ? ANTENNA_B : ANTENNA_A;
 
         if (rt2x00dev->link.ant.flags & ANTENNA_TX_DIVERSITY)
-                tx = (tx == ANTENNA_A) ? ANTENNA_B : ANTENNA_A;
+                ant.tx = (ant.tx == ANTENNA_A) ? ANTENNA_B : ANTENNA_A;
 
-        rt2x00lib_config_antenna(rt2x00dev, rx, tx);
+        rt2x00lib_config_antenna(rt2x00dev, &ant);
 }
 
 static void rt2x00lib_evaluate_antenna(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index c52ee218e4f5..0c3a3559ba48 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -93,7 +93,7 @@ void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
                           struct rt2x00_intf *intf,
                           struct ieee80211_bss_conf *conf);
 void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
-                              enum antenna rx, enum antenna tx);
+                              struct antenna_setup *ant);
 void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
                       struct ieee80211_conf *conf,
                       const unsigned int changed_flags);
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 188f89177bd8..48636b0dd895 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -339,7 +339,6 @@ int rt2x00mac_config(struct ieee80211_hw *hw, u32 changed)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
         struct ieee80211_conf *conf = &hw->conf;
-        int radio_on;
         int status;
 
         /*
@@ -356,7 +355,6 @@ int rt2x00mac_config(struct ieee80211_hw *hw, u32 changed)
          * some configuration parameters (e.g. channel and antenna values) can
          * only be set when the radio is enabled.
          */
-        radio_on = test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags);
         if (conf->radio_enabled) {
                 /* For programming the values, we have to turn RX off */
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
@@ -372,6 +370,17 @@ int rt2x00mac_config(struct ieee80211_hw *hw, u32 changed)
                  */
                 rt2x00lib_config(rt2x00dev, conf, changed);
 
+                /*
+                 * The radio was enabled, configure the antenna to the
+                 * default settings, the link tuner will later start
+                 * continue configuring the antenna based on the software
+                 * diversity. But for non-diversity configurations, we need
+                 * to have configured the correct state now.
+                 */
+                if (changed & IEEE80211_CONF_CHANGE_RADIO_ENABLED)
+                        rt2x00lib_config_antenna(rt2x00dev,
+                                                 &rt2x00dev->default_ant);
+
                 /* Turn RX back on */
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
         } else {