32 files changed, 877 insertions, 523 deletions

diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index dda7cc2e1f57..a8871a84d87e 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -2,8 +2,17 @@
 # Wireless LAN device configuration
 #
 
-menu "Wireless LAN"
+menuconfig WLAN
+        bool "Wireless LAN"
         depends on !S390
+        ---help---
+          This section contains all the pre 802.11 and 802.11 wireless
+          device drivers. For a complete list of drivers and documentation
+          on them refer to the wireless wiki:
+
+          http://wireless.kernel.org/en/users/Drivers
+
+if WLAN
 
 menuconfig WLAN_PRE80211
         bool "Wireless LAN (pre-802.11)"
@@ -337,7 +346,6 @@ config USB_NET_RNDIS_WLAN
         select USB_USBNET
         select USB_NET_CDCETHER
         select USB_NET_RNDIS_HOST
-        select WIRELESS_EXT
         ---help---
           This is a driver for wireless RNDIS devices.
           These are USB based adapters found in devices such as:
@@ -506,4 +514,4 @@ source "drivers/net/wireless/orinoco/Kconfig"
 source "drivers/net/wireless/wl12xx/Kconfig"
 source "drivers/net/wireless/iwmc3200wifi/Kconfig"
 
-endmenu
+endif # WLAN
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.c b/drivers/net/wireless/ath/ath9k/btcoex.c
index 8fb356748823..e8bfb01ee78a 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.c
+++ b/drivers/net/wireless/ath/ath9k/btcoex.c
@@ -84,15 +84,14 @@ static void ath_btcoex_period_timer(unsigned long data)
 {
         struct ath_softc *sc = (struct ath_softc *) data;
         struct ath_btcoex_info *btinfo = &sc->btcoex_info;
-        unsigned long flags;
 
         ath_detect_bt_priority(sc);
 
-        spin_lock_irqsave(&btinfo->btcoex_lock, flags);
+        spin_lock_bh(&btinfo->btcoex_lock);
 
         ath_btcoex_bt_stomp(sc, btinfo, btinfo->bt_stomp_type);
 
-        spin_unlock_irqrestore(&btinfo->btcoex_lock, flags);
+        spin_unlock_bh(&btinfo->btcoex_lock);
 
         if (btinfo->btcoex_period != btinfo->btcoex_no_stomp) {
                 if (btinfo->hw_timer_enabled)
@@ -119,18 +118,17 @@ static void ath_btcoex_no_stomp_timer(void *arg)
 {
         struct ath_softc *sc = (struct ath_softc *)arg;
         struct ath_btcoex_info *btinfo = &sc->btcoex_info;
-        unsigned long flags;
 
         DPRINTF(sc, ATH_DBG_BTCOEX, "no stomp timer running \n");
 
-        spin_lock_irqsave(&btinfo->btcoex_lock, flags);
+        spin_lock_bh(&btinfo->btcoex_lock);
 
         if (btinfo->bt_stomp_type == ATH_BTCOEX_STOMP_LOW)
                 ath_btcoex_bt_stomp(sc, btinfo, ATH_BTCOEX_STOMP_NONE);
          else if (btinfo->bt_stomp_type == ATH_BTCOEX_STOMP_ALL)
                 ath_btcoex_bt_stomp(sc, btinfo, ATH_BTCOEX_STOMP_LOW);
 
-        spin_unlock_irqrestore(&btinfo->btcoex_lock, flags);
+        spin_unlock_bh(&btinfo->btcoex_lock);
 }
 
 static int ath_init_btcoex_info(struct ath_hw *hw,
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index e340dacc6ebe..71f27f324cea 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1712,8 +1712,15 @@ static bool ath9k_hw_set_reset_power_on(struct ath_hw *ah)
         REG_WRITE(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_EN |
                   AR_RTC_FORCE_WAKE_ON_INT);
 
+        if (!AR_SREV_9100(ah))
+                REG_WRITE(ah, AR_RC, AR_RC_AHB);
+
         REG_WRITE(ah, AR_RTC_RESET, 0);
         udelay(2);
+
+        if (!AR_SREV_9100(ah))
+                REG_WRITE(ah, AR_RC, 0);
+
         REG_WRITE(ah, AR_RTC_RESET, 1);
 
         if (!ath9k_hw_wait(ah,
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 4fae699a53c2..c2efdf2d72d3 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -506,6 +506,10 @@ static void ath9k_tasklet(unsigned long data)
                 sc->sc_flags |= SC_OP_WAIT_FOR_BEACON | SC_OP_BEACON_SYNC;
         }
 
+        if (sc->btcoex_info.btcoex_scheme == ATH_BTCOEX_CFG_3WIRE)
+                if (status & ATH9K_INT_GENTIMER)
+                        ath_gen_timer_isr(sc->sc_ah);
+
         /* re-enable hardware interrupt */
         ath9k_hw_set_interrupts(sc->sc_ah, sc->imask);
         ath9k_ps_restore(sc);
@@ -521,7 +525,8 @@ irqreturn_t ath_isr(int irq, void *dev)
                 ATH9K_INT_TX |                  \
                 ATH9K_INT_BMISS |               \
                 ATH9K_INT_CST |                 \
-                ATH9K_INT_TSFOOR)
+                ATH9K_INT_TSFOOR |              \
+                ATH9K_INT_GENTIMER)
 
         struct ath_softc *sc = dev;
         struct ath_hw *ah = sc->sc_ah;
@@ -602,10 +607,6 @@ irqreturn_t ath_isr(int irq, void *dev)
                         sc->sc_flags |= SC_OP_WAIT_FOR_BEACON;
                 }
 
-        if (sc->btcoex_info.btcoex_scheme == ATH_BTCOEX_CFG_3WIRE)
-                if (status & ATH9K_INT_GENTIMER)
-                        ath_gen_timer_isr(ah);
-
 chip_reset:
 
         ath_debug_stat_interrupt(sc, status);
@@ -2772,6 +2773,7 @@ static void ath9k_sw_scan_complete(struct ieee80211_hw *hw)
         sc->sc_flags &= ~SC_OP_SCANNING;
         sc->sc_flags |= SC_OP_FULL_RESET;
         spin_unlock_bh(&sc->ani_lock);
+        ath_beacon_config(sc, NULL);
         mutex_unlock(&sc->mutex);
 }
 
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 0f168443ad49..ae05f6671149 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -58,6 +58,7 @@ MODULE_DESCRIPTION("Broadcom B43 wireless driver");
 MODULE_AUTHOR("Martin Langer");
 MODULE_AUTHOR("Stefano Brivio");
 MODULE_AUTHOR("Michael Buesch");
+MODULE_AUTHOR("Gábor Stefanik");
 MODULE_LICENSE("GPL");
 
 MODULE_FIRMWARE(B43_SUPPORTED_FIRMWARE_ID);
@@ -90,7 +91,7 @@ MODULE_PARM_DESC(qos, "Enable QOS support (default on)");
 
 static int modparam_btcoex = 1;
 module_param_named(btcoex, modparam_btcoex, int, 0444);
-MODULE_PARM_DESC(btcoex, "Enable Bluetooth coexistance (default on)");
+MODULE_PARM_DESC(btcoex, "Enable Bluetooth coexistence (default on)");
 
 int b43_modparam_verbose = B43_VERBOSITY_DEFAULT;
 module_param_named(verbose, b43_modparam_verbose, int, 0644);
diff --git a/drivers/net/wireless/b43/phy_a.c b/drivers/net/wireless/b43/phy_a.c
index 809ec97031cb..d90217c3a706 100644
--- a/drivers/net/wireless/b43/phy_a.c
+++ b/drivers/net/wireless/b43/phy_a.c
@@ -518,58 +518,40 @@ static unsigned int b43_aphy_op_get_default_chan(struct b43_wldev *dev)
 static void b43_aphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
 {//TODO
         struct b43_phy *phy = &dev->phy;
-        u64 hf;
         u16 tmp;
         int autodiv = 0;
 
         if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
                 autodiv = 1;
 
-        hf = b43_hf_read(dev);
-        hf &= ~B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
+        b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ANTDIVHELP);
 
-        tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
-        tmp &= ~B43_PHY_BBANDCFG_RXANT;
-        tmp |= (autodiv ? B43_ANTENNA_AUTO1 : antenna)
-            << B43_PHY_BBANDCFG_RXANT_SHIFT;
-        b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
+        b43_phy_maskset(dev, B43_PHY_BBANDCFG, ~B43_PHY_BBANDCFG_RXANT,
+                        (autodiv ? B43_ANTENNA_AUTO1 : antenna) <<
+                        B43_PHY_BBANDCFG_RXANT_SHIFT);
 
         if (autodiv) {
                 tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
-                if (antenna == B43_ANTENNA_AUTO0)
+                if (antenna == B43_ANTENNA_AUTO1)
                         tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
                 else
                         tmp |= B43_PHY_ANTDWELL_AUTODIV1;
                 b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
         }
-        if (phy->rev < 3) {
-                tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
-                tmp = (tmp & 0xFF00) | 0x24;
-                b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
-        } else {
-                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
-                tmp |= 0x10;
-                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
-                if (phy->analog == 3) {
-                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
-                                      0x1D);
-                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                      8);
+        if (phy->rev < 3)
+                b43_phy_maskset(dev, B43_PHY_ANTDWELL, 0xFF00, 0x24);
+        else {
+                b43_phy_set(dev, B43_PHY_OFDM61, 0x10);
+                if (phy->rev == 3) {
+                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT, 0x1D);
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED, 8);
                 } else {
-                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
-                                      0x3A);
-                        tmp =
-                            b43_phy_read(dev,
-                                         B43_PHY_ADIVRELATED);
-                        tmp = (tmp & 0xFF00) | 8;
-                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                      tmp);
+                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT, 0x3A);
+                        b43_phy_maskset(dev, B43_PHY_ADIVRELATED, 0xFF00, 8);
                 }
         }
 
-        hf |= B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
+        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ANTDIVHELP);
 }
 
 static void b43_aphy_op_adjust_txpower(struct b43_wldev *dev)
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/b43/phy_g.c
index bdff9afe3f81..5afa4df0b02f 100644
--- a/drivers/net/wireless/b43/phy_g.c
+++ b/drivers/net/wireless/b43/phy_g.c
@@ -2651,65 +2651,54 @@ static unsigned int b43_gphy_op_get_default_chan(struct b43_wldev *dev)
 static void b43_gphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
 {
         struct b43_phy *phy = &dev->phy;
-        u64 hf;
         u16 tmp;
         int autodiv = 0;
 
         if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
                 autodiv = 1;
 
-        hf = b43_hf_read(dev);
-        hf &= ~B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
+        b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ANTDIVHELP);
 
-        tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
-        tmp &= ~B43_PHY_BBANDCFG_RXANT;
-        tmp |= (autodiv ? B43_ANTENNA_AUTO1 : antenna)
-                        << B43_PHY_BBANDCFG_RXANT_SHIFT;
-        b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
+        b43_phy_maskset(dev, B43_PHY_BBANDCFG, ~B43_PHY_BBANDCFG_RXANT,
+                        (autodiv ? B43_ANTENNA_AUTO1 : antenna) <<
+                        B43_PHY_BBANDCFG_RXANT_SHIFT);
 
         if (autodiv) {
                 tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
-                if (antenna == B43_ANTENNA_AUTO0)
+                if (antenna == B43_ANTENNA_AUTO1)
                         tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
                 else
                         tmp |= B43_PHY_ANTDWELL_AUTODIV1;
                 b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
         }
+
         tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
         if (autodiv)
                 tmp |= B43_PHY_ANTWRSETT_ARXDIV;
         else
                 tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
         b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
-        if (phy->rev >= 2) {
-                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
-                tmp |= B43_PHY_OFDM61_10;
-                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
 
-                tmp =
-                    b43_phy_read(dev, B43_PHY_DIVSRCHGAINBACK);
-                tmp = (tmp & 0xFF00) | 0x15;
-                b43_phy_write(dev, B43_PHY_DIVSRCHGAINBACK,
-                              tmp);
+        if (autodiv)
+                b43_phy_set(dev, B43_PHY_ANTWRSETT, B43_PHY_ANTWRSETT_ARXDIV);
+        else {
+                b43_phy_mask(dev, B43_PHY_ANTWRSETT,
+                             B43_PHY_ANTWRSETT_ARXDIV);
+        }
 
-                if (phy->rev == 2) {
-                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                      8);
-                } else {
-                        tmp =
-                            b43_phy_read(dev,
-                                         B43_PHY_ADIVRELATED);
-                        tmp = (tmp & 0xFF00) | 8;
-                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                      tmp);
-                }
+        if (phy->rev >= 2) {
+                b43_phy_set(dev, B43_PHY_OFDM61, B43_PHY_OFDM61_10);
+                b43_phy_maskset(dev, B43_PHY_DIVSRCHGAINBACK, 0xFF00, 0x15);
+
+                if (phy->rev == 2)
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED, 8);
+                else
+                        b43_phy_maskset(dev, B43_PHY_ADIVRELATED, 0xFF00, 8);
         }
         if (phy->rev >= 6)
                 b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
 
-        hf |= B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
+        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ANTDIVHELP);
 }
 
 static int b43_gphy_op_interf_mitigation(struct b43_wldev *dev,
diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index 1ab00b034cbd..3e02d969f683 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -1,9 +1,10 @@
 /*
 
   Broadcom B43 wireless driver
-  IEEE 802.11g LP-PHY driver
+  IEEE 802.11a/g LP-PHY driver
 
   Copyright (c) 2008-2009 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2009 Gábor Stefanik <netrolller.3d@gmail.com>
 
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
diff --git a/drivers/net/wireless/b43/tables_lpphy.c b/drivers/net/wireless/b43/tables_lpphy.c
index c784def19b19..61027ee84fb5 100644
--- a/drivers/net/wireless/b43/tables_lpphy.c
+++ b/drivers/net/wireless/b43/tables_lpphy.c
@@ -1,9 +1,10 @@
 /*
 
   Broadcom B43 wireless driver
-  IEEE 802.11g LP-PHY and radio device data tables
+  IEEE 802.11a/g LP-PHY and radio device data tables
 
   Copyright (c) 2009 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2009 Gábor Stefanik <netrolller.3d@gmail.com>
 
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -1612,11 +1613,62 @@ static struct lpphy_tx_gain_table_entry lpphy_rev1_nopa_tx_gain_table[] = {
 };
 
 static struct lpphy_tx_gain_table_entry lpphy_rev1_2ghz_tx_gain_table[] = {
-        { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 85, },
-        { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 81, },
-        { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 78, },
-        { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 76, },
-        { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 74, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 90, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 88, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 85, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 83, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 81, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 78, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 76, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 74, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 15, .dac = 0, .bb_mult = 59, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 14, .dac = 0, .bb_mult = 59, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 13, .dac = 0, .bb_mult = 59, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 12, .dac = 0, .bb_mult = 59, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 11, .dac = 0, .bb_mult = 59, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 72, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 70, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 68, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 66, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 64, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 62, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 60, },
+        { .gm = 4, .pga = 15, .pad = 10, .dac = 0, .bb_mult = 59, },
         { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 72, },
         { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 70, },
         { .gm = 4, .pga = 15, .pad = 9, .dac = 0, .bb_mult = 68, },
@@ -1689,57 +1741,6 @@ static struct lpphy_tx_gain_table_entry lpphy_rev1_2ghz_tx_gain_table[] = {
         { .gm = 4, .pga = 10, .pad = 6, .dac = 0, .bb_mult = 64, },
         { .gm = 4, .pga = 10, .pad = 6, .dac = 0, .bb_mult = 62, },
         { .gm = 4, .pga = 10, .pad = 6, .dac = 0, .bb_mult = 60, },
-        { .gm = 4, .pga = 10, .pad = 6, .dac = 0, .bb_mult = 59, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 72, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 70, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 68, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 66, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 64, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 62, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 60, },
-        { .gm = 4, .pga = 10, .pad = 5, .dac = 0, .bb_mult = 59, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 70, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 68, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 66, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 64, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 63, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 61, },
-        { .gm = 4, .pga = 9, .pad = 5, .dac = 0, .bb_mult = 59, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 71, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 69, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 67, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 65, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 63, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 62, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 60, },
-        { .gm = 4, .pga = 9, .pad = 4, .dac = 0, .bb_mult = 58, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 70, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 68, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 66, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 65, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 63, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 61, },
-        { .gm = 4, .pga = 8, .pad = 4, .dac = 0, .bb_mult = 59, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 68, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 66, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 64, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 62, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 61, },
-        { .gm = 4, .pga = 7, .pad = 4, .dac = 0, .bb_mult = 59, },
-        { .gm = 4, .pga = 7, .pad = 3, .dac = 0, .bb_mult = 67, },
-        { .gm = 4, .pga = 7, .pad = 3, .dac = 0, .bb_mult = 65, },
-        { .gm = 4, .pga = 7, .pad = 3, .dac = 0, .bb_mult = 63, },
-        { .gm = 4, .pga = 7, .pad = 3, .dac = 0, .bb_mult = 62, },
-        { .gm = 4, .pga = 7, .pad = 3, .dac = 0, .bb_mult = 60, },
-        { .gm = 4, .pga = 6, .pad = 3, .dac = 0, .bb_mult = 65, },
-        { .gm = 4, .pga = 6, .pad = 3, .dac = 0, .bb_mult = 63, },
-        { .gm = 4, .pga = 6, .pad = 3, .dac = 0, .bb_mult = 61, },
-        { .gm = 4, .pga = 6, .pad = 3, .dac = 0, .bb_mult = 60, },
-        { .gm = 4, .pga = 6, .pad = 3, .dac = 0, .bb_mult = 58, },
-        { .gm = 4, .pga = 5, .pad = 3, .dac = 0, .bb_mult = 68, },
-        { .gm = 4, .pga = 5, .pad = 3, .dac = 0, .bb_mult = 66, },
-        { .gm = 4, .pga = 5, .pad = 3, .dac = 0, .bb_mult = 64, },
-        { .gm = 4, .pga = 5, .pad = 3, .dac = 0, .bb_mult = 62, },
 };
 
 static struct lpphy_tx_gain_table_entry lpphy_rev1_5ghz_tx_gain_table[] = {
@@ -2167,103 +2168,103 @@ static struct lpphy_tx_gain_table_entry lpphy_rev2_5ghz_tx_gain_table[] = {
         { .gm = 255, .pga = 255, .pad = 255, .dac = 0, .bb_mult = 68, },
         { .gm = 255, .pga = 255, .pad = 255, .dac = 0, .bb_mult = 66, },
         { .gm = 255, .pga = 255, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 248, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 241, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 234, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 227, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 221, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 215, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 208, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 203, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 197, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 191, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 186, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 181, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 175, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 170, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 166, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 161, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 156, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 152, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 148, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 143, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 139, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 135, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 132, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 128, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 124, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 121, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 117, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 114, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 111, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 108, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 104, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 102, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 99, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 96, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 93, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 90, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 88, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 85, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 83, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 81, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 78, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 76, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 74, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 72, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 70, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 68, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 66, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 64, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 64, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 62, .pad = 255, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 62, .pad = 248, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 60, .pad = 248, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 60, .pad = 241, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 59, .pad = 241, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 59, .pad = 234, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 57, .pad = 234, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 57, .pad = 227, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 55, .pad = 227, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 55, .pad = 221, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 54, .pad = 221, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 54, .pad = 215, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 52, .pad = 215, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 52, .pad = 208, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 51, .pad = 208, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 51, .pad = 203, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 49, .pad = 203, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 49, .pad = 197, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 48, .pad = 197, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 48, .pad = 191, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 47, .pad = 191, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 47, .pad = 186, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 45, .pad = 186, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 45, .pad = 181, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 44, .pad = 181, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 44, .pad = 175, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 43, .pad = 175, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 43, .pad = 170, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 42, .pad = 170, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 42, .pad = 166, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 40, .pad = 166, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 40, .pad = 161, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 39, .pad = 161, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 39, .pad = 156, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 38, .pad = 156, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 38, .pad = 152, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 37, .pad = 152, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 37, .pad = 148, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 36, .pad = 148, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 36, .pad = 143, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 35, .pad = 143, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 35, .pad = 139, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 34, .pad = 139, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 34, .pad = 135, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 33, .pad = 135, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 33, .pad = 132, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 32, .pad = 132, .dac = 0, .bb_mult = 64, },
-        { .gm = 255, .pga = 32, .pad = 128, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 248, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 241, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 234, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 227, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 221, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 215, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 208, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 203, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 197, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 191, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 186, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 181, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 175, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 170, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 166, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 161, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 156, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 152, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 148, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 143, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 139, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 135, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 132, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 128, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 124, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 121, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 117, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 114, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 111, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 108, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 104, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 102, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 99, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 96, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 93, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 90, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 88, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 85, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 83, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 81, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 78, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 76, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 74, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 72, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 70, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 68, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 66, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 64, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 64, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 255, .pad = 62, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 248, .pad = 62, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 248, .pad = 60, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 241, .pad = 60, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 241, .pad = 59, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 234, .pad = 59, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 234, .pad = 57, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 227, .pad = 57, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 227, .pad = 55, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 221, .pad = 55, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 221, .pad = 54, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 215, .pad = 54, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 215, .pad = 52, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 208, .pad = 52, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 208, .pad = 51, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 203, .pad = 51, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 203, .pad = 49, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 197, .pad = 49, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 197, .pad = 48, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 191, .pad = 48, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 191, .pad = 47, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 186, .pad = 47, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 186, .pad = 45, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 181, .pad = 45, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 181, .pad = 44, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 175, .pad = 44, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 175, .pad = 43, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 170, .pad = 43, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 170, .pad = 42, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 166, .pad = 42, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 166, .pad = 40, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 161, .pad = 40, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 161, .pad = 39, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 156, .pad = 39, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 156, .pad = 38, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 152, .pad = 38, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 152, .pad = 37, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 148, .pad = 37, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 148, .pad = 36, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 143, .pad = 36, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 143, .pad = 35, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 139, .pad = 35, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 139, .pad = 34, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 135, .pad = 34, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 135, .pad = 33, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 132, .pad = 33, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 132, .pad = 32, .dac = 0, .bb_mult = 64, },
+        { .gm = 255, .pga = 128, .pad = 32, .dac = 0, .bb_mult = 64, },
 };
 
 void lpphy_rev0_1_table_init(struct b43_wldev *dev)
diff --git a/drivers/net/wireless/ipw2x00/Kconfig b/drivers/net/wireless/ipw2x00/Kconfig
index 85cc79995f6f..a8131384c6b9 100644
--- a/drivers/net/wireless/ipw2x00/Kconfig
+++ b/drivers/net/wireless/ipw2x00/Kconfig
@@ -4,7 +4,7 @@
 
 config IPW2100
         tristate "Intel PRO/Wireless 2100 Network Connection"
-        depends on PCI && WLAN_80211
+        depends on PCI && WLAN_80211 && CFG80211
         select WIRELESS_EXT
         select FW_LOADER
         select LIB80211
@@ -63,7 +63,7 @@ config IPW2100_DEBUG
 
 config IPW2200
         tristate "Intel PRO/Wireless 2200BG and 2915ABG Network Connection"
-        depends on PCI && WLAN_80211
+        depends on PCI && WLAN_80211 && CFG80211
         select WIRELESS_EXT
         select FW_LOADER
         select LIB80211
@@ -150,7 +150,7 @@ config IPW2200_DEBUG
 
 config LIBIPW
         tristate
-        depends on PCI && WLAN_80211
+        depends on PCI && WLAN_80211 && CFG80211
         select WIRELESS_EXT
         select CRYPTO
         select CRYPTO_ARC4
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
index 26ec969e265d..40b207aa8fef 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
@@ -818,15 +818,15 @@ static void rs_tx_status(void *priv_r, struct ieee80211_supported_band *sband,
 {
         int status;
         u8 retries;
-        int rs_index, index = 0;
+        int rs_index, mac_index, index = 0;
         struct iwl_lq_sta *lq_sta = priv_sta;
         struct iwl_link_quality_cmd *table;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         struct iwl_priv *priv = (struct iwl_priv *)priv_r;
-        struct ieee80211_hw *hw = priv->hw;
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct iwl_rate_scale_data *window = NULL;
         struct iwl_rate_scale_data *search_win = NULL;
+        enum mac80211_rate_control_flags mac_flags;
         u32 tx_rate;
         struct iwl_scale_tbl_info tbl_type;
         struct iwl_scale_tbl_info *curr_tbl, *search_tbl;
@@ -876,17 +876,24 @@ static void rs_tx_status(void *priv_r, struct ieee80211_supported_band *sband,
         rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
         if (priv->band == IEEE80211_BAND_5GHZ)
                 rs_index -= IWL_FIRST_OFDM_RATE;
+        mac_flags = info->status.rates[0].flags;
+        mac_index = info->status.rates[0].idx;
+        /* For HT packets, map MCS to PLCP */
+        if (mac_flags & IEEE80211_TX_RC_MCS) {
+                mac_index &= RATE_MCS_CODE_MSK; /* Remove # of streams */
+                if (mac_index >= (IWL_RATE_9M_INDEX - IWL_FIRST_OFDM_RATE))
+                        mac_index++;
+        }
 
-        if ((info->status.rates[0].idx < 0) ||
-            (tbl_type.is_SGI != !!(info->status.rates[0].flags & IEEE80211_TX_RC_SHORT_GI)) ||
-            (tbl_type.is_ht40 != !!(info->status.rates[0].flags & IEEE80211_TX_RC_40_MHZ_WIDTH)) ||
-            (tbl_type.is_dup != !!(info->status.rates[0].flags & IEEE80211_TX_RC_DUP_DATA)) ||
+        if ((mac_index < 0) ||
+            (tbl_type.is_SGI != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
+            (tbl_type.is_ht40 != !!(mac_flags & IEEE80211_TX_RC_40_MHZ_WIDTH)) ||
+            (tbl_type.is_dup != !!(mac_flags & IEEE80211_TX_RC_DUP_DATA)) ||
             (tbl_type.ant_type != info->antenna_sel_tx) ||
-            (!!(tx_rate & RATE_MCS_HT_MSK) != !!(info->status.rates[0].flags & IEEE80211_TX_RC_MCS)) ||
-            (!!(tx_rate & RATE_MCS_GF_MSK) != !!(info->status.rates[0].flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
-            (hw->wiphy->bands[priv->band]->bitrates[rs_index].bitrate !=
-             hw->wiphy->bands[info->band]->bitrates[info->status.rates[0].idx].bitrate)) {
-                IWL_DEBUG_RATE(priv, "initial rate does not match 0x%x\n", tx_rate);
+            (!!(tx_rate & RATE_MCS_HT_MSK) != !!(mac_flags & IEEE80211_TX_RC_MCS)) ||
+            (!!(tx_rate & RATE_MCS_GF_MSK) != !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
+            (rs_index != mac_index)) {
+                IWL_DEBUG_RATE(priv, "initial rate %d does not match %d (0x%x)\n", mac_index, rs_index, tx_rate);
                 /* the last LQ command could failed so the LQ in ucode not
                  * the same in driver sync up
                  */
@@ -2542,8 +2549,12 @@ static void rs_get_rate(void *priv_r, struct ieee80211_sta *sta, void *priv_sta,
                 if (lq_sta->last_rate_n_flags & RATE_MCS_GF_MSK)
                         info->control.rates[0].flags |= IEEE80211_TX_RC_GREEN_FIELD;
         } else {
-                if (rate_idx < 0 || rate_idx > IWL_RATE_COUNT)
+                /* Check for invalid rates */
+                if ((rate_idx < 0) || (rate_idx >= IWL_RATE_COUNT_LEGACY) ||
+                                ((sband->band == IEEE80211_BAND_5GHZ) &&
+                                 (rate_idx < IWL_FIRST_OFDM_RATE)))
                         rate_idx = rate_lowest_index(sband, sta);
+                /* On valid 5 GHz rate, adjust index */
                 else if (sband->band == IEEE80211_BAND_5GHZ)
                         rate_idx -= IWL_FIRST_OFDM_RATE;
                 info->control.rates[0].flags = 0;
@@ -2584,9 +2595,6 @@ static void rs_rate_init(void *priv_r, struct ieee80211_supported_band *sband,
         struct ieee80211_conf *conf = &priv->hw->conf;
         struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
         struct iwl_lq_sta *lq_sta = priv_sta;
-        u16 mask_bit = 0;
-        int count;
-        int start_rate = 0;
 
         lq_sta->flush_timer = 0;
         lq_sta->supp_rates = sta->supp_rates[sband->band];
@@ -2661,20 +2669,10 @@ static void rs_rate_init(void *priv_r, struct ieee80211_supported_band *sband,
         lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
         lq_sta->drv = priv;
 
-        /* Find highest tx rate supported by hardware and destination station */
-        mask_bit = sta->supp_rates[sband->band];
-        count = sband->n_bitrates;
-        if (sband->band == IEEE80211_BAND_5GHZ) {
-                count += IWL_FIRST_OFDM_RATE;
-                start_rate = IWL_FIRST_OFDM_RATE;
-                mask_bit <<= IWL_FIRST_OFDM_RATE;
-        }
-
-        mask_bit = mask_bit & lq_sta->active_legacy_rate;
-        lq_sta->last_txrate_idx = 4;
-        for (i = start_rate; i < count; i++)
-                if (mask_bit & BIT(i))
-                        lq_sta->last_txrate_idx = i;
+        /* Set last_txrate_idx to lowest rate */
+        lq_sta->last_txrate_idx = rate_lowest_index(sband, sta);
+        if (sband->band == IEEE80211_BAND_5GHZ)
+                lq_sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
 
         rs_initialize_lq(priv, conf, sta, lq_sta);
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
index 25050bf315a2..9fac530cfb7e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
@@ -73,6 +73,7 @@ enum {
         IWL_RATE_54M_INDEX,
         IWL_RATE_60M_INDEX,
         IWL_RATE_COUNT, /*FIXME:RS:change to IWL_RATE_INDEX_COUNT,*/
+        IWL_RATE_COUNT_LEGACY = IWL_RATE_COUNT - 1,     /* Excluding 60M */
         IWL_RATE_COUNT_3945 = IWL_RATE_COUNT - 1,
         IWL_RATE_INVM_INDEX = IWL_RATE_COUNT,
         IWL_RATE_INVALID = IWL_RATE_COUNT,
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 0bfd4e918139..acfd7b40afb8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -96,7 +96,6 @@ EXPORT_SYMBOL(iwl_rates);
 void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
                                   struct ieee80211_tx_info *info)
 {
-        int rate_index;
         struct ieee80211_tx_rate *r = &info->control.rates[0];
 
         info->antenna_sel_tx =
@@ -111,10 +110,7 @@ void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
                 r->flags |= IEEE80211_TX_RC_DUP_DATA;
         if (rate_n_flags & RATE_MCS_SGI_MSK)
                 r->flags |= IEEE80211_TX_RC_SHORT_GI;
-        rate_index = iwl_hwrate_to_plcp_idx(rate_n_flags);
-        if (info->band == IEEE80211_BAND_5GHZ)
-                rate_index -= IWL_FIRST_OFDM_RATE;
-        r->idx = rate_index;
+        r->idx = iwl_hwrate_to_mac80211_idx(rate_n_flags, info->band);
 }
 EXPORT_SYMBOL(iwl_hwrate_to_tx_control);
 
@@ -149,6 +145,27 @@ int iwl_hwrate_to_plcp_idx(u32 rate_n_flags)
 }
 EXPORT_SYMBOL(iwl_hwrate_to_plcp_idx);
 
+int iwl_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band)
+{
+        int idx = 0;
+        int band_offset = 0;
+
+        /* HT rate format: mac80211 wants an MCS number, which is just LSB */
+        if (rate_n_flags & RATE_MCS_HT_MSK) {
+                idx = (rate_n_flags & 0xff);
+                return idx;
+        /* Legacy rate format, search for match in table */
+        } else {
+                if (band == IEEE80211_BAND_5GHZ)
+                        band_offset = IWL_FIRST_OFDM_RATE;
+                for (idx = band_offset; idx < IWL_RATE_COUNT_LEGACY; idx++)
+                        if (iwl_rates[idx].plcp == (rate_n_flags & 0xFF))
+                                return idx - band_offset;
+        }
+
+        return -1;
+}
+
 u8 iwl_toggle_tx_ant(struct iwl_priv *priv, u8 ant)
 {
         int i;
@@ -439,12 +456,12 @@ static void iwlcore_init_hw_rates(struct iwl_priv *priv,
 {
         int i;
 
-        for (i = 0; i < IWL_RATE_COUNT; i++) {
+        for (i = 0; i < IWL_RATE_COUNT_LEGACY; i++) {
                 rates[i].bitrate = iwl_rates[i].ieee * 5;
                 rates[i].hw_value = i; /* Rate scaling will work on indexes */
                 rates[i].hw_value_short = i;
                 rates[i].flags = 0;
-                if ((i > IWL_LAST_OFDM_RATE) || (i < IWL_FIRST_OFDM_RATE)) {
+                if ((i >= IWL_FIRST_CCK_RATE) && (i <= IWL_LAST_CCK_RATE)) {
                         /*
                          * If CCK != 1M then set short preamble rate flag.
                          */
@@ -480,7 +497,7 @@ int iwlcore_init_geos(struct iwl_priv *priv)
         if (!channels)
                 return -ENOMEM;
 
-        rates = kzalloc((sizeof(struct ieee80211_rate) * (IWL_RATE_COUNT + 1)),
+        rates = kzalloc((sizeof(struct ieee80211_rate) * IWL_RATE_COUNT_LEGACY),
                         GFP_KERNEL);
         if (!rates) {
                 kfree(channels);
@@ -492,7 +509,7 @@ int iwlcore_init_geos(struct iwl_priv *priv)
         sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
         /* just OFDM */
         sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
-        sband->n_bitrates = IWL_RATE_COUNT - IWL_FIRST_OFDM_RATE;
+        sband->n_bitrates = IWL_RATE_COUNT_LEGACY - IWL_FIRST_OFDM_RATE;
 
         if (priv->cfg->sku & IWL_SKU_N)
                 iwlcore_init_ht_hw_capab(priv, &sband->ht_cap,
@@ -502,7 +519,7 @@ int iwlcore_init_geos(struct iwl_priv *priv)
         sband->channels = channels;
         /* OFDM & CCK */
         sband->bitrates = rates;
-        sband->n_bitrates = IWL_RATE_COUNT;
+        sband->n_bitrates = IWL_RATE_COUNT_LEGACY;
 
         if (priv->cfg->sku & IWL_SKU_N)
                 iwlcore_init_ht_hw_capab(priv, &sband->ht_cap,
@@ -1231,7 +1248,7 @@ static void iwl_set_rate(struct iwl_priv *priv)
 
         for (i = 0; i < hw->n_bitrates; i++) {
                 rate = &(hw->bitrates[i]);
-                if (rate->hw_value < IWL_RATE_COUNT)
+                if (rate->hw_value < IWL_RATE_COUNT_LEGACY)
                         priv->active_rate |= (1 << rate->hw_value);
         }
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index 62d90364b61d..c04d2a270819 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -423,6 +423,7 @@ int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force);
 void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
                               struct ieee80211_tx_info *info);
 int iwl_hwrate_to_plcp_idx(u32 rate_n_flags);
+int iwl_hwrate_to_mac80211_idx(u32 rate_n_flags, enum ieee80211_band band);
 
 u8 iwl_rate_get_lowest_plcp(struct iwl_priv *priv);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
index 0b16841f45f4..4ec6a8307cc6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -216,8 +216,27 @@ static void iwl_power_fill_sleep_cmd(struct iwl_priv *priv,
                                      struct iwl_powertable_cmd *cmd,
                                      int dynps_ms, int wakeup_period)
 {
+        /*
+         * These are the original power level 3 sleep successions. The
+         * device may behave better with such succession and was also
+         * only tested with that. Just like the original sleep commands,
+         * also adjust the succession here to the wakeup_period below.
+         * The ranges are the same as for the sleep commands, 0-2, 3-9
+         * and >10, which is selected based on the DTIM interval for
+         * the sleep index but here we use the wakeup period since that
+         * is what we need to do for the latency requirements.
+         */
+        static const u8 slp_succ_r0[IWL_POWER_VEC_SIZE] = { 2, 2, 2, 2, 2 };
+        static const u8 slp_succ_r1[IWL_POWER_VEC_SIZE] = { 2, 4, 6, 7, 9 };
+        static const u8 slp_succ_r2[IWL_POWER_VEC_SIZE] = { 2, 7, 9, 9, 0xFF };
+        const u8 *slp_succ = slp_succ_r0;
         int i;
 
+        if (wakeup_period > IWL_DTIM_RANGE_0_MAX)
+                slp_succ = slp_succ_r1;
+        if (wakeup_period > IWL_DTIM_RANGE_1_MAX)
+                slp_succ = slp_succ_r2;
+
         memset(cmd, 0, sizeof(*cmd));
 
         cmd->flags = IWL_POWER_DRIVER_ALLOW_SLEEP_MSK |
@@ -230,7 +249,8 @@ static void iwl_power_fill_sleep_cmd(struct iwl_priv *priv,
         cmd->tx_data_timeout = cpu_to_le32(1000 * dynps_ms);
 
         for (i = 0; i < IWL_POWER_VEC_SIZE; i++)
-                cmd->sleep_interval[i] = cpu_to_le32(wakeup_period);
+                cmd->sleep_interval[i] =
+                        cpu_to_le32(min_t(int, slp_succ[i], wakeup_period));
 
         IWL_DEBUG_POWER(priv, "Automatic sleep command\n");
 }
@@ -301,7 +321,7 @@ int iwl_power_update_mode(struct iwl_priv *priv, bool force)
                         if (priv->cfg->ops->lib->update_chain_flags &&
                             update_chains)
                                 priv->cfg->ops->lib->update_chain_flags(priv);
-                        else
+                        else if (priv->cfg->ops->lib->update_chain_flags)
                                 IWL_DEBUG_POWER(priv,
                                         "Cannot update the power, chain noise "
                                         "calibration running: %d\n",
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
index e34d3fcb6c3d..8150c5c3a16b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -962,6 +962,9 @@ void iwl_rx_reply_rx(struct iwl_priv *priv,
                 return;
         }
 
+        /* This will be used in several places later */
+        rate_n_flags = le32_to_cpu(phy_res->rate_n_flags);
+
         /* rx_status carries information about the packet to mac80211 */
         rx_status.mactime = le64_to_cpu(phy_res->timestamp);
         rx_status.freq =
@@ -969,10 +972,7 @@ void iwl_rx_reply_rx(struct iwl_priv *priv,
         rx_status.band = (phy_res->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
                                 IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
         rx_status.rate_idx =
-                iwl_hwrate_to_plcp_idx(le32_to_cpu(phy_res->rate_n_flags));
-        if (rx_status.band == IEEE80211_BAND_5GHZ)
-                rx_status.rate_idx -= IWL_FIRST_OFDM_RATE;
-
+                iwl_hwrate_to_mac80211_idx(rate_n_flags, rx_status.band);
         rx_status.flag = 0;
 
         /* TSF isn't reliable. In order to allow smooth user experience,
@@ -1034,7 +1034,6 @@ void iwl_rx_reply_rx(struct iwl_priv *priv,
                 rx_status.flag |= RX_FLAG_SHORTPRE;
 
         /* Set up the HT phy flags */
-        rate_n_flags = le32_to_cpu(phy_res->rate_n_flags);
         if (rate_n_flags & RATE_MCS_HT_MSK)
                 rx_status.flag |= RX_FLAG_HT;
         if (rate_n_flags & RATE_MCS_HT40_MSK)
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index 7bc9c0039f79..a7422e52d883 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -566,62 +566,81 @@ static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
 static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
                               struct iwl_tx_cmd *tx_cmd,
                               struct ieee80211_tx_info *info,
-                              __le16 fc, int sta_id,
-                              int is_hcca)
+                              __le16 fc, int is_hcca)
 {
-        u32 rate_flags = 0;
+        u32 rate_flags;
         int rate_idx;
-        u8 rts_retry_limit = 0;
-        u8 data_retry_limit = 0;
+        u8 rts_retry_limit;
+        u8 data_retry_limit;
         u8 rate_plcp;
 
-        rate_idx = min(ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xffff,
-                        IWL_RATE_COUNT - 1);
-
-        rate_plcp = iwl_rates[rate_idx].plcp;
-
-        rts_retry_limit = (is_hcca) ?
-            RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
-
-        if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
-                rate_flags |= RATE_MCS_CCK_MSK;
-
-
-        if (ieee80211_is_probe_resp(fc)) {
-                data_retry_limit = 3;
-                if (data_retry_limit < rts_retry_limit)
-                        rts_retry_limit = data_retry_limit;
-        } else
-                data_retry_limit = IWL_DEFAULT_TX_RETRY;
-
+        /* Set retry limit on DATA packets and Probe Responses*/
         if (priv->data_retry_limit != -1)
                 data_retry_limit = priv->data_retry_limit;
+        else if (ieee80211_is_probe_resp(fc))
+                data_retry_limit = 3;
+        else
+                data_retry_limit = IWL_DEFAULT_TX_RETRY;
+        tx_cmd->data_retry_limit = data_retry_limit;
 
+        /* Set retry limit on RTS packets */
+        rts_retry_limit = (is_hcca) ?  RTS_HCCA_RETRY_LIMIT :
+                RTS_DFAULT_RETRY_LIMIT;
+        if (data_retry_limit < rts_retry_limit)
+                rts_retry_limit = data_retry_limit;
+        tx_cmd->rts_retry_limit = rts_retry_limit;
 
+        /* DATA packets will use the uCode station table for rate/antenna
+         * selection */
         if (ieee80211_is_data(fc)) {
                 tx_cmd->initial_rate_index = 0;
                 tx_cmd->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
-        } else {
-                switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-                case cpu_to_le16(IEEE80211_STYPE_AUTH):
-                case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
-                case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
-                case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
-                        if (tx_cmd->tx_flags & TX_CMD_FLG_RTS_MSK) {
-                                tx_cmd->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-                                tx_cmd->tx_flags |= TX_CMD_FLG_CTS_MSK;
-                        }
-                        break;
-                default:
-                        break;
-                }
+                return;
+        }
+
+        /**
+         * If the current TX rate stored in mac80211 has the MCS bit set, it's
+         * not really a TX rate.  Thus, we use the lowest supported rate for
+         * this band.  Also use the lowest supported rate if the stored rate
+         * index is invalid.
+         */
+        rate_idx = info->control.rates[0].idx;
+        if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS ||
+                        (rate_idx < 0) || (rate_idx > IWL_RATE_COUNT_LEGACY))
+                rate_idx = rate_lowest_index(&priv->bands[info->band],
+                                info->control.sta);
+        /* For 5 GHZ band, remap mac80211 rate indices into driver indices */
+        if (info->band == IEEE80211_BAND_5GHZ)
+                rate_idx += IWL_FIRST_OFDM_RATE;
+        /* Get PLCP rate for tx_cmd->rate_n_flags */
+        rate_plcp = iwl_rates[rate_idx].plcp;
+        /* Zero out flags for this packet */
+        rate_flags = 0;
 
-                priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant);
-                rate_flags |= iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
+        /* Set CCK flag as needed */
+        if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
+                rate_flags |= RATE_MCS_CCK_MSK;
+
+        /* Set up RTS and CTS flags for certain packets */
+        switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+        case cpu_to_le16(IEEE80211_STYPE_AUTH):
+        case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
+        case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
+        case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
+                if (tx_cmd->tx_flags & TX_CMD_FLG_RTS_MSK) {
+                        tx_cmd->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+                        tx_cmd->tx_flags |= TX_CMD_FLG_CTS_MSK;
+                }
+                break;
+        default:
+                break;
         }
 
-        tx_cmd->rts_retry_limit = rts_retry_limit;
-        tx_cmd->data_retry_limit = data_retry_limit;
+        /* Set up antennas */
+        priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant);
+        rate_flags |= iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
+
+        /* Set the rate in the TX cmd */
         tx_cmd->rate_n_flags = iwl_hw_set_rate_n_flags(rate_plcp, rate_flags);
 }
 
@@ -701,12 +720,6 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                 goto drop_unlock;
         }
 
-        if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xFF) ==
-             IWL_INVALID_RATE) {
-                IWL_ERR(priv, "ERROR: No TX rate available.\n");
-                goto drop_unlock;
-        }
-
         fc = hdr->frame_control;
 
 #ifdef CONFIG_IWLWIFI_DEBUG
@@ -807,7 +820,7 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
         iwl_dbg_log_tx_data_frame(priv, len, hdr);
 
         /* set is_hcca to 0; it probably will never be implemented */
-        iwl_tx_cmd_build_rate(priv, tx_cmd, info, fc, sta_id, 0);
+        iwl_tx_cmd_build_rate(priv, tx_cmd, info, fc, 0);
 
         iwl_update_stats(priv, true, fc, len);
         /*
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index 34790413eadd..2238c9f2018c 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -1373,7 +1373,7 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
                 fill_rx = 1;
         /* Rx interrupt, but nothing sent from uCode */
         if (i == r)
-                IWL_DEBUG(priv, IWL_DL_RX | IWL_DL_ISR, "r = %d, i = %d\n", r, i);
+                IWL_DEBUG_RX(priv, "r = %d, i = %d\n", r, i);
 
         while (i != r) {
                 rxb = rxq->queue[i];
@@ -1404,15 +1404,13 @@ static void iwl3945_rx_handle(struct iwl_priv *priv)
                  *   handle those that need handling via function in
                  *   rx_handlers table.  See iwl3945_setup_rx_handlers() */
                 if (priv->rx_handlers[pkt->hdr.cmd]) {
-                        IWL_DEBUG(priv, IWL_DL_HCMD | IWL_DL_RX | IWL_DL_ISR,
-                                "r = %d, i = %d, %s, 0x%02x\n", r, i,
+                        IWL_DEBUG_RX(priv, "r = %d, i = %d, %s, 0x%02x\n", r, i,
                                 get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
                         priv->rx_handlers[pkt->hdr.cmd] (priv, rxb);
                         priv->isr_stats.rx_handlers[pkt->hdr.cmd]++;
                 } else {
                         /* No handling needed */
-                        IWL_DEBUG(priv, IWL_DL_HCMD | IWL_DL_RX | IWL_DL_ISR,
-                                "r %d i %d No handler needed for %s, 0x%02x\n",
+                        IWL_DEBUG_RX(priv, "r %d i %d No handler needed for %s, 0x%02x\n",
                                 r, i, get_cmd_string(pkt->hdr.cmd),
                                 pkt->hdr.cmd);
                 }
diff --git a/drivers/net/wireless/iwmc3200wifi/cfg80211.c b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
index a6e852f4f92c..a56a2b0ac99a 100644
--- a/drivers/net/wireless/iwmc3200wifi/cfg80211.c
+++ b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
@@ -238,8 +238,9 @@ static int iwm_cfg80211_set_default_key(struct wiphy *wiphy,
         return iwm_set_tx_key(iwm, key_index);
 }
 
-int iwm_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
-                             u8 *mac, struct station_info *sinfo)
+static int iwm_cfg80211_get_station(struct wiphy *wiphy,
+                                    struct net_device *ndev,
+                                    u8 *mac, struct station_info *sinfo)
 {
         struct iwm_priv *iwm = ndev_to_iwm(ndev);
 
@@ -326,11 +327,8 @@ static int iwm_cfg80211_change_iface(struct wiphy *wiphy,
 
         iwm->umac_profile->mode = cpu_to_le32(iwm->conf.mode);
 
-        if (iwm->umac_profile_active) {
-                int ret = iwm_invalidate_mlme_profile(iwm);
-                if (ret < 0)
-                        IWM_ERR(iwm, "Couldn't invalidate profile\n");
-        }
+        if (iwm->umac_profile_active)
+                iwm_invalidate_mlme_profile(iwm);
 
         return 0;
 }
@@ -565,6 +563,7 @@ static int iwm_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 {
         struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
         struct ieee80211_channel *chan = sme->channel;
+        struct key_params key_param;
         int ret;
 
         if (!test_bit(IWM_STATUS_READY, &iwm->status))
@@ -573,6 +572,14 @@ static int iwm_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
         if (!sme->ssid)
                 return -EINVAL;
 
+        if (iwm->umac_profile_active) {
+                ret = iwm_invalidate_mlme_profile(iwm);
+                if (ret) {
+                        IWM_ERR(iwm, "Couldn't invalidate profile\n");
+                        return ret;
+                }
+        }
+
         if (chan)
                 iwm->channel =
                         ieee80211_frequency_to_channel(chan->center_freq);
@@ -614,7 +621,48 @@ static int iwm_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
                         return ret;
         }
 
-        return iwm_send_mlme_profile(iwm);
+        /*
+         * We save the WEP key in case we want to do shared authentication.
+         * We have to do it so because UMAC will assert whenever it gets a
+         * key before a profile.
+         */
+        if (sme->key) {
+                key_param.key = kmemdup(sme->key, sme->key_len, GFP_KERNEL);
+                if (key_param.key == NULL)
+                        return -ENOMEM;
+                key_param.key_len = sme->key_len;
+                key_param.seq_len = 0;
+                key_param.cipher = sme->crypto.ciphers_pairwise[0];
+
+                ret = iwm_key_init(&iwm->keys[sme->key_idx], sme->key_idx,
+                                   NULL, &key_param);
+                kfree(key_param.key);
+                if (ret < 0) {
+                        IWM_ERR(iwm, "Invalid key_params\n");
+                        return ret;
+                }
+
+                iwm->default_key = sme->key_idx;
+        }
+
+        ret = iwm_send_mlme_profile(iwm);
+
+        if (iwm->umac_profile->sec.auth_type != UMAC_AUTH_TYPE_LEGACY_PSK ||
+            sme->key == NULL)
+                return ret;
+
+        /*
+         * We want to do shared auth.
+         * We need to actually set the key we previously cached,
+         * and then tell the UMAC it's the default one.
+         * That will trigger the auth+assoc UMAC machinery, and again,
+         * this must be done after setting the profile.
+         */
+        ret = iwm_set_key(iwm, 0, &iwm->keys[sme->key_idx]);
+        if (ret < 0)
+                return ret;
+
+        return iwm_set_tx_key(iwm, iwm->default_key);
 }
 
 static int iwm_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
@@ -625,7 +673,7 @@ static int iwm_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *dev,
         IWM_DBG_WEXT(iwm, DBG, "Active: %d\n", iwm->umac_profile_active);
 
         if (iwm->umac_profile_active)
-                return iwm_invalidate_mlme_profile(iwm);
+                iwm_invalidate_mlme_profile(iwm);
 
         return 0;
 }
diff --git a/drivers/net/wireless/iwmc3200wifi/commands.c b/drivers/net/wireless/iwmc3200wifi/commands.c
index a68a2aff3c1e..23b52fa2605f 100644
--- a/drivers/net/wireless/iwmc3200wifi/commands.c
+++ b/drivers/net/wireless/iwmc3200wifi/commands.c
@@ -756,6 +756,7 @@ int iwm_send_mlme_profile(struct iwm_priv *iwm)
                 return ret;
         }
 
+        set_bit(IWM_STATUS_SME_CONNECTING, &iwm->status);
         return 0;
 }
 
diff --git a/drivers/net/wireless/iwmc3200wifi/debug.h b/drivers/net/wireless/iwmc3200wifi/debug.h
index 8fbb42d9c21f..e35c9b693d1f 100644
--- a/drivers/net/wireless/iwmc3200wifi/debug.h
+++ b/drivers/net/wireless/iwmc3200wifi/debug.h
@@ -108,6 +108,8 @@ struct iwm_debugfs {
         struct dentry *txq_dentry;
         struct dentry *tx_credit_dentry;
         struct dentry *rx_ticket_dentry;
+
+        struct dentry *fw_err_dentry;
 };
 
 #ifdef CONFIG_IWM_DEBUG
diff --git a/drivers/net/wireless/iwmc3200wifi/debugfs.c b/drivers/net/wireless/iwmc3200wifi/debugfs.c
index 0fa7b9150d58..1465379f900a 100644
--- a/drivers/net/wireless/iwmc3200wifi/debugfs.c
+++ b/drivers/net/wireless/iwmc3200wifi/debugfs.c
@@ -98,7 +98,7 @@ DEFINE_SIMPLE_ATTRIBUTE(fops_iwm_dbg_modules,
                         iwm_debugfs_u32_read, iwm_debugfs_dbg_modules_write,
                         "%llu\n");
 
-static int iwm_txrx_open(struct inode *inode, struct file *filp)
+static int iwm_generic_open(struct inode *inode, struct file *filp)
 {
         filp->private_data = inode->i_private;
         return 0;
@@ -289,25 +289,111 @@ static ssize_t iwm_debugfs_rx_ticket_read(struct file *filp,
         return ret;
 }
 
+static ssize_t iwm_debugfs_fw_err_read(struct file *filp,
+                                       char __user *buffer,
+                                       size_t count, loff_t *ppos)
+{
+
+        struct iwm_priv *iwm = filp->private_data;
+        char buf[512];
+        int buf_len = 512;
+        size_t len = 0;
+
+        if (*ppos != 0)
+                return 0;
+        if (count < sizeof(buf))
+                return -ENOSPC;
+
+        if (!iwm->last_fw_err)
+                return -ENOMEM;
+
+        if (iwm->last_fw_err->line_num == 0)
+                goto out;
+
+        len += snprintf(buf + len, buf_len - len, "%cMAC FW ERROR:\n",
+             (le32_to_cpu(iwm->last_fw_err->category) == UMAC_SYS_ERR_CAT_LMAC)
+                        ? 'L' : 'U');
+        len += snprintf(buf + len, buf_len - len,
+                        "\tCategory:    %d\n",
+                        le32_to_cpu(iwm->last_fw_err->category));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tStatus:      0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->status));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tPC:          0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->pc));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tblink1:      %d\n",
+                        le32_to_cpu(iwm->last_fw_err->blink1));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tblink2:      %d\n",
+                        le32_to_cpu(iwm->last_fw_err->blink2));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tilink1:      %d\n",
+                        le32_to_cpu(iwm->last_fw_err->ilink1));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tilink2:      %d\n",
+                        le32_to_cpu(iwm->last_fw_err->ilink2));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tData1:       0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->data1));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tData2:       0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->data2));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tLine number: %d\n",
+                        le32_to_cpu(iwm->last_fw_err->line_num));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tUMAC status: 0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->umac_status));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tLMAC status: 0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->lmac_status));
+
+        len += snprintf(buf + len, buf_len - len,
+                        "\tSDIO status: 0x%x\n",
+                        le32_to_cpu(iwm->last_fw_err->sdio_status));
+
+out:
+
+        return simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
+}
 
 static const struct file_operations iwm_debugfs_txq_fops = {
         .owner =        THIS_MODULE,
-        .open =         iwm_txrx_open,
+        .open =         iwm_generic_open,
         .read =         iwm_debugfs_txq_read,
 };
 
 static const struct file_operations iwm_debugfs_tx_credit_fops = {
         .owner =        THIS_MODULE,
-        .open =         iwm_txrx_open,
+        .open =         iwm_generic_open,
         .read =         iwm_debugfs_tx_credit_read,
 };
 
 static const struct file_operations iwm_debugfs_rx_ticket_fops = {
         .owner =        THIS_MODULE,
-        .open =         iwm_txrx_open,
+        .open =         iwm_generic_open,
         .read =         iwm_debugfs_rx_ticket_read,
 };
 
+static const struct file_operations iwm_debugfs_fw_err_fops = {
+        .owner =        THIS_MODULE,
+        .open =         iwm_generic_open,
+        .read =         iwm_debugfs_fw_err_read,
+};
+
 int iwm_debugfs_init(struct iwm_priv *iwm)
 {
         int i, result;
@@ -423,6 +509,16 @@ int iwm_debugfs_init(struct iwm_priv *iwm)
                 goto error;
         }
 
+        iwm->dbg.fw_err_dentry = debugfs_create_file("last_fw_err", 0200,
+                                                     iwm->dbg.dbgdir, iwm,
+                                                     &iwm_debugfs_fw_err_fops);
+        result = PTR_ERR(iwm->dbg.fw_err_dentry);
+        if (IS_ERR(iwm->dbg.fw_err_dentry) && (result != -ENODEV)) {
+                IWM_ERR(iwm, "Couldn't create last FW err: %d\n", result);
+                goto error;
+        }
+
+
         return 0;
 
  error:
@@ -441,6 +537,7 @@ void iwm_debugfs_exit(struct iwm_priv *iwm)
         debugfs_remove(iwm->dbg.txq_dentry);
         debugfs_remove(iwm->dbg.tx_credit_dentry);
         debugfs_remove(iwm->dbg.rx_ticket_dentry);
+        debugfs_remove(iwm->dbg.fw_err_dentry);
         if (iwm->bus_ops->debugfs_exit)
                 iwm->bus_ops->debugfs_exit(iwm);
 
diff --git a/drivers/net/wireless/iwmc3200wifi/fw.c b/drivers/net/wireless/iwmc3200wifi/fw.c
index 0f32cab9ced4..6b0bcad758ca 100644
--- a/drivers/net/wireless/iwmc3200wifi/fw.c
+++ b/drivers/net/wireless/iwmc3200wifi/fw.c
@@ -261,6 +261,33 @@ static int iwm_load_lmac(struct iwm_priv *iwm, const char *img_name)
                         cpu_to_le32(UMAC_RST_CTRL_FLG_LARC_CLK_EN), 0);
 }
 
+static int iwm_init_calib(struct iwm_priv *iwm, unsigned long cfg_bitmap,
+                          unsigned long expected_bitmap, u8 rx_iq_cmd)
+{
+        /* Read RX IQ calibration result from EEPROM */
+        if (test_bit(rx_iq_cmd, &cfg_bitmap)) {
+                iwm_store_rxiq_calib_result(iwm);
+                set_bit(PHY_CALIBRATE_RX_IQ_CMD, &iwm->calib_done_map);
+        }
+
+        iwm_send_prio_table(iwm);
+        iwm_send_init_calib_cfg(iwm, cfg_bitmap);
+
+        while (iwm->calib_done_map != expected_bitmap) {
+                if (iwm_notif_handle(iwm, CALIBRATION_RES_NOTIFICATION,
+                                     IWM_SRC_LMAC, WAIT_NOTIF_TIMEOUT)) {
+                        IWM_DBG_FW(iwm, DBG, "Initial calibration timeout\n");
+                        return -ETIMEDOUT;
+                }
+
+                IWM_DBG_FW(iwm, DBG, "Got calibration result. calib_done_map: "
+                           "0x%lx, expected calibrations: 0x%lx\n",
+                           iwm->calib_done_map, expected_bitmap);
+        }
+
+        return 0;
+}
+
 /*
  * We currently have to load 3 FWs:
  * 1) The UMAC (Upper MAC).
@@ -276,6 +303,7 @@ static int iwm_load_lmac(struct iwm_priv *iwm, const char *img_name)
 int iwm_load_fw(struct iwm_priv *iwm)
 {
         unsigned long init_calib_map, periodic_calib_map;
+        unsigned long expected_calib_map;
         int ret;
 
         /* We first start downloading the UMAC */
@@ -317,27 +345,21 @@ int iwm_load_fw(struct iwm_priv *iwm)
         }
 
         init_calib_map = iwm->conf.calib_map & IWM_CALIB_MAP_INIT_MSK;
+        expected_calib_map = iwm->conf.expected_calib_map &
+                IWM_CALIB_MAP_INIT_MSK;
         periodic_calib_map = IWM_CALIB_MAP_PER_LMAC(iwm->conf.calib_map);
 
-        /* Read RX IQ calibration result from EEPROM */
-        if (test_bit(PHY_CALIBRATE_RX_IQ_CMD, &init_calib_map)) {
-                iwm_store_rxiq_calib_result(iwm);
-                set_bit(PHY_CALIBRATE_RX_IQ_CMD, &iwm->calib_done_map);
-        }
-
-        iwm_send_prio_table(iwm);
-        iwm_send_init_calib_cfg(iwm, init_calib_map);
-
-        while (iwm->calib_done_map != init_calib_map) {
-                ret = iwm_notif_handle(iwm, CALIBRATION_RES_NOTIFICATION,
-                                       IWM_SRC_LMAC, WAIT_NOTIF_TIMEOUT);
-                if (ret) {
-                        IWM_ERR(iwm, "Wait for calibration result timeout\n");
+        ret = iwm_init_calib(iwm, init_calib_map, expected_calib_map,
+                             CALIB_CFG_RX_IQ_IDX);
+        if (ret < 0) {
+                /* Let's try the old way */
+                ret = iwm_init_calib(iwm, expected_calib_map,
+                                     expected_calib_map,
+                                     PHY_CALIBRATE_RX_IQ_CMD);
+                if (ret < 0) {
+                        IWM_ERR(iwm, "Calibration result timeout\n");
                         goto out;
                 }
-                IWM_DBG_FW(iwm, DBG, "Got calibration result. calib_done_map: "
-                           "0x%lx, requested calibrations: 0x%lx\n",
-                           iwm->calib_done_map, init_calib_map);
         }
 
         /* Handle LMAC CALIBRATION_COMPLETE notification */
diff --git a/drivers/net/wireless/iwmc3200wifi/iwm.h b/drivers/net/wireless/iwmc3200wifi/iwm.h
index 7a51bc340fda..1b02a4e2a1ac 100644
--- a/drivers/net/wireless/iwmc3200wifi/iwm.h
+++ b/drivers/net/wireless/iwmc3200wifi/iwm.h
@@ -64,6 +64,7 @@
 struct iwm_conf {
         u32 sdio_ior_timeout;
         unsigned long calib_map;
+        unsigned long expected_calib_map;
         bool reset_on_fatal_err;
         bool auto_connect;
         bool wimax_not_present;
@@ -175,8 +176,9 @@ struct iwm_key {
 #define IWM_STATUS_READY                0
 #define IWM_STATUS_SCANNING             1
 #define IWM_STATUS_SCAN_ABORTING        2
-#define IWM_STATUS_ASSOCIATING          3
+#define IWM_STATUS_SME_CONNECTING       3
 #define IWM_STATUS_ASSOCIATED           4
+#define IWM_STATUS_RESETTING            5
 
 struct iwm_tx_queue {
         int id;
@@ -273,6 +275,7 @@ struct iwm_priv {
 
         struct iw_statistics wstats;
         struct delayed_work stats_request;
+        struct delayed_work disconnect;
 
         struct iwm_debugfs dbg;
 
@@ -286,6 +289,8 @@ struct iwm_priv {
         u8 *resp_ie;
         int resp_ie_len;
 
+        struct iwm_fw_error_hdr *last_fw_err;
+
         char private[0] __attribute__((__aligned__(NETDEV_ALIGN)));
 };
 
@@ -315,6 +320,7 @@ int iwm_mode_to_nl80211_iftype(int mode);
 int iwm_priv_init(struct iwm_priv *iwm);
 void iwm_priv_deinit(struct iwm_priv *iwm);
 void iwm_reset(struct iwm_priv *iwm);
+void iwm_resetting(struct iwm_priv *iwm);
 void iwm_tx_credit_init_pools(struct iwm_priv *iwm,
                               struct iwm_umac_notif_alive *alive);
 int iwm_tx_credit_alloc(struct iwm_priv *iwm, int id, int nb);
diff --git a/drivers/net/wireless/iwmc3200wifi/lmac.h b/drivers/net/wireless/iwmc3200wifi/lmac.h
index 19213e165f5f..6c1a14c4480f 100644
--- a/drivers/net/wireless/iwmc3200wifi/lmac.h
+++ b/drivers/net/wireless/iwmc3200wifi/lmac.h
@@ -396,9 +396,24 @@ enum {
         CALIBRATION_CMD_NUM,
 };
 
+enum {
+        CALIB_CFG_RX_BB_IDX       = 0,
+        CALIB_CFG_DC_IDX          = 1,
+        CALIB_CFG_LO_IDX          = 2,
+        CALIB_CFG_TX_IQ_IDX       = 3,
+        CALIB_CFG_RX_IQ_IDX       = 4,
+        CALIB_CFG_NOISE_IDX       = 5,
+        CALIB_CFG_CRYSTAL_IDX     = 6,
+        CALIB_CFG_TEMPERATURE_IDX = 7,
+        CALIB_CFG_PAPD_IDX        = 8,
+        CALIB_CFG_LAST_IDX        = CALIB_CFG_PAPD_IDX,
+        CALIB_CFG_MODULE_NUM,
+};
+
 #define IWM_CALIB_MAP_INIT_MSK          0xFFFF
 #define IWM_CALIB_MAP_PER_LMAC(m)       ((m & 0xFF0000) >> 16)
 #define IWM_CALIB_MAP_PER_UMAC(m)       ((m & 0xFF000000) >> 24)
+#define IWM_CALIB_OPCODE_TO_INDEX(op)   (op - PHY_CALIBRATE_OPCODES_NUM)
 
 struct iwm_lmac_calib_hdr {
         u8 opcode;
diff --git a/drivers/net/wireless/iwmc3200wifi/main.c b/drivers/net/wireless/iwmc3200wifi/main.c
index cf2574442b57..d668e4756324 100644
--- a/drivers/net/wireless/iwmc3200wifi/main.c
+++ b/drivers/net/wireless/iwmc3200wifi/main.c
@@ -53,7 +53,12 @@
 static struct iwm_conf def_iwm_conf = {
 
         .sdio_ior_timeout       = 5000,
-        .calib_map              = BIT(PHY_CALIBRATE_DC_CMD)     |
+        .calib_map              = BIT(CALIB_CFG_DC_IDX) |
+                                  BIT(CALIB_CFG_LO_IDX) |
+                                  BIT(CALIB_CFG_TX_IQ_IDX)      |
+                                  BIT(CALIB_CFG_RX_IQ_IDX)      |
+                                  BIT(SHILOH_PHY_CALIBRATE_BASE_BAND_CMD),
+        .expected_calib_map     = BIT(PHY_CALIBRATE_DC_CMD)     |
                                   BIT(PHY_CALIBRATE_LO_CMD)     |
                                   BIT(PHY_CALIBRATE_TX_IQ_CMD)  |
                                   BIT(PHY_CALIBRATE_RX_IQ_CMD)  |
@@ -108,8 +113,28 @@ static void iwm_statistics_request(struct work_struct *work)
         iwm_send_umac_stats_req(iwm, 0);
 }
 
-int __iwm_up(struct iwm_priv *iwm);
-int __iwm_down(struct iwm_priv *iwm);
+static void iwm_disconnect_work(struct work_struct *work)
+{
+        struct iwm_priv *iwm =
+                container_of(work, struct iwm_priv, disconnect.work);
+
+        if (iwm->umac_profile_active)
+                iwm_invalidate_mlme_profile(iwm);
+
+        clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
+        iwm->umac_profile_active = 0;
+        memset(iwm->bssid, 0, ETH_ALEN);
+        iwm->channel = 0;
+
+        iwm_link_off(iwm);
+
+        wake_up_interruptible(&iwm->mlme_queue);
+
+        cfg80211_disconnected(iwm_to_ndev(iwm), 0, NULL, 0, GFP_KERNEL);
+}
+
+static int __iwm_up(struct iwm_priv *iwm);
+static int __iwm_down(struct iwm_priv *iwm);
 
 static void iwm_reset_worker(struct work_struct *work)
 {
@@ -162,7 +187,8 @@ static void iwm_reset_worker(struct work_struct *work)
                 memcpy(iwm->umac_profile, profile, sizeof(*profile));
                 iwm_send_mlme_profile(iwm);
                 kfree(profile);
-        }
+        } else
+                clear_bit(IWM_STATUS_RESETTING, &iwm->status);
 
  out:
         mutex_unlock(&iwm->mutex);
@@ -175,7 +201,7 @@ static void iwm_watchdog(unsigned long data)
         IWM_WARN(iwm, "Watchdog expired: UMAC stalls!\n");
 
         if (modparam_reset)
-                schedule_work(&iwm->reset_worker);
+                iwm_resetting(iwm);
 }
 
 int iwm_priv_init(struct iwm_priv *iwm)
@@ -198,6 +224,7 @@ int iwm_priv_init(struct iwm_priv *iwm)
         spin_lock_init(&iwm->cmd_lock);
         iwm->scan_id = 1;
         INIT_DELAYED_WORK(&iwm->stats_request, iwm_statistics_request);
+        INIT_DELAYED_WORK(&iwm->disconnect, iwm_disconnect_work);
         INIT_WORK(&iwm->reset_worker, iwm_reset_worker);
         INIT_LIST_HEAD(&iwm->bss_list);
 
@@ -233,6 +260,11 @@ int iwm_priv_init(struct iwm_priv *iwm)
         iwm->watchdog.data = (unsigned long)iwm;
         mutex_init(&iwm->mutex);
 
+        iwm->last_fw_err = kzalloc(sizeof(struct iwm_fw_error_hdr),
+                                   GFP_KERNEL);
+        if (iwm->last_fw_err == NULL)
+                return -ENOMEM;
+
         return 0;
 }
 
@@ -244,6 +276,7 @@ void iwm_priv_deinit(struct iwm_priv *iwm)
                 destroy_workqueue(iwm->txq[i].wq);
 
         destroy_workqueue(iwm->rx_wq);
+        kfree(iwm->last_fw_err);
 }
 
 /*
@@ -258,7 +291,11 @@ void iwm_reset(struct iwm_priv *iwm)
         if (test_bit(IWM_STATUS_READY, &iwm->status))
                 iwm_target_reset(iwm);
 
-        iwm->status = 0;
+        if (test_bit(IWM_STATUS_RESETTING, &iwm->status)) {
+                iwm->status = 0;
+                set_bit(IWM_STATUS_RESETTING, &iwm->status);
+        } else
+                iwm->status = 0;
         iwm->scan_id = 1;
 
         list_for_each_entry_safe(notif, next, &iwm->pending_notif, pending) {
@@ -274,6 +311,13 @@ void iwm_reset(struct iwm_priv *iwm)
         iwm_link_off(iwm);
 }
 
+void iwm_resetting(struct iwm_priv *iwm)
+{
+        set_bit(IWM_STATUS_RESETTING, &iwm->status);
+
+        schedule_work(&iwm->reset_worker);
+}
+
 /*
  * Notification code:
  *
@@ -538,7 +582,7 @@ static int iwm_channels_init(struct iwm_priv *iwm)
         return 0;
 }
 
-int __iwm_up(struct iwm_priv *iwm)
+static int __iwm_up(struct iwm_priv *iwm)
 {
         int ret;
         struct iwm_notif *notif_reboot, *notif_ack = NULL;
@@ -672,7 +716,7 @@ int iwm_up(struct iwm_priv *iwm)
         return ret;
 }
 
-int __iwm_down(struct iwm_priv *iwm)
+static int __iwm_down(struct iwm_priv *iwm)
 {
         int ret;
 
diff --git a/drivers/net/wireless/iwmc3200wifi/rx.c b/drivers/net/wireless/iwmc3200wifi/rx.c
index 86079a187eef..40dbcbc16593 100644
--- a/drivers/net/wireless/iwmc3200wifi/rx.c
+++ b/drivers/net/wireless/iwmc3200wifi/rx.c
@@ -102,6 +102,8 @@ static int iwm_ntf_error(struct iwm_priv *iwm, u8 *buf,
         error = (struct iwm_umac_notif_error *)buf;
         fw_err = &error->err;
 
+        memcpy(iwm->last_fw_err, fw_err, sizeof(struct iwm_fw_error_hdr));
+
         IWM_ERR(iwm, "%cMAC FW ERROR:\n",
          (le32_to_cpu(fw_err->category) == UMAC_SYS_ERR_CAT_LMAC) ? 'L' : 'U');
         IWM_ERR(iwm, "\tCategory:    %d\n", le32_to_cpu(fw_err->category));
@@ -118,6 +120,8 @@ static int iwm_ntf_error(struct iwm_priv *iwm, u8 *buf,
         IWM_ERR(iwm, "\tLMAC status: 0x%x\n", le32_to_cpu(fw_err->lmac_status));
         IWM_ERR(iwm, "\tSDIO status: 0x%x\n", le32_to_cpu(fw_err->sdio_status));
 
+        iwm_resetting(iwm);
+
         return 0;
 }
 
@@ -487,8 +491,6 @@ static int iwm_mlme_assoc_start(struct iwm_priv *iwm, u8 *buf,
 
         start = (struct iwm_umac_notif_assoc_start *)buf;
 
-        set_bit(IWM_STATUS_ASSOCIATING, &iwm->status);
-
         IWM_DBG_MLME(iwm, INFO, "Association with %pM Started, reason: %d\n",
                      start->bssid, le32_to_cpu(start->roam_reason));
 
@@ -507,47 +509,80 @@ static int iwm_mlme_assoc_complete(struct iwm_priv *iwm, u8 *buf,
         IWM_DBG_MLME(iwm, INFO, "Association with %pM completed, status: %d\n",
                      complete->bssid, complete->status);
 
-        clear_bit(IWM_STATUS_ASSOCIATING, &iwm->status);
-
         switch (le32_to_cpu(complete->status)) {
         case UMAC_ASSOC_COMPLETE_SUCCESS:
                 set_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
                 memcpy(iwm->bssid, complete->bssid, ETH_ALEN);
                 iwm->channel = complete->channel;
 
+                /* Internal roaming state, avoid notifying SME. */
+                if (!test_and_clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status)
+                    && iwm->conf.mode == UMAC_MODE_BSS) {
+                        cancel_delayed_work(&iwm->disconnect);
+                        cfg80211_roamed(iwm_to_ndev(iwm),
+                                        complete->bssid,
+                                        iwm->req_ie, iwm->req_ie_len,
+                                        iwm->resp_ie, iwm->resp_ie_len,
+                                        GFP_KERNEL);
+                        break;
+                }
+
                 iwm_link_on(iwm);
 
                 if (iwm->conf.mode == UMAC_MODE_IBSS)
                         goto ibss;
 
-                cfg80211_connect_result(iwm_to_ndev(iwm),
+                if (!test_bit(IWM_STATUS_RESETTING, &iwm->status))
+                        cfg80211_connect_result(iwm_to_ndev(iwm),
+                                                complete->bssid,
+                                                iwm->req_ie, iwm->req_ie_len,
+                                                iwm->resp_ie, iwm->resp_ie_len,
+                                                WLAN_STATUS_SUCCESS,
+                                                GFP_KERNEL);
+                else
+                        cfg80211_roamed(iwm_to_ndev(iwm),
                                         complete->bssid,
                                         iwm->req_ie, iwm->req_ie_len,
                                         iwm->resp_ie, iwm->resp_ie_len,
-                                        WLAN_STATUS_SUCCESS, GFP_KERNEL);
+                                        GFP_KERNEL);
                 break;
         case UMAC_ASSOC_COMPLETE_FAILURE:
                 clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
                 memset(iwm->bssid, 0, ETH_ALEN);
                 iwm->channel = 0;
 
+                /* Internal roaming state, avoid notifying SME. */
+                if (!test_and_clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status)
+                    && iwm->conf.mode == UMAC_MODE_BSS) {
+                        cancel_delayed_work(&iwm->disconnect);
+                        break;
+                }
+
                 iwm_link_off(iwm);
 
                 if (iwm->conf.mode == UMAC_MODE_IBSS)
                         goto ibss;
 
-                cfg80211_connect_result(iwm_to_ndev(iwm), complete->bssid,
-                                        NULL, 0, NULL, 0,
-                                        WLAN_STATUS_UNSPECIFIED_FAILURE,
-                                        GFP_KERNEL);
+                if (!test_bit(IWM_STATUS_RESETTING, &iwm->status))
+                        cfg80211_connect_result(iwm_to_ndev(iwm),
+                                                complete->bssid,
+                                                NULL, 0, NULL, 0,
+                                                WLAN_STATUS_UNSPECIFIED_FAILURE,
+                                                GFP_KERNEL);
+                else
+                        cfg80211_disconnected(iwm_to_ndev(iwm), 0, NULL, 0,
+                                              GFP_KERNEL);
+                break;
         default:
                 break;
         }
 
+        clear_bit(IWM_STATUS_RESETTING, &iwm->status);
         return 0;
 
  ibss:
         cfg80211_ibss_joined(iwm_to_ndev(iwm), iwm->bssid, GFP_KERNEL);
+        clear_bit(IWM_STATUS_RESETTING, &iwm->status);
         return 0;
 }
 
@@ -556,13 +591,20 @@ static int iwm_mlme_profile_invalidate(struct iwm_priv *iwm, u8 *buf,
                                        struct iwm_wifi_cmd *cmd)
 {
         struct iwm_umac_notif_profile_invalidate *invalid;
+        u32 reason;
 
         invalid = (struct iwm_umac_notif_profile_invalidate *)buf;
+        reason = le32_to_cpu(invalid->reason);
 
-        IWM_DBG_MLME(iwm, INFO, "Profile Invalidated. Reason: %d\n",
-                     le32_to_cpu(invalid->reason));
+        IWM_DBG_MLME(iwm, INFO, "Profile Invalidated. Reason: %d\n", reason);
 
-        clear_bit(IWM_STATUS_ASSOCIATING, &iwm->status);
+        if (reason != UMAC_PROFILE_INVALID_REQUEST &&
+            test_bit(IWM_STATUS_SME_CONNECTING, &iwm->status))
+                cfg80211_connect_result(iwm_to_ndev(iwm), NULL, NULL, 0, NULL,
+                                        0, WLAN_STATUS_UNSPECIFIED_FAILURE,
+                                        GFP_KERNEL);
+
+        clear_bit(IWM_STATUS_SME_CONNECTING, &iwm->status);
         clear_bit(IWM_STATUS_ASSOCIATED, &iwm->status);
 
         iwm->umac_profile_active = 0;
@@ -576,6 +618,19 @@ static int iwm_mlme_profile_invalidate(struct iwm_priv *iwm, u8 *buf,
         return 0;
 }
 
+#define IWM_DISCONNECT_INTERVAL (5 * HZ)
+
+static int iwm_mlme_connection_terminated(struct iwm_priv *iwm, u8 *buf,
+                                          unsigned long buf_size,
+                                          struct iwm_wifi_cmd *cmd)
+{
+        IWM_DBG_MLME(iwm, DBG, "Connection terminated\n");
+
+        schedule_delayed_work(&iwm->disconnect, IWM_DISCONNECT_INTERVAL);
+
+        return 0;
+}
+
 static int iwm_mlme_scan_complete(struct iwm_priv *iwm, u8 *buf,
                                   unsigned long buf_size,
                                   struct iwm_wifi_cmd *cmd)
@@ -813,7 +868,8 @@ static int iwm_mlme_mgt_frame(struct iwm_priv *iwm, u8 *buf,
                 iwm->resp_ie = kmemdup(mgt->u.reassoc_resp.variable,
                                        iwm->resp_ie_len, GFP_KERNEL);
         } else {
-                IWM_ERR(iwm, "Unsupported management frame");
+                IWM_ERR(iwm, "Unsupported management frame: 0x%x",
+                        le16_to_cpu(mgt->frame_control));
                 return 0;
         }
 
@@ -834,8 +890,7 @@ static int iwm_ntf_mlme(struct iwm_priv *iwm, u8 *buf,
         case WIFI_IF_NTFY_PROFILE_INVALIDATE_COMPLETE:
                 return iwm_mlme_profile_invalidate(iwm, buf, buf_size, cmd);
         case WIFI_IF_NTFY_CONNECTION_TERMINATED:
-                IWM_DBG_MLME(iwm, DBG, "Connection terminated\n");
-                break;
+                return iwm_mlme_connection_terminated(iwm, buf, buf_size, cmd);
         case WIFI_IF_NTFY_SCAN_COMPLETE:
                 return iwm_mlme_scan_complete(iwm, buf, buf_size, cmd);
         case WIFI_IF_NTFY_STA_TABLE_CHANGE:
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 402d36757765..54175b6fa86c 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -253,8 +253,8 @@ struct ndis_80211_pmkid_cand_list {
 struct ndis_80211_status_indication {
         __le32 status_type;
         union {
-                enum ndis_80211_media_stream_mode       media_stream_mode;
-                enum ndis_80211_radio_status            radio_status;
+                __le32                                  media_stream_mode;
+                __le32                                  radio_status;
                 struct ndis_80211_auth_request          auth_request[0];
                 struct ndis_80211_pmkid_cand_list       cand_list;
         } u;
@@ -466,7 +466,7 @@ struct rndis_wlan_private {
         u32  param_workaround_interval;
 
         /* hardware state */
-        int radio_on;
+        bool radio_on;
         int infra_mode;
         bool connected;
         u8 bssid[ETH_ALEN];
@@ -560,7 +560,6 @@ static struct rndis_wlan_private *get_rndis_wlan_priv(struct usbnet *dev)
         return (struct rndis_wlan_private *)dev->driver_priv;
 }
 
-
 static u32 get_bcm4320_power_dbm(struct rndis_wlan_private *priv)
 {
         switch (priv->param_power_output) {
@@ -576,7 +575,6 @@ static u32 get_bcm4320_power_dbm(struct rndis_wlan_private *priv)
         }
 }
 
-
 static bool is_wpa_key(struct rndis_wlan_private *priv, int idx)
 {
         int cipher = priv->encr_keys[idx].cipher;
@@ -585,7 +583,6 @@ static bool is_wpa_key(struct rndis_wlan_private *priv, int idx)
                 cipher == WLAN_CIPHER_SUITE_TKIP);
 }
 
-
 static int rndis_cipher_to_alg(u32 cipher)
 {
         switch (cipher) {
@@ -613,7 +610,6 @@ static int rndis_akm_suite_to_key_mgmt(u32 akm_suite)
         }
 }
 
-
 #ifdef DEBUG
 static const char *oid_to_string(__le32 oid)
 {
@@ -675,7 +671,6 @@ static const char *oid_to_string(__le32 oid)
 }
 #endif
 
-
 /* translate error code */
 static int rndis_error_status(__le32 rndis_status)
 {
@@ -699,7 +694,6 @@ static int rndis_error_status(__le32 rndis_status)
         return ret;
 }
 
-
 static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(dev);
@@ -758,7 +752,6 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
         return ret;
 }
 
-
 static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(dev);
@@ -817,7 +810,6 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
         return ret;
 }
 
-
 static int rndis_reset(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -840,7 +832,6 @@ static int rndis_reset(struct usbnet *usbdev)
         return 0;
 }
 
-
 /*
  * Specs say that we can only set config parameters only soon after device
  * initialization.
@@ -927,16 +918,9 @@ static int rndis_set_config_parameter(struct usbnet *dev, char *param,
 static int rndis_set_config_parameter_str(struct usbnet *dev,
                                                 char *param, char *value)
 {
-        return(rndis_set_config_parameter(dev, param, 2, value));
+        return rndis_set_config_parameter(dev, param, 2, value);
 }
 
-/*static int rndis_set_config_parameter_u32(struct usbnet *dev,
-                                                char *param, u32 value)
-{
-        return(rndis_set_config_parameter(dev, param, 0, &value));
-}*/
-
-
 /*
  * data conversion functions
  */
@@ -946,7 +930,6 @@ static int level_to_qual(int level)
         return qual >= 0 ? (qual <= 100 ? qual : 100) : 0;
 }
 
-
 /*
  * common functions
  */
@@ -966,8 +949,8 @@ static int set_essid(struct usbnet *usbdev, struct ndis_80211_ssid *ssid)
         }
         if (ret == 0) {
                 memcpy(&priv->essid, ssid, sizeof(priv->essid));
-                priv->radio_on = 1;
-                devdbg(usbdev, "set_essid: radio_on = 1");
+                priv->radio_on = true;
+                devdbg(usbdev, "set_essid: radio_on = true");
         }
 
         return ret;
@@ -1027,8 +1010,7 @@ static bool is_associated(struct usbnet *usbdev)
         return (ret == 0 && !is_zero_ether_addr(bssid));
 }
 
-
-static int disassociate(struct usbnet *usbdev, int reset_ssid)
+static int disassociate(struct usbnet *usbdev, bool reset_ssid)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
         struct ndis_80211_ssid ssid;
@@ -1037,8 +1019,8 @@ static int disassociate(struct usbnet *usbdev, int reset_ssid)
         if (priv->radio_on) {
                 ret = rndis_set_oid(usbdev, OID_802_11_DISASSOCIATE, NULL, 0);
                 if (ret == 0) {
-                        priv->radio_on = 0;
-                        devdbg(usbdev, "disassociate: radio_on = 0");
+                        priv->radio_on = false;
+                        devdbg(usbdev, "disassociate: radio_on = false");
 
                         if (reset_ssid)
                                 msleep(100);
@@ -1064,7 +1046,6 @@ static int disassociate(struct usbnet *usbdev, int reset_ssid)
         return ret;
 }
 
-
 static int set_auth_mode(struct usbnet *usbdev, u32 wpa_version,
                                 enum nl80211_auth_type auth_type, int keymgmt)
 {
@@ -1109,7 +1090,6 @@ static int set_auth_mode(struct usbnet *usbdev, u32 wpa_version,
         return 0;
 }
 
-
 static int set_priv_filter(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -1127,7 +1107,6 @@ static int set_priv_filter(struct usbnet *usbdev)
                                                                 sizeof(tmp));
 }
 
-
 static int set_encr_mode(struct usbnet *usbdev, int pairwise, int groupwise)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -1163,7 +1142,6 @@ static int set_encr_mode(struct usbnet *usbdev, int pairwise, int groupwise)
         return 0;
 }
 
-
 static int set_infra_mode(struct usbnet *usbdev, int mode)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -1189,7 +1167,6 @@ static int set_infra_mode(struct usbnet *usbdev, int mode)
         return 0;
 }
 
-
 static int set_rts_threshold(struct usbnet *usbdev, u32 rts_threshold)
 {
         __le32 tmp;
@@ -1204,7 +1181,6 @@ static int set_rts_threshold(struct usbnet *usbdev, u32 rts_threshold)
                                                                 sizeof(tmp));
 }
 
-
 static int set_frag_threshold(struct usbnet *usbdev, u32 frag_threshold)
 {
         __le32 tmp;
@@ -1219,7 +1195,6 @@ static int set_frag_threshold(struct usbnet *usbdev, u32 frag_threshold)
                                                                 sizeof(tmp));
 }
 
-
 static void set_default_iw_params(struct usbnet *usbdev)
 {
         set_infra_mode(usbdev, NDIS_80211_INFRA_INFRA);
@@ -1229,17 +1204,15 @@ static void set_default_iw_params(struct usbnet *usbdev)
         set_encr_mode(usbdev, RNDIS_WLAN_ALG_NONE, RNDIS_WLAN_ALG_NONE);
 }
 
-
 static int deauthenticate(struct usbnet *usbdev)
 {
         int ret;
 
-        ret = disassociate(usbdev, 1);
+        ret = disassociate(usbdev, true);
         set_default_iw_params(usbdev);
         return ret;
 }
 
-
 static int set_channel(struct usbnet *usbdev, int channel)
 {
         struct ndis_80211_conf config;
@@ -1270,7 +1243,6 @@ static int set_channel(struct usbnet *usbdev, int channel)
         return ret;
 }
 
-
 /* index must be 0 - N, as per NDIS  */
 static int add_wep_key(struct usbnet *usbdev, const u8 *key, int key_len,
                                                                 int index)
@@ -1322,10 +1294,9 @@ static int add_wep_key(struct usbnet *usbdev, const u8 *key, int key_len,
         return 0;
 }
 
-
 static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
                         int index, const u8 *addr, const u8 *rx_seq,
-                        int seq_len, u32 cipher, int flags)
+                        int seq_len, u32 cipher, __le32 flags)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
         struct ndis_80211_key ndis_key;
@@ -1417,7 +1388,6 @@ static int add_wpa_key(struct usbnet *usbdev, const u8 *key, int key_len,
         return 0;
 }
 
-
 static int restore_key(struct usbnet *usbdev, int key_idx)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -1436,7 +1406,6 @@ static int restore_key(struct usbnet *usbdev, int key_idx)
         return add_wep_key(usbdev, key.material, key.len, key_idx);
 }
 
-
 static void restore_keys(struct usbnet *usbdev)
 {
         int i;
@@ -1445,13 +1414,11 @@ static void restore_keys(struct usbnet *usbdev)
                 restore_key(usbdev, i);
 }
 
-
 static void clear_key(struct rndis_wlan_private *priv, int idx)
 {
         memset(&priv->encr_keys[idx], 0, sizeof(priv->encr_keys[idx]));
 }
 
-
 /* remove_key is for both wep and wpa */
 static int remove_key(struct usbnet *usbdev, int index, const u8 *bssid)
 {
@@ -1508,7 +1475,6 @@ static int remove_key(struct usbnet *usbdev, int index, const u8 *bssid)
         return 0;
 }
 
-
 static void set_multicast_list(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -1568,7 +1534,6 @@ static void set_multicast_list(struct usbnet *usbdev)
                                                 le32_to_cpu(filter), ret);
 }
 
-
 /*
  * cfg80211 ops
  */
@@ -1597,7 +1562,6 @@ static int rndis_change_virtual_intf(struct wiphy *wiphy,
         return set_infra_mode(usbdev, mode);
 }
 
-
 static int rndis_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
         struct rndis_wlan_private *priv = wiphy_priv(wiphy);
@@ -1619,7 +1583,6 @@ static int rndis_set_wiphy_params(struct wiphy *wiphy, u32 changed)
         return 0;
 }
 
-
 static int rndis_set_tx_power(struct wiphy *wiphy, enum tx_power_setting type,
                                 int dbm)
 {
@@ -1634,7 +1597,7 @@ static int rndis_set_tx_power(struct wiphy *wiphy, enum tx_power_setting type,
          */
         if (type == TX_POWER_AUTOMATIC || dbm == get_bcm4320_power_dbm(priv)) {
                 if (!priv->radio_on)
-                        disassociate(usbdev, 1); /* turn on radio */
+                        disassociate(usbdev, true); /* turn on radio */
 
                 return 0;
         }
@@ -1642,7 +1605,6 @@ static int rndis_set_tx_power(struct wiphy *wiphy, enum tx_power_setting type,
         return -ENOTSUPP;
 }
 
-
 static int rndis_get_tx_power(struct wiphy *wiphy, int *dbm)
 {
         struct rndis_wlan_private *priv = wiphy_priv(wiphy);
@@ -1655,7 +1617,6 @@ static int rndis_get_tx_power(struct wiphy *wiphy, int *dbm)
         return 0;
 }
 
-
 #define SCAN_DELAY_JIFFIES (6 * HZ)
 static int rndis_scan(struct wiphy *wiphy, struct net_device *dev,
                         struct cfg80211_scan_request *request)
@@ -1692,7 +1653,6 @@ static int rndis_scan(struct wiphy *wiphy, struct net_device *dev,
         return ret;
 }
 
-
 static struct cfg80211_bss *rndis_bss_info_update(struct usbnet *usbdev,
                                         struct ndis_80211_bssid_ex *bssid)
 {
@@ -1741,7 +1701,6 @@ static struct cfg80211_bss *rndis_bss_info_update(struct usbnet *usbdev,
                 GFP_KERNEL);
 }
 
-
 static int rndis_check_bssid_list(struct usbnet *usbdev)
 {
         void *buf = NULL;
@@ -1790,7 +1749,6 @@ out:
         return ret;
 }
 
-
 static void rndis_get_scan_results(struct work_struct *work)
 {
         struct rndis_wlan_private *priv =
@@ -1923,7 +1881,7 @@ static int rndis_connect(struct wiphy *wiphy, struct net_device *dev,
         return ret;
 
 err_turn_radio_on:
-        disassociate(usbdev, 1);
+        disassociate(usbdev, true);
 
         return ret;
 }
@@ -2031,7 +1989,7 @@ static int rndis_join_ibss(struct wiphy *wiphy, struct net_device *dev,
         return ret;
 
 err_turn_radio_on:
-        disassociate(usbdev, 1);
+        disassociate(usbdev, true);
 
         return ret;
 }
@@ -2065,7 +2023,7 @@ static int rndis_add_key(struct wiphy *wiphy, struct net_device *netdev,
 {
         struct rndis_wlan_private *priv = wiphy_priv(wiphy);
         struct usbnet *usbdev = priv->usbdev;
-        int flags;
+        __le32 flags;
 
         devdbg(usbdev, "rndis_add_key(%i, %pM, %08x)", key_index, mac_addr,
                                                         params->cipher);
@@ -2175,7 +2133,9 @@ static int rndis_dump_station(struct wiphy *wiphy, struct net_device *dev,
         return 0;
 }
 
-
+/*
+ * workers, indication handlers, device poller
+ */
 static void rndis_wlan_do_link_up_work(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -2293,7 +2253,6 @@ static void rndis_wlan_set_multicast_list(struct net_device *dev)
         queue_work(priv->workqueue, &priv->work);
 }
 
-
 static void rndis_wlan_auth_indication(struct usbnet *usbdev,
                                 struct ndis_80211_status_indication *indication,
                                 int len)
@@ -2476,7 +2435,6 @@ static void rndis_wlan_media_specific_indication(struct usbnet *usbdev,
         }
 }
 
-
 static void rndis_wlan_indication(struct usbnet *usbdev, void *ind, int buflen)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -2523,7 +2481,6 @@ static void rndis_wlan_indication(struct usbnet *usbdev, void *ind, int buflen)
         }
 }
 
-
 static int rndis_wlan_get_caps(struct usbnet *usbdev)
 {
         struct {
@@ -2560,7 +2517,6 @@ static int rndis_wlan_get_caps(struct usbnet *usbdev)
         return retval;
 }
 
-
 #define DEVICE_POLLER_JIFFIES (HZ)
 static void rndis_device_poller(struct work_struct *work)
 {
@@ -2632,7 +2588,9 @@ end:
                                                                 update_jiffies);
 }
 
-
+/*
+ * driver/device initialization
+ */
 static int bcm4320a_early_init(struct usbnet *usbdev)
 {
         /* bcm4320a doesn't handle configuration parameters well. Try
@@ -2642,7 +2600,6 @@ static int bcm4320a_early_init(struct usbnet *usbdev)
         return 0;
 }
 
-
 static int bcm4320b_early_init(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -2721,7 +2678,6 @@ static const struct net_device_ops rndis_wlan_netdev_ops = {
         .ndo_set_multicast_list = rndis_wlan_set_multicast_list,
 };
 
-
 static int rndis_wlan_bind(struct usbnet *usbdev, struct usb_interface *intf)
 {
         struct wiphy *wiphy;
@@ -2823,8 +2779,8 @@ static int rndis_wlan_bind(struct usbnet *usbdev, struct usb_interface *intf)
                         WIPHY_PARAM_FRAG_THRESHOLD | WIPHY_PARAM_RTS_THRESHOLD);
 
         /* turn radio on */
-        priv->radio_on = 1;
-        disassociate(usbdev, 1);
+        priv->radio_on = true;
+        disassociate(usbdev, true);
         netif_carrier_off(usbdev->net);
 
         return 0;
@@ -2840,13 +2796,12 @@ fail:
         return retval;
 }
 
-
 static void rndis_wlan_unbind(struct usbnet *usbdev, struct usb_interface *intf)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
 
         /* turn radio off */
-        disassociate(usbdev, 0);
+        disassociate(usbdev, false);
 
         cancel_delayed_work_sync(&priv->dev_poller_work);
         cancel_delayed_work_sync(&priv->scan_work);
@@ -2863,7 +2818,6 @@ static void rndis_wlan_unbind(struct usbnet *usbdev, struct usb_interface *intf)
         wiphy_free(priv->wdev.wiphy);
 }
 
-
 static int rndis_wlan_reset(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -2885,7 +2839,6 @@ static int rndis_wlan_reset(struct usbnet *usbdev)
         return deauthenticate(usbdev);
 }
 
-
 static int rndis_wlan_stop(struct usbnet *usbdev)
 {
         struct rndis_wlan_private *priv = get_rndis_wlan_priv(usbdev);
@@ -2894,7 +2847,7 @@ static int rndis_wlan_stop(struct usbnet *usbdev)
 
         devdbg(usbdev, "rndis_wlan_stop");
 
-        retval = disassociate(usbdev, 0);
+        retval = disassociate(usbdev, false);
 
         priv->work_pending = 0;
         cancel_delayed_work_sync(&priv->dev_poller_work);
@@ -2916,7 +2869,6 @@ static int rndis_wlan_stop(struct usbnet *usbdev)
         return retval;
 }
 
-
 static const struct driver_info bcm4320b_info = {
         .description =  "Wireless RNDIS device, BCM4320b based",
         .flags =        FLAG_WLAN | FLAG_FRAMING_RN | FLAG_NO_SETINT |
diff --git a/drivers/net/wireless/rt2x00/rt2x00crypto.c b/drivers/net/wireless/rt2x00/rt2x00crypto.c
index 30fbd3bbe08b..de36837dcf86 100644
--- a/drivers/net/wireless/rt2x00/rt2x00crypto.c
+++ b/drivers/net/wireless/rt2x00/rt2x00crypto.c
@@ -154,7 +154,7 @@ void rt2x00crypto_tx_insert_iv(struct sk_buff *skb, unsigned int header_length)
         skbdesc->flags &= ~SKBDESC_IV_STRIPPED;
 }
 
-void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, bool l2pad,
+void rt2x00crypto_rx_insert_iv(struct sk_buff *skb,
                                unsigned int header_length,
                                struct rxdone_entry_desc *rxdesc)
 {
@@ -199,7 +199,7 @@ void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, bool l2pad,
          * move the header more then iv_len since we must
          * make room for the payload move as well.
          */
-        if (l2pad) {
+        if (rxdesc->dev_flags & RXDONE_L2PAD) {
                 skb_push(skb, iv_len - align);
                 skb_put(skb, icv_len);
 
@@ -230,7 +230,7 @@ void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, bool l2pad,
          * Move payload for alignment purposes. Note that
          * this is only needed when no l2 padding is present.
          */
-        if (!l2pad) {
+        if (!(rxdesc->dev_flags & RXDONE_L2PAD)) {
                 memmove(skb->data + transfer,
                         skb->data + transfer + align,
                         payload_len);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 3f8c70ebe9ad..71761b343839 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -206,6 +206,7 @@ void rt2x00lib_txdone(struct queue_entry *entry,
         unsigned int header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
         u8 rate_idx, rate_flags, retry_rates;
         unsigned int i;
+        bool success;
 
         /*
          * Unmap the skb.
@@ -216,7 +217,7 @@ void rt2x00lib_txdone(struct queue_entry *entry,
          * Remove L2 padding which was added during
          */
         if (test_bit(DRIVER_REQUIRE_L2PAD, &rt2x00dev->flags))
-                rt2x00queue_payload_align(entry->skb, true, header_length);
+                rt2x00queue_remove_l2pad(entry->skb, header_length);
 
         /*
          * If the IV/EIV data was stripped from the frame before it was
@@ -234,13 +235,18 @@ void rt2x00lib_txdone(struct queue_entry *entry,
         rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TXDONE, entry->skb);
 
         /*
-         * Update TX statistics.
+         * Determine if the frame has been successfully transmitted.
          */
-        rt2x00dev->link.qual.tx_success +=
+        success =
             test_bit(TXDONE_SUCCESS, &txdesc->flags) ||
-            test_bit(TXDONE_UNKNOWN, &txdesc->flags);
-        rt2x00dev->link.qual.tx_failed +=
-            test_bit(TXDONE_FAILURE, &txdesc->flags);
+            test_bit(TXDONE_UNKNOWN, &txdesc->flags) ||
+            test_bit(TXDONE_FALLBACK, &txdesc->flags);
+
+        /*
+         * Update TX statistics.
+         */
+        rt2x00dev->link.qual.tx_success += success;
+        rt2x00dev->link.qual.tx_failed += !success;
 
         rate_idx = skbdesc->tx_rate_idx;
         rate_flags = skbdesc->tx_rate_flags;
@@ -263,22 +269,20 @@ void rt2x00lib_txdone(struct queue_entry *entry,
                 tx_info->status.rates[i].flags = rate_flags;
                 tx_info->status.rates[i].count = 1;
         }
-        if (i < (IEEE80211_TX_MAX_RATES -1))
+        if (i < (IEEE80211_TX_MAX_RATES - 1))
                 tx_info->status.rates[i].idx = -1; /* terminate */
 
         if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK)) {
-                if (test_bit(TXDONE_SUCCESS, &txdesc->flags) ||
-                                test_bit(TXDONE_UNKNOWN, &txdesc->flags))
+                if (success)
                         tx_info->flags |= IEEE80211_TX_STAT_ACK;
-                else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
+                else
                         rt2x00dev->low_level_stats.dot11ACKFailureCount++;
         }
 
         if (rate_flags & IEEE80211_TX_RC_USE_RTS_CTS) {
-                if (test_bit(TXDONE_SUCCESS, &txdesc->flags) ||
-                                test_bit(TXDONE_UNKNOWN, &txdesc->flags))
+                if (success)
                         rt2x00dev->low_level_stats.dot11RTSSuccessCount++;
-                else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
+                else
                         rt2x00dev->low_level_stats.dot11RTSFailureCount++;
         }
 
@@ -360,7 +364,6 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
         struct sk_buff *skb;
         struct ieee80211_rx_status *rx_status = &rt2x00dev->rx_status;
         unsigned int header_length;
-        bool l2pad;
         int rate_idx;
         /*
          * Allocate a new sk_buffer. If no new buffer available, drop the
@@ -389,7 +392,6 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
          * aligned on a 4 byte boundary.
          */
         header_length = ieee80211_get_hdrlen_from_skb(entry->skb);
-        l2pad = !!(rxdesc.dev_flags & RXDONE_L2PAD);
 
         /*
          * Hardware might have stripped the IV/EIV/ICV data,
@@ -399,10 +401,12 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
          */
         if ((rxdesc.dev_flags & RXDONE_CRYPTO_IV) &&
             (rxdesc.flags & RX_FLAG_IV_STRIPPED))
-                rt2x00crypto_rx_insert_iv(entry->skb, l2pad, header_length,
+                rt2x00crypto_rx_insert_iv(entry->skb, header_length,
                                           &rxdesc);
+        else if (rxdesc.dev_flags & RXDONE_L2PAD)
+                rt2x00queue_remove_l2pad(entry->skb, header_length);
         else
-                rt2x00queue_payload_align(entry->skb, l2pad, header_length);
+                rt2x00queue_align_payload(entry->skb, header_length);
 
         /*
          * Check if the frame was received using HT. In that case,
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index eeb2881e818e..5462cb5ad994 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -120,21 +120,42 @@ void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
 void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
 
 /**
- * rt2x00queue_payload_align - Align 802.11 payload to 4-byte boundary
+ * rt2x00queue_align_frame - Align 802.11 frame to 4-byte boundary
+ * @skb: The skb to align
+ *
+ * Align the start of the 802.11 frame to a 4-byte boundary, this could
+ * mean the payload is not aligned properly though.
+ */
+void rt2x00queue_align_frame(struct sk_buff *skb);
+
+/**
+ * rt2x00queue_align_payload - Align 802.11 payload to 4-byte boundary
+ * @skb: The skb to align
+ * @header_length: Length of 802.11 header
+ *
+ * Align the 802.11 payload to a 4-byte boundary, this could
+ * mean the header is not aligned properly though.
+ */
+void rt2x00queue_align_payload(struct sk_buff *skb, unsigned int header_length);
+
+/**
+ * rt2x00queue_insert_l2pad - Align 802.11 header & payload to 4-byte boundary
+ * @skb: The skb to align
+ * @header_length: Length of 802.11 header
+ *
+ * Apply L2 padding to align both header and payload to 4-byte boundary
+ */
+void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length);
+
+/**
+ * rt2x00queue_insert_l2pad - Remove L2 padding from 802.11 frame
  * @skb: The skb to align
- * @l2pad: Should L2 padding be used
  * @header_length: Length of 802.11 header
  *
- * This function prepares the @skb to be send to the device or mac80211.
- * If @l2pad is set to true padding will occur between the 802.11 header
- * and payload. Otherwise the padding will be done in front of the 802.11
- * header.
- * When @l2pad is set the function will check for the &SKBDESC_L2_PADDED
- * flag in &skb_frame_desc. If that flag is set, the padding is removed
- * and the flag cleared. Otherwise the padding is added and the flag is set.
+ * Remove L2 padding used to align both header and payload to 4-byte boundary,
+ * by removing the L2 padding the header will no longer be 4-byte aligned.
  */
-void rt2x00queue_payload_align(struct sk_buff *skb,
-                               bool l2pad, unsigned int header_length);
+void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length);
 
 /**
  * rt2x00queue_write_tx_frame - Write TX frame to hardware
@@ -324,7 +345,7 @@ void rt2x00crypto_tx_copy_iv(struct sk_buff *skb,
 void rt2x00crypto_tx_remove_iv(struct sk_buff *skb,
                                struct txentry_desc *txdesc);
 void rt2x00crypto_tx_insert_iv(struct sk_buff *skb, unsigned int header_length);
-void rt2x00crypto_rx_insert_iv(struct sk_buff *skb, bool l2pad,
+void rt2x00crypto_rx_insert_iv(struct sk_buff *skb,
                                unsigned int header_length,
                                struct rxdone_entry_desc *rxdesc);
 #else
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 06af823efd83..577029efe320 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -148,35 +148,89 @@ void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
         dev_kfree_skb_any(skb);
 }
 
-void rt2x00queue_payload_align(struct sk_buff *skb,
-                               bool l2pad, unsigned int header_length)
+void rt2x00queue_align_frame(struct sk_buff *skb)
 {
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
         unsigned int frame_length = skb->len;
-        unsigned int align = ALIGN_SIZE(skb, header_length);
+        unsigned int align = ALIGN_SIZE(skb, 0);
 
         if (!align)
                 return;
 
-        if (l2pad) {
-                if (skbdesc->flags & SKBDESC_L2_PADDED) {
-                        /* Remove L2 padding */
-                        memmove(skb->data + align, skb->data, header_length);
-                        skb_pull(skb, align);
-                        skbdesc->flags &= ~SKBDESC_L2_PADDED;
-                } else {
-                        /* Add L2 padding */
-                        skb_push(skb, align);
-                        memmove(skb->data, skb->data + align, header_length);
-                        skbdesc->flags |= SKBDESC_L2_PADDED;
-                }
+        skb_push(skb, align);
+        memmove(skb->data, skb->data + align, frame_length);
+        skb_trim(skb, frame_length);
+}
+
+void rt2x00queue_align_payload(struct sk_buff *skb, unsigned int header_lengt)
+{
+        unsigned int frame_length = skb->len;
+        unsigned int align = ALIGN_SIZE(skb, header_lengt);
+
+        if (!align)
+                return;
+
+        skb_push(skb, align);
+        memmove(skb->data, skb->data + align, frame_length);
+        skb_trim(skb, frame_length);
+}
+
+void rt2x00queue_insert_l2pad(struct sk_buff *skb, unsigned int header_length)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        unsigned int frame_length = skb->len;
+        unsigned int header_align = ALIGN_SIZE(skb, 0);
+        unsigned int payload_align = ALIGN_SIZE(skb, header_length);
+        unsigned int l2pad = 4 - (payload_align - header_align);
+
+        if (header_align == payload_align) {
+                /*
+                 * Both header and payload must be moved the same
+                 * amount of bytes to align them properly. This means
+                 * we don't use the L2 padding but just move the entire
+                 * frame.
+                 */
+                rt2x00queue_align_frame(skb);
+        } else if (!payload_align) {
+                /*
+                 * Simple L2 padding, only the header needs to be moved,
+                 * the payload is already properly aligned.
+                 */
+                skb_push(skb, header_align);
+                memmove(skb->data, skb->data + header_align, frame_length);
+                skbdesc->flags |= SKBDESC_L2_PADDED;
         } else {
-                /* Generic payload alignment to 4-byte boundary */
-                skb_push(skb, align);
-                memmove(skb->data, skb->data + align, frame_length);
+                /*
+                 *
+                 * Complicated L2 padding, both header and payload need
+                 * to be moved. By default we only move to the start
+                 * of the buffer, so our header alignment needs to be
+                 * increased if there is not enough room for the header
+                 * to be moved.
+                 */
+                if (payload_align > header_align)
+                        header_align += 4;
+
+                skb_push(skb, header_align);
+                memmove(skb->data, skb->data + header_align, header_length);
+                memmove(skb->data + header_length + l2pad,
+                        skb->data + header_length + l2pad + header_align,
+                        frame_length - header_length);
+                skbdesc->flags |= SKBDESC_L2_PADDED;
         }
 }
 
+void rt2x00queue_remove_l2pad(struct sk_buff *skb, unsigned int header_length)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        unsigned int l2pad = 4 - (header_length & 3);
+
+        if (!l2pad || (skbdesc->flags & SKBDESC_L2_PADDED))
+                return;
+
+        memmove(skb->data + l2pad, skb->data, header_length);
+        skb_pull(skb, l2pad);
+}
+
 static void rt2x00queue_create_tx_descriptor_seq(struct queue_entry *entry,
                                                  struct txentry_desc *txdesc)
 {
@@ -456,18 +510,15 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         /*
          * When DMA allocation is required we should guarentee to the
          * driver that the DMA is aligned to a 4-byte boundary.
-         * Aligning the header to this boundary can be done by calling
-         * rt2x00queue_payload_align with the header length of 0.
          * However some drivers require L2 padding to pad the payload
          * rather then the header. This could be a requirement for
          * PCI and USB devices, while header alignment only is valid
          * for PCI devices.
          */
         if (test_bit(DRIVER_REQUIRE_L2PAD, &queue->rt2x00dev->flags))
-                rt2x00queue_payload_align(entry->skb, true,
-                                          txdesc.header_length);
+                rt2x00queue_insert_l2pad(entry->skb, txdesc.header_length);
         else if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
-                rt2x00queue_payload_align(entry->skb, false, 0);
+                rt2x00queue_align_frame(entry->skb);
 
         /*
          * It could be possible that the queue was corrupted and this