}
 
 static void rt2400pci_config_antenna(struct rt2x00_dev *rt2x00dev,
-                                    int antenna_tx, int antenna_rx)
+                                    struct antenna_setup *ant)
 {
        u8 r1;
        u8 r4;
        /*
         * Configure the TX antenna.
         */
-       switch (antenna_tx) {
+       switch (ant->tx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r1, BBP_R1_TX_ANTENNA, 1);
        /*
         * Configure the RX antenna.
         */
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
                rt2400pci_config_txpower(rt2x00dev,
                                         libconf->conf->power_level);
        if (flags & CONFIG_UPDATE_ANTENNA)
-               rt2400pci_config_antenna(rt2x00dev,
-                                        libconf->conf->antenna_sel_tx,
-                                        libconf->conf->antenna_sel_rx);
+               rt2400pci_config_antenna(rt2x00dev, &libconf->ant);
        if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
                rt2400pci_config_duration(rt2x00dev, libconf);
 }
        /*
         * Identify default antenna configuration.
         */
-       rt2x00dev->hw->conf.antenna_sel_tx =
+       rt2x00dev->default_ant.tx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
-       rt2x00dev->hw->conf.antenna_sel_rx =
+       rt2x00dev->default_ant.rx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
 
+       /*
+        * When the eeprom indicates SW_DIVERSITY use HW_DIVERSITY instead.
+        * I am not 100% sure about this, but the legacy drivers do not
+        * indicate antenna swapping in software is required when
+        * diversity is enabled.
+        */
+       if (rt2x00dev->default_ant.tx == ANTENNA_SW_DIVERSITY)
+               rt2x00dev->default_ant.tx = ANTENNA_HW_DIVERSITY;
+       if (rt2x00dev->default_ant.rx == ANTENNA_SW_DIVERSITY)
+               rt2x00dev->default_ant.rx = ANTENNA_HW_DIVERSITY;
+
        /*
         * Store led mode, for correct led behaviour.
         */
 
 }
 
 static void rt2500pci_config_antenna(struct rt2x00_dev *rt2x00dev,
-                                    const int antenna_tx, const int antenna_rx)
+                                    struct antenna_setup *ant)
 {
        u32 reg;
        u8 r14;
        /*
         * Configure the TX antenna.
         */
-       switch (antenna_tx) {
+       switch (ant->tx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 2);
        /*
         * Configure the RX antenna.
         */
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 2);
                rt2500pci_config_txpower(rt2x00dev,
                                         libconf->conf->power_level);
        if (flags & CONFIG_UPDATE_ANTENNA)
-               rt2500pci_config_antenna(rt2x00dev,
-                                        libconf->conf->antenna_sel_tx,
-                                        libconf->conf->antenna_sel_rx);
+               rt2500pci_config_antenna(rt2x00dev, &libconf->ant);
        if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
                rt2500pci_config_duration(rt2x00dev, libconf);
 }
        /*
         * Identify default antenna configuration.
         */
-       rt2x00dev->hw->conf.antenna_sel_tx =
+       rt2x00dev->default_ant.tx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
-       rt2x00dev->hw->conf.antenna_sel_rx =
+       rt2x00dev->default_ant.rx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
 
        /*
 
 }
 
 static void rt2500usb_config_antenna(struct rt2x00_dev *rt2x00dev,
-                                    const int antenna_tx, const int antenna_rx)
+                                    struct antenna_setup *ant)
 {
        u8 r2;
        u8 r14;
        /*
         * Configure the TX antenna.
         */
-       switch (antenna_tx) {
+       switch (ant->tx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 1);
        /*
         * Configure the RX antenna.
         */
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 1);
                rt2500usb_config_txpower(rt2x00dev,
                                         libconf->conf->power_level);
        if (flags & CONFIG_UPDATE_ANTENNA)
-               rt2500usb_config_antenna(rt2x00dev,
-                                        libconf->conf->antenna_sel_tx,
-                                        libconf->conf->antenna_sel_rx);
+               rt2500usb_config_antenna(rt2x00dev, &libconf->ant);
        if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
                rt2500usb_config_duration(rt2x00dev, libconf);
 }
        /*
         * Identify default antenna configuration.
         */
-       rt2x00dev->hw->conf.antenna_sel_tx =
+       rt2x00dev->default_ant.tx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
-       rt2x00dev->hw->conf.antenna_sel_rx =
+       rt2x00dev->default_ant.rx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
 
+       /*
+        * When the eeprom indicates SW_DIVERSITY use HW_DIVERSITY instead.
+        * I am not 100% sure about this, but the legacy drivers do not
+        * indicate antenna swapping in software is required when
+        * diversity is enabled.
+        */
+       if (rt2x00dev->default_ant.tx == ANTENNA_SW_DIVERSITY)
+               rt2x00dev->default_ant.tx = ANTENNA_HW_DIVERSITY;
+       if (rt2x00dev->default_ant.rx == ANTENNA_SW_DIVERSITY)
+               rt2x00dev->default_ant.rx = ANTENNA_HW_DIVERSITY;
+
        /*
         * Store led mode, for correct led behaviour.
         */
 
        u32 rf4;
 };
 
+/*
+ * Antenna setup values.
+ */
+struct antenna_setup {
+       enum antenna rx;
+       enum antenna tx;
+};
+
 /*
  * Quality statistics about the currently active link.
  */
         */
        struct link_qual qual;
 
+       /*
+        * Currently active TX/RX antenna setup.
+        * When software diversity is used, this will indicate
+        * which antenna is actually used at this time.
+        */
+       struct antenna_setup active_ant;
+
        /*
         * Active VGC level
         */
        link->qual.tx_percentage = 50;
 }
 
-
 /*
  * Update the rssi using the walking average approach.
  */
        struct ieee80211_conf *conf;
        struct rf_channel rf;
 
+       struct antenna_setup ant;
+
        int phymode;
 
        int basic_rates;
         */
        struct hw_mode_spec spec;
 
+       /*
+        * 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.
+        */
+       struct antenna_setup default_ant;
+
        /*
         * Register pointers
         * csr_addr: Base register address. (PCI)
 
        struct rt2x00lib_conf libconf;
        struct ieee80211_hw_mode *mode;
        struct ieee80211_rate *rate;
+       struct antenna_setup *default_ant = &rt2x00dev->default_ant;
+       struct antenna_setup *active_ant = &rt2x00dev->link.active_ant;
        int flags = 0;
        int short_slot_time;
 
                flags |= CONFIG_UPDATE_CHANNEL;
        if (rt2x00dev->tx_power != conf->power_level)
                flags |= CONFIG_UPDATE_TXPOWER;
-       if (rt2x00dev->rx_status.antenna == conf->antenna_sel_rx)
+
+       /*
+        * Determining changes in the antenna setups request several checks:
+        * antenna_sel_{r,t}x = 0
+        *    -> Does active_{r,t}x match default_{r,t}x
+        *    -> Is default_{r,t}x SW_DIVERSITY
+        * antenna_sel_{r,t}x = 1/2
+        *    -> Does active_{r,t}x match antenna_sel_{r,t}x
+        * The reason for not updating the antenna while SW diversity
+        * should be used is simple: Software diversity means that
+        * we should switch between the antenna's based on the
+        * quality. This means that the current antenna is good enough
+        * to work with untill the link tuner decides that an antenna
+        * switch should be performed.
+        */
+       if (!conf->antenna_sel_rx &&
+           default_ant->rx != ANTENNA_SW_DIVERSITY &&
+           default_ant->rx != active_ant->rx)
+               flags |= CONFIG_UPDATE_ANTENNA;
+       else if (conf->antenna_sel_rx &&
+                conf->antenna_sel_rx != active_ant->rx)
+               flags |= CONFIG_UPDATE_ANTENNA;
+
+       if (!conf->antenna_sel_tx &&
+           default_ant->tx != ANTENNA_SW_DIVERSITY &&
+           default_ant->tx != active_ant->tx)
+               flags |= CONFIG_UPDATE_ANTENNA;
+       else if (conf->antenna_sel_tx &&
+                conf->antenna_sel_tx != active_ant->tx)
                flags |= CONFIG_UPDATE_ANTENNA;
 
        /*
                       sizeof(libconf.rf));
        }
 
+       if (flags & CONFIG_UPDATE_ANTENNA) {
+               if (conf->antenna_sel_rx)
+                       libconf.ant.rx = conf->antenna_sel_rx;
+               else if (default_ant->rx != ANTENNA_SW_DIVERSITY)
+                       libconf.ant.rx = default_ant->rx;
+               else if (active_ant->rx == ANTENNA_SW_DIVERSITY)
+                       libconf.ant.rx = ANTENNA_B;
+
+               if (conf->antenna_sel_tx)
+                       libconf.ant.tx = conf->antenna_sel_tx;
+               else if (default_ant->tx != ANTENNA_SW_DIVERSITY)
+                       libconf.ant.tx = default_ant->tx;
+               else if (active_ant->tx == ANTENNA_SW_DIVERSITY)
+                       libconf.ant.tx = ANTENNA_B;
+       }
+
        if (flags & CONFIG_UPDATE_SLOT_TIME) {
                short_slot_time = conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME;
 
        rt2x00dev->rx_status.freq = conf->freq;
        rt2x00dev->rx_status.channel = conf->channel;
        rt2x00dev->tx_power = conf->power_level;
-       rt2x00dev->rx_status.antenna = conf->antenna_sel_rx;
+       rt2x00dev->link.active_ant.rx = libconf.ant.rx;
+       rt2x00dev->link.active_ant.tx = libconf.ant.tx;
 }
 
            rt2x00lib_calculate_link_signal(rt2x00dev, desc->rssi);
        rx_status->ssi = desc->rssi;
        rx_status->flag = desc->flags;
+       rx_status->antenna = rt2x00dev->link.active_ant.rx;
 
        /*
         * Send frame to mac80211
 
 }
 
 static void rt61pci_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
-                                     const int antenna_tx,
-                                     const int antenna_rx)
+                                     struct antenna_setup *ant)
 {
        u8 r3;
        u8 r4;
        rt2x00_set_field8(&r3, BBP_R3_SMART_MODE,
                          !rt2x00_rf(&rt2x00dev->chip, RF5225));
 
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
 }
 
 static void rt61pci_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
-                                     const int antenna_tx,
-                                     const int antenna_rx)
+                                     struct antenna_setup *ant)
 {
        u8 r3;
        u8 r4;
        rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
                          !test_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags));
 
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
 }
 
 static void rt61pci_config_antenna_2529(struct rt2x00_dev *rt2x00dev,
-                                       const int antenna_tx,
-                                       const int antenna_rx)
+                                       struct antenna_setup *ant)
 {
        u16 eeprom;
        u8 r3;
 };
 
 static void rt61pci_config_antenna(struct rt2x00_dev *rt2x00dev,
-                                  const int antenna_tx, const int antenna_rx)
+                                  struct antenna_setup *ant)
 {
        const struct antenna_sel *sel;
        unsigned int lna;
 
        if (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
            rt2x00_rf(&rt2x00dev->chip, RF5325))
-               rt61pci_config_antenna_5x(rt2x00dev, antenna_tx, antenna_rx);
+               rt61pci_config_antenna_5x(rt2x00dev, ant);
        else if (rt2x00_rf(&rt2x00dev->chip, RF2527))
-               rt61pci_config_antenna_2x(rt2x00dev, antenna_tx, antenna_rx);
+               rt61pci_config_antenna_2x(rt2x00dev, ant);
        else if (rt2x00_rf(&rt2x00dev->chip, RF2529)) {
                if (test_bit(CONFIG_DOUBLE_ANTENNA, &rt2x00dev->flags))
-                       rt61pci_config_antenna_2x(rt2x00dev, antenna_tx,
-                                                 antenna_rx);
+                       rt61pci_config_antenna_2x(rt2x00dev, ant);
                else
-                       rt61pci_config_antenna_2529(rt2x00dev, antenna_tx,
-                                                   antenna_rx);
+                       rt61pci_config_antenna_2529(rt2x00dev, ant);
        }
 }
 
        if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
                rt61pci_config_txpower(rt2x00dev, libconf->conf->power_level);
        if (flags & CONFIG_UPDATE_ANTENNA)
-               rt61pci_config_antenna(rt2x00dev, libconf->conf->antenna_sel_tx,
-                                      libconf->conf->antenna_sel_rx);
+               rt61pci_config_antenna(rt2x00dev, &libconf->ant);
        if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
                rt61pci_config_duration(rt2x00dev, libconf);
 }
        /*
         * Identify default antenna configuration.
         */
-       rt2x00dev->hw->conf.antenna_sel_tx =
+       rt2x00dev->default_ant.tx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
-       rt2x00dev->hw->conf.antenna_sel_rx =
+       rt2x00dev->default_ant.rx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
 
        /*
 
 }
 
 static void rt73usb_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
-                                     const int antenna_tx,
-                                     const int antenna_rx)
+                                     struct antenna_setup *ant)
 {
        u8 r3;
        u8 r4;
 
        rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
 
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
 }
 
 static void rt73usb_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
-                                     const int antenna_tx,
-                                     const int antenna_rx)
+                                     struct antenna_setup *ant)
 {
        u8 r3;
        u8 r4;
        rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
                          !test_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags));
 
-       switch (antenna_rx) {
+       switch (ant->rx) {
        case ANTENNA_SW_DIVERSITY:
        case ANTENNA_HW_DIVERSITY:
                rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
 };
 
 static void rt73usb_config_antenna(struct rt2x00_dev *rt2x00dev,
-                                  const int antenna_tx, const int antenna_rx)
+                                  struct antenna_setup *ant)
 {
        const struct antenna_sel *sel;
        unsigned int lna;
 
        if (rt2x00_rf(&rt2x00dev->chip, RF5226) ||
            rt2x00_rf(&rt2x00dev->chip, RF5225))
-               rt73usb_config_antenna_5x(rt2x00dev, antenna_tx, antenna_rx);
+               rt73usb_config_antenna_5x(rt2x00dev, ant);
        else if (rt2x00_rf(&rt2x00dev->chip, RF2528) ||
                 rt2x00_rf(&rt2x00dev->chip, RF2527))
-               rt73usb_config_antenna_2x(rt2x00dev, antenna_tx, antenna_rx);
+               rt73usb_config_antenna_2x(rt2x00dev, ant);
 }
 
 static void rt73usb_config_duration(struct rt2x00_dev *rt2x00dev,
        if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
                rt73usb_config_txpower(rt2x00dev, libconf->conf->power_level);
        if (flags & CONFIG_UPDATE_ANTENNA)
-               rt73usb_config_antenna(rt2x00dev, libconf->conf->antenna_sel_tx,
-                                      libconf->conf->antenna_sel_rx);
+               rt73usb_config_antenna(rt2x00dev, &libconf->ant);
        if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
                rt73usb_config_duration(rt2x00dev, libconf);
 }
        /*
         * Identify default antenna configuration.
         */
-       rt2x00dev->hw->conf.antenna_sel_tx =
+       rt2x00dev->default_ant.tx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
-       rt2x00dev->hw->conf.antenna_sel_rx =
+       rt2x00dev->default_ant.rx =
            rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
 
        /*