Merge branch 'for-davem' of git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless-next

John Linville says:

====================
This is a sizeable batch of updates intended for 3.6...

The bulk of the changes here are Bluetooth.  Gustavo says:

	Here goes the first Bluetooth pull request for 3.6, we have
	queued quite a lot of work. Andrei Emeltchenko added the AMP
	Manager code, a lot of work is needed, but the first bit are
	already there. This code is disabled by default.  Mat Martineau
	changed the whole L2CAP ERTM state machine code, replacing
	the old one with a new implementation. Besides that we had
	lot of coding style fixes (to follow net rules), more l2cap
	core separation from socket and many clean ups and fixed all
	over the tree.

Along with the above, there is a healthy dose of ath9k, iwlwifi,
and other driver updates.  There is also another pull from the
wireless tree to resolve some merge issues.  I also fixed-up some
merge discrepencies between net-next and wireless-next.
====================

Signed-off-by: David S. Miller <davem@davemloft.net>

53 files changed, 1890 insertions, 1303 deletions

diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 1fcd92380356..585c88e01893 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -231,12 +231,12 @@ static void bluecard_write_wakeup(bluecard_info_t *info)
         }
 
         do {
-                register unsigned int iobase = info->p_dev->resource[0]->start;
-                register unsigned int offset;
-                register unsigned char command;
-                register unsigned long ready_bit;
+                unsigned int iobase = info->p_dev->resource[0]->start;
+                unsigned int offset;
+                unsigned char command;
+                unsigned long ready_bit;
                 register struct sk_buff *skb;
-                register int len;
+                int len;
 
                 clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index 609861a53c28..29caaed2d715 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -470,7 +470,7 @@ static int bpa10x_probe(struct usb_interface *intf, const struct usb_device_id *
         hdev->flush    = bpa10x_flush;
         hdev->send     = bpa10x_send_frame;
 
-        set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+        set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
         err = hci_register_dev(hdev);
         if (err < 0) {
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 308c8599ab55..b2b0fbbb43b5 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -186,9 +186,9 @@ static void bt3c_write_wakeup(bt3c_info_t *info)
                 return;
 
         do {
-                register unsigned int iobase = info->p_dev->resource[0]->start;
+                unsigned int iobase = info->p_dev->resource[0]->start;
                 register struct sk_buff *skb;
-                register int len;
+                int len;
 
                 if (!pcmcia_dev_present(info->p_dev))
                         break;
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index a853244e7fd7..2867499f7256 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -110,6 +110,9 @@ static const struct sdio_device_id btmrvl_sdio_ids[] = {
         /* Marvell SD8787 Bluetooth device */
         { SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x911A),
                         .driver_data = (unsigned long) &btmrvl_sdio_sd8787 },
+        /* Marvell SD8787 Bluetooth AMP device */
+        { SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x911B),
+                        .driver_data = (unsigned long) &btmrvl_sdio_sd8787 },
         /* Marvell SD8797 Bluetooth device */
         { SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x912A),
                         .driver_data = (unsigned long) &btmrvl_sdio_sd8797 },
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index c4fc2f3fc32c..65b8d996840c 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -140,9 +140,9 @@ static void btuart_write_wakeup(btuart_info_t *info)
         }
 
         do {
-                register unsigned int iobase = info->p_dev->resource[0]->start;
+                unsigned int iobase = info->p_dev->resource[0]->start;
                 register struct sk_buff *skb;
-                register int len;
+                int len;
 
                 clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index c9463af8e564..a45e717f5f84 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -21,15 +21,7 @@
  *
  */
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/sched.h>
-#include <linux/errno.h>
-#include <linux/skbuff.h>
-
 #include <linux/usb.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -1026,7 +1018,7 @@ static int btusb_probe(struct usb_interface *intf,
         data->isoc = usb_ifnum_to_if(data->udev, 1);
 
         if (!reset)
-                set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+                set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
         if (force_scofix || id->driver_info & BTUSB_WRONG_SCO_MTU) {
                 if (!disable_scofix)
@@ -1038,7 +1030,7 @@ static int btusb_probe(struct usb_interface *intf,
 
         if (id->driver_info & BTUSB_DIGIANSWER) {
                 data->cmdreq_type = USB_TYPE_VENDOR;
-                set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+                set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
         }
 
         if (id->driver_info & BTUSB_CSR) {
@@ -1046,7 +1038,7 @@ static int btusb_probe(struct usb_interface *intf,
 
                 /* Old firmware would otherwise execute USB reset */
                 if (le16_to_cpu(udev->descriptor.bcdDevice) < 0x117)
-                        set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+                        set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
         }
 
         if (id->driver_info & BTUSB_SNIFFER) {
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 6e8d96189684..b1b37ccd3cd4 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -144,9 +144,9 @@ static void dtl1_write_wakeup(dtl1_info_t *info)
         }
 
         do {
-                register unsigned int iobase = info->p_dev->resource[0]->start;
+                unsigned int iobase = info->p_dev->resource[0]->start;
                 register struct sk_buff *skb;
-                register int len;
+                int len;
 
                 clear_bit(XMIT_WAKEUP, &(info->tx_state));
 
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 661a8dc4d2f8..57e502e06080 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -552,7 +552,7 @@ static u16 bscp_get_crc(struct bcsp_struct *bcsp)
 static int bcsp_recv(struct hci_uart *hu, void *data, int count)
 {
         struct bcsp_struct *bcsp = hu->priv;
-        register unsigned char *ptr;
+        unsigned char *ptr;
 
         BT_DBG("hu %p count %d rx_state %d rx_count %ld", 
                 hu, count, bcsp->rx_state, bcsp->rx_count);
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 748329468d26..c60623f206d4 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -126,7 +126,7 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 
 static inline int h4_check_data_len(struct h4_struct *h4, int len)
 {
-        register int room = skb_tailroom(h4->rx_skb);
+        int room = skb_tailroom(h4->rx_skb);
 
         BT_DBG("len %d room %d", len, room);
 
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index e564579a6115..2f9b796e106e 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -394,7 +394,7 @@ static int hci_uart_register_dev(struct hci_uart *hu)
                 set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
 
         if (!test_bit(HCI_UART_RESET_ON_INIT, &hu->hdev_flags))
-                set_bit(HCI_QUIRK_NO_RESET, &hdev->quirks);
+                set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
 
         if (test_bit(HCI_UART_CREATE_AMP, &hu->hdev_flags))
                 hdev->dev_type = HCI_AMP;
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index b874c0efde24..ff6d589c34a5 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -348,7 +348,7 @@ static int ll_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 
 static inline int ll_check_data_len(struct ll_struct *ll, int len)
 {
-        register int room = skb_tailroom(ll->rx_skb);
+        int room = skb_tailroom(ll->rx_skb);
 
         BT_DBG("len %d room %d", len, room);
 
@@ -374,11 +374,11 @@ static inline int ll_check_data_len(struct ll_struct *ll, int len)
 static int ll_recv(struct hci_uart *hu, void *data, int count)
 {
         struct ll_struct *ll = hu->priv;
-        register char *ptr;
+        char *ptr;
         struct hci_event_hdr *eh;
         struct hci_acl_hdr   *ah;
         struct hci_sco_hdr   *sh;
-        register int len, type, dlen;
+        int len, type, dlen;
 
         BT_DBG("hu %p count %d rx_state %ld rx_count %ld", hu, count, ll->rx_state, ll->rx_count);
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index d9e0824af093..78816b8b2173 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -181,11 +181,14 @@ static bool ar9003_hw_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
         u32 mask2 = 0;
         struct ath9k_hw_capabilities *pCap = &ah->caps;
         struct ath_common *common = ath9k_hw_common(ah);
-        u32 sync_cause = 0, async_cause;
+        u32 sync_cause = 0, async_cause, async_mask = AR_INTR_MAC_IRQ;
+
+        if (ath9k_hw_mci_is_enabled(ah))
+                async_mask |= AR_INTR_ASYNC_MASK_MCI;
 
         async_cause = REG_READ(ah, AR_INTR_ASYNC_CAUSE);
 
-        if (async_cause & (AR_INTR_MAC_IRQ | AR_INTR_ASYNC_MASK_MCI)) {
+        if (async_cause & async_mask) {
                 if ((REG_READ(ah, AR_RTC_STATUS) & AR_RTC_STATUS_M)
                                 == AR_RTC_STATUS_ON)
                         isr = REG_READ(ah, AR_ISR);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index b1ced2a76da3..cc2853ade8f8 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -321,7 +321,7 @@ void ar9003_mci_set_full_sleep(struct ath_hw *ah)
 {
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 
-        if (ar9003_mci_state(ah, MCI_STATE_ENABLE, NULL) &&
+        if (ar9003_mci_state(ah, MCI_STATE_ENABLE) &&
             (mci->bt_state != MCI_BT_SLEEP) &&
             !mci->halted_bt_gpm) {
                 ar9003_mci_send_coex_halt_bt_gpm(ah, true, true);
@@ -484,7 +484,7 @@ static void ar9003_mci_sync_bt_state(struct ath_hw *ah)
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
         u32 cur_bt_state;
 
-        cur_bt_state = ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL);
+        cur_bt_state = ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP);
 
         if (mci->bt_state != cur_bt_state)
                 mci->bt_state = cur_bt_state;
@@ -593,8 +593,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
                 if (!time_out)
                         break;
 
-                offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-                                          &more_data);
+                offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
 
                 if (offset == MCI_GPM_INVALID)
                         continue;
@@ -658,8 +657,7 @@ static u32 ar9003_mci_wait_for_gpm(struct ath_hw *ah, u8 gpm_type,
                 time_out = 0;
 
         while (more_data == MCI_GPM_MORE) {
-                offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-                                          &more_data);
+                offset = ar9003_mci_get_next_gpm_offset(ah, false, &more_data);
                 if (offset == MCI_GPM_INVALID)
                         break;
 
@@ -893,13 +891,16 @@ void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
                 udelay(100);
         }
 
+        /* Check pending GPM msg before MCI Reset Rx */
+        ar9003_mci_check_gpm_offset(ah);
+
         regval |= SM(1, AR_MCI_COMMAND2_RESET_RX);
         REG_WRITE(ah, AR_MCI_COMMAND2, regval);
         udelay(1);
         regval &= ~SM(1, AR_MCI_COMMAND2_RESET_RX);
         REG_WRITE(ah, AR_MCI_COMMAND2, regval);
 
-        ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET, NULL);
+        ar9003_mci_get_next_gpm_offset(ah, true, NULL);
 
         REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE,
                   (SM(0xe801, AR_MCI_MSG_ATTRIBUTES_TABLE_INVALID_HDR) |
@@ -1010,38 +1011,32 @@ static void ar9003_mci_queue_unsent_gpm(struct ath_hw *ah, u8 header,
         }
 }
 
-void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool wait_done)
+void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool force)
 {
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
 
-        if (!mci->update_2g5g)
+        if (!mci->update_2g5g && !force)
                 return;
 
         if (mci->is_2g) {
                 ar9003_mci_send_2g5g_status(ah, true);
-                ar9003_mci_send_lna_transfer(ah, true);
-                udelay(5);
 
-                REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
+                REG_SET_BIT(ah, AR_MCI_TX_CTRL,
                             AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
                 REG_CLR_BIT(ah, AR_PHY_GLB_CONTROL,
                             AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
 
                 if (!(mci->config & ATH_MCI_CONFIG_DISABLE_OSLA))
-                        REG_SET_BIT(ah, AR_BTCOEX_CTRL,
-                                    AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+                        ar9003_mci_osla_setup(ah, true);
         } else {
-                ar9003_mci_send_lna_take(ah, true);
-                udelay(5);
-
                 REG_SET_BIT(ah, AR_MCI_TX_CTRL,
                             AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
                 REG_SET_BIT(ah, AR_PHY_GLB_CONTROL,
                             AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
-                REG_CLR_BIT(ah, AR_BTCOEX_CTRL,
-                            AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
 
-                ar9003_mci_send_2g5g_status(ah, true);
+                ar9003_mci_osla_setup(ah, false);
+                if (!force)
+                        ar9003_mci_send_2g5g_status(ah, true);
         }
 }
 
@@ -1169,11 +1164,10 @@ void ar9003_mci_cleanup(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ar9003_mci_cleanup);
 
-u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
+u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
 {
-        struct ath_common *common = ath9k_hw_common(ah);
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
-        u32 value = 0, more_gpm = 0, gpm_ptr;
+        u32 value = 0;
         u8 query_type;
 
         switch (state_type) {
@@ -1186,81 +1180,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
                 }
                 value &= AR_BTCOEX_CTRL_MCI_MODE_EN;
                 break;
-        case MCI_STATE_INIT_GPM_OFFSET:
-                value = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
-                mci->gpm_idx = value;
-                break;
-        case MCI_STATE_NEXT_GPM_OFFSET:
-        case MCI_STATE_LAST_GPM_OFFSET:
-                /*
-                * This could be useful to avoid new GPM message interrupt which
-                * may lead to spurious interrupt after power sleep, or multiple
-                * entry of ath_mci_intr().
-                * Adding empty GPM check by returning HAL_MCI_GPM_INVALID can
-                * alleviate this effect, but clearing GPM RX interrupt bit is
-                * safe, because whether this is called from hw or driver code
-                * there must be an interrupt bit set/triggered initially
-                */
-                REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
-                          AR_MCI_INTERRUPT_RX_MSG_GPM);
-
-                gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
-                value = gpm_ptr;
-
-                if (value == 0)
-                        value = mci->gpm_len - 1;
-                else if (value >= mci->gpm_len) {
-                        if (value != 0xFFFF)
-                                value = 0;
-                } else {
-                        value--;
-                }
-
-                if (value == 0xFFFF) {
-                        value = MCI_GPM_INVALID;
-                        more_gpm = MCI_GPM_NOMORE;
-                } else if (state_type == MCI_STATE_NEXT_GPM_OFFSET) {
-                        if (gpm_ptr == mci->gpm_idx) {
-                                value = MCI_GPM_INVALID;
-                                more_gpm = MCI_GPM_NOMORE;
-                        } else {
-                                for (;;) {
-                                        u32 temp_index;
-
-                                        /* skip reserved GPM if any */
-
-                                        if (value != mci->gpm_idx)
-                                                more_gpm = MCI_GPM_MORE;
-                                        else
-                                                more_gpm = MCI_GPM_NOMORE;
-
-                                        temp_index = mci->gpm_idx;
-                                        mci->gpm_idx++;
-
-                                        if (mci->gpm_idx >=
-                                            mci->gpm_len)
-                                                mci->gpm_idx = 0;
-
-                                        if (ar9003_mci_is_gpm_valid(ah,
-                                                                    temp_index)) {
-                                                value = temp_index;
-                                                break;
-                                        }
-
-                                        if (more_gpm == MCI_GPM_NOMORE) {
-                                                value = MCI_GPM_INVALID;
-                                                break;
-                                        }
-                                }
-                        }
-                        if (p_data)
-                                *p_data = more_gpm;
-                }
-
-                if (value != MCI_GPM_INVALID)
-                        value <<= 4;
-
-                break;
         case MCI_STATE_LAST_SCHD_MSG_OFFSET:
                 value = MS(REG_READ(ah, AR_MCI_RX_STATUS),
                                     AR_MCI_RX_LAST_SCHD_MSG_INDEX);
@@ -1272,21 +1191,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
                            AR_MCI_RX_REMOTE_SLEEP) ?
                         MCI_BT_SLEEP : MCI_BT_AWAKE;
                 break;
-        case MCI_STATE_CONT_RSSI_POWER:
-                value = MS(mci->cont_status, AR_MCI_CONT_RSSI_POWER);
-                break;
-        case MCI_STATE_CONT_PRIORITY:
-                value = MS(mci->cont_status, AR_MCI_CONT_RRIORITY);
-                break;
-        case MCI_STATE_CONT_TXRX:
-                value = MS(mci->cont_status, AR_MCI_CONT_TXRX);
-                break;
-        case MCI_STATE_BT:
-                value = mci->bt_state;
-                break;
-        case MCI_STATE_SET_BT_SLEEP:
-                mci->bt_state = MCI_BT_SLEEP;
-                break;
         case MCI_STATE_SET_BT_AWAKE:
                 mci->bt_state = MCI_BT_AWAKE;
                 ar9003_mci_send_coex_version_query(ah, true);
@@ -1295,7 +1199,7 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
                 if (mci->unhalt_bt_gpm)
                         ar9003_mci_send_coex_halt_bt_gpm(ah, false, true);
 
-                ar9003_mci_2g5g_switch(ah, true);
+                ar9003_mci_2g5g_switch(ah, false);
                 break;
         case MCI_STATE_SET_BT_CAL_START:
                 mci->bt_state = MCI_BT_CAL_START;
@@ -1319,34 +1223,6 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
         case MCI_STATE_SEND_WLAN_COEX_VERSION:
                 ar9003_mci_send_coex_version_response(ah, true);
                 break;
-        case MCI_STATE_SET_BT_COEX_VERSION:
-                if (!p_data)
-                        ath_dbg(common, MCI,
-                                "MCI Set BT Coex version with NULL data!!\n");
-                else {
-                        mci->bt_ver_major = (*p_data >> 8) & 0xff;
-                        mci->bt_ver_minor = (*p_data) & 0xff;
-                        mci->bt_version_known = true;
-                        ath_dbg(common, MCI, "MCI BT version set: %d.%d\n",
-                                mci->bt_ver_major, mci->bt_ver_minor);
-                }
-                break;
-        case MCI_STATE_SEND_WLAN_CHANNELS:
-                if (p_data) {
-                        if (((mci->wlan_channels[1] & 0xffff0000) ==
-                             (*(p_data + 1) & 0xffff0000)) &&
-                            (mci->wlan_channels[2] == *(p_data + 2)) &&
-                            (mci->wlan_channels[3] == *(p_data + 3)))
-                                break;
-
-                        mci->wlan_channels[0] = *p_data++;
-                        mci->wlan_channels[1] = *p_data++;
-                        mci->wlan_channels[2] = *p_data++;
-                        mci->wlan_channels[3] = *p_data++;
-                }
-                mci->wlan_channels_update = true;
-                ar9003_mci_send_coex_wlan_channels(ah, true);
-                break;
         case MCI_STATE_SEND_VERSION_QUERY:
                 ar9003_mci_send_coex_version_query(ah, true);
                 break;
@@ -1354,29 +1230,12 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data)
                 query_type = MCI_GPM_COEX_QUERY_BT_TOPOLOGY;
                 ar9003_mci_send_coex_bt_status_query(ah, true, query_type);
                 break;
-        case MCI_STATE_NEED_FLUSH_BT_INFO:
-                /*
-                 * btcoex_hw.mci.unhalt_bt_gpm means whether it's
-                 * needed to send UNHALT message. It's set whenever
-                 * there's a request to send HALT message.
-                 * mci_halted_bt_gpm means whether HALT message is sent
-                 * out successfully.
-                 *
-                 * Checking (mci_unhalt_bt_gpm == false) instead of
-                 * checking (ah->mci_halted_bt_gpm == false) will make
-                 * sure currently is in UNHALT-ed mode and BT can
-                 * respond to status query.
-                 */
-                value = (!mci->unhalt_bt_gpm && mci->need_flush_btinfo) ? 1 : 0;
-                if (p_data)
-                        mci->need_flush_btinfo = (*p_data != 0) ? true : false;
-                break;
         case MCI_STATE_RECOVER_RX:
                 ar9003_mci_prep_interface(ah);
                 mci->query_bt = true;
                 mci->need_flush_btinfo = true;
                 ar9003_mci_send_coex_wlan_channels(ah, true);
-                ar9003_mci_2g5g_switch(ah, true);
+                ar9003_mci_2g5g_switch(ah, false);
                 break;
         case MCI_STATE_NEED_FTP_STOMP:
                 value = !(mci->config & ATH_MCI_CONFIG_DISABLE_FTP_STOMP);
@@ -1404,3 +1263,154 @@ void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah)
         /* Force another 2g5g update at next scanning */
         mci->update_2g5g = true;
 }
+
+void ar9003_mci_set_power_awake(struct ath_hw *ah)
+{
+        u32 btcoex_ctrl2, diag_sw;
+        int i;
+        u8 lna_ctrl, bt_sleep;
+
+        for (i = 0; i < AH_WAIT_TIMEOUT; i++) {
+                btcoex_ctrl2 = REG_READ(ah, AR_BTCOEX_CTRL2);
+                if (btcoex_ctrl2 != 0xdeadbeef)
+                        break;
+                udelay(AH_TIME_QUANTUM);
+        }
+        REG_WRITE(ah, AR_BTCOEX_CTRL2, (btcoex_ctrl2 | BIT(23)));
+
+        for (i = 0; i < AH_WAIT_TIMEOUT; i++) {
+                diag_sw = REG_READ(ah, AR_DIAG_SW);
+                if (diag_sw != 0xdeadbeef)
+                        break;
+                udelay(AH_TIME_QUANTUM);
+        }
+        REG_WRITE(ah, AR_DIAG_SW, (diag_sw | BIT(27) | BIT(19) | BIT(18)));
+        lna_ctrl = REG_READ(ah, AR_OBS_BUS_CTRL) & 0x3;
+        bt_sleep = REG_READ(ah, AR_MCI_RX_STATUS) & AR_MCI_RX_REMOTE_SLEEP;
+
+        REG_WRITE(ah, AR_BTCOEX_CTRL2, btcoex_ctrl2);
+        REG_WRITE(ah, AR_DIAG_SW, diag_sw);
+
+        if (bt_sleep && (lna_ctrl == 2)) {
+                REG_SET_BIT(ah, AR_BTCOEX_RC, 0x1);
+                REG_CLR_BIT(ah, AR_BTCOEX_RC, 0x1);
+                udelay(50);
+        }
+}
+
+void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
+{
+        struct ath_common *common = ath9k_hw_common(ah);
+        struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+        u32 offset;
+
+        /*
+         * This should only be called before "MAC Warm Reset" or "MCI Reset Rx".
+         */
+        offset = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+        if (mci->gpm_idx == offset)
+                return;
+        ath_dbg(common, MCI, "GPM cached write pointer mismatch %d %d\n",
+                mci->gpm_idx, offset);
+        mci->query_bt = true;
+        mci->need_flush_btinfo = true;
+        mci->gpm_idx = 0;
+}
+
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more)
+{
+        struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+        u32 offset, more_gpm = 0, gpm_ptr;
+
+        if (first) {
+                gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+                mci->gpm_idx = gpm_ptr;
+                return gpm_ptr;
+        }
+
+        /*
+         * This could be useful to avoid new GPM message interrupt which
+         * may lead to spurious interrupt after power sleep, or multiple
+         * entry of ath_mci_intr().
+         * Adding empty GPM check by returning HAL_MCI_GPM_INVALID can
+         * alleviate this effect, but clearing GPM RX interrupt bit is
+         * safe, because whether this is called from hw or driver code
+         * there must be an interrupt bit set/triggered initially
+         */
+        REG_WRITE(ah, AR_MCI_INTERRUPT_RX_MSG_RAW,
+                        AR_MCI_INTERRUPT_RX_MSG_GPM);
+
+        gpm_ptr = MS(REG_READ(ah, AR_MCI_GPM_1), AR_MCI_GPM_WRITE_PTR);
+        offset = gpm_ptr;
+
+        if (!offset)
+                offset = mci->gpm_len - 1;
+        else if (offset >= mci->gpm_len) {
+                if (offset != 0xFFFF)
+                        offset = 0;
+        } else {
+                offset--;
+        }
+
+        if ((offset == 0xFFFF) || (gpm_ptr == mci->gpm_idx)) {
+                offset = MCI_GPM_INVALID;
+                more_gpm = MCI_GPM_NOMORE;
+                goto out;
+        }
+        for (;;) {
+                u32 temp_index;
+
+                /* skip reserved GPM if any */
+
+                if (offset != mci->gpm_idx)
+                        more_gpm = MCI_GPM_MORE;
+                else
+                        more_gpm = MCI_GPM_NOMORE;
+
+                temp_index = mci->gpm_idx;
+                mci->gpm_idx++;
+
+                if (mci->gpm_idx >= mci->gpm_len)
+                        mci->gpm_idx = 0;
+
+                if (ar9003_mci_is_gpm_valid(ah, temp_index)) {
+                        offset = temp_index;
+                        break;
+                }
+
+                if (more_gpm == MCI_GPM_NOMORE) {
+                        offset = MCI_GPM_INVALID;
+                        break;
+                }
+        }
+
+        if (offset != MCI_GPM_INVALID)
+                offset <<= 4;
+out:
+        if (more)
+                *more = more_gpm;
+
+        return offset;
+}
+EXPORT_SYMBOL(ar9003_mci_get_next_gpm_offset);
+
+void ar9003_mci_set_bt_version(struct ath_hw *ah, u8 major, u8 minor)
+{
+        struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+        mci->bt_ver_major = major;
+        mci->bt_ver_minor = minor;
+        mci->bt_version_known = true;
+        ath_dbg(ath9k_hw_common(ah), MCI, "MCI BT version set: %d.%d\n",
+                mci->bt_ver_major, mci->bt_ver_minor);
+}
+EXPORT_SYMBOL(ar9003_mci_set_bt_version);
+
+void ar9003_mci_send_wlan_channels(struct ath_hw *ah)
+{
+        struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+
+        mci->wlan_channels_update = true;
+        ar9003_mci_send_coex_wlan_channels(ah, true);
+}
+EXPORT_SYMBOL(ar9003_mci_send_wlan_channels);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.h b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
index 10282e2bcdc9..d33b8e128855 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
@@ -189,26 +189,15 @@ enum mci_bt_state {
 /* Type of state query */
 enum mci_state_type {
         MCI_STATE_ENABLE,
-        MCI_STATE_INIT_GPM_OFFSET,
-        MCI_STATE_NEXT_GPM_OFFSET,
-        MCI_STATE_LAST_GPM_OFFSET,
-        MCI_STATE_BT,
-        MCI_STATE_SET_BT_SLEEP,
         MCI_STATE_SET_BT_AWAKE,
         MCI_STATE_SET_BT_CAL_START,
         MCI_STATE_SET_BT_CAL,
         MCI_STATE_LAST_SCHD_MSG_OFFSET,
         MCI_STATE_REMOTE_SLEEP,
-        MCI_STATE_CONT_RSSI_POWER,
-        MCI_STATE_CONT_PRIORITY,
-        MCI_STATE_CONT_TXRX,
         MCI_STATE_RESET_REQ_WAKE,
         MCI_STATE_SEND_WLAN_COEX_VERSION,
-        MCI_STATE_SET_BT_COEX_VERSION,
-        MCI_STATE_SEND_WLAN_CHANNELS,
         MCI_STATE_SEND_VERSION_QUERY,
         MCI_STATE_SEND_STATUS_QUERY,
-        MCI_STATE_NEED_FLUSH_BT_INFO,
         MCI_STATE_SET_CONCUR_TX_PRI,
         MCI_STATE_RECOVER_RX,
         MCI_STATE_NEED_FTP_STOMP,
@@ -259,14 +248,15 @@ enum mci_gpm_coex_opcode {
 bool ar9003_mci_send_message(struct ath_hw *ah, u8 header, u32 flag,
                              u32 *payload, u8 len, bool wait_done,
                              bool check_bt);
-u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type, u32 *p_data);
+u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type);
 void ar9003_mci_setup(struct ath_hw *ah, u32 gpm_addr, void *gpm_buf,
                       u16 len, u32 sched_addr);
 void ar9003_mci_cleanup(struct ath_hw *ah);
 void ar9003_mci_get_interrupt(struct ath_hw *ah, u32 *raw_intr,
                               u32 *rx_msg_intr);
-void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah);
-
+u32 ar9003_mci_get_next_gpm_offset(struct ath_hw *ah, bool first, u32 *more);
+void ar9003_mci_set_bt_version(struct ath_hw *ah, u8 major, u8 minor);
+void ar9003_mci_send_wlan_channels(struct ath_hw *ah);
 /*
  * These functions are used by ath9k_hw.
  */
@@ -277,7 +267,7 @@ void ar9003_mci_stop_bt(struct ath_hw *ah, bool save_fullsleep);
 void ar9003_mci_init_cal_req(struct ath_hw *ah, bool *is_reusable);
 void ar9003_mci_init_cal_done(struct ath_hw *ah);
 void ar9003_mci_set_full_sleep(struct ath_hw *ah);
-void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool wait_done);
+void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool force);
 void ar9003_mci_check_bt(struct ath_hw *ah);
 bool ar9003_mci_start_reset(struct ath_hw *ah, struct ath9k_channel *chan);
 int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
@@ -285,6 +275,9 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
                       bool is_full_sleep);
 void ar9003_mci_get_isr(struct ath_hw *ah, enum ath9k_int *masked);
+void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah);
+void ar9003_mci_set_power_awake(struct ath_hw *ah);
+void ar9003_mci_check_gpm_offset(struct ath_hw *ah);
 
 #else
 
@@ -322,6 +315,15 @@ static inline void ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 static inline void ar9003_mci_get_isr(struct ath_hw *ah, enum ath9k_int *masked)
 {
 }
+static inline void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah)
+{
+}
+static inline void ar9003_mci_set_power_awake(struct ath_hw *ah)
+{
+}
+static inline void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
+{
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index 4a93e1534c1d..8f406ff2c95e 100644
--- a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
@@ -52,7 +52,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
         {0x00009e04, 0x001c2020, 0x001c2020, 0x001c2020, 0x001c2020},
         {0x00009e0c, 0x6c4000e2, 0x6d4000e2, 0x6d4000e2, 0x6c4000d8},
         {0x00009e10, 0x92c88d2e, 0x7ec88d2e, 0x7ec84d2e, 0x7ec86d2e},
-        {0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3376605e, 0x33795d5e},
+        {0x00009e14, 0x37b95d5e, 0x37b9605e, 0x3376605e, 0x32395d5e},
         {0x00009e18, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
         {0x00009e1c, 0x0001cf9c, 0x0001cf9c, 0x00021f9c, 0x00021f9c},
         {0x00009e20, 0x000003b5, 0x000003b5, 0x000003ce, 0x000003ce},
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 02fc1c1e5eeb..a8c050085648 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -698,6 +698,7 @@ struct ath_softc {
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
         struct ath_btcoex btcoex;
         struct ath_mci_coex mci_coex;
+        struct work_struct mci_work;
 #endif
 
         struct ath_descdma txsdma;
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index af6d27350291..26032cb59b8a 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -202,7 +202,7 @@ static void ath_btcoex_period_timer(unsigned long data)
 
         btcoex->bt_wait_time += btcoex->btcoex_period;
         if (btcoex->bt_wait_time > ATH_BTCOEX_RX_WAIT_TIME) {
-                if (ar9003_mci_state(ah, MCI_STATE_NEED_FTP_STOMP, NULL) &&
+                if (ar9003_mci_state(ah, MCI_STATE_NEED_FTP_STOMP) &&
                     (mci->num_pan || mci->num_other_acl))
                         ah->btcoex_hw.mci.stomp_ftp =
                                 (sc->rx.num_pkts < ATH_BTCOEX_STOMP_FTP_THRESH);
@@ -232,7 +232,7 @@ static void ath_btcoex_period_timer(unsigned long data)
         }
 
         ath9k_ps_restore(sc);
-        timer_period = btcoex->btcoex_period / 1000;
+        timer_period = btcoex->btcoex_period;
         mod_timer(&btcoex->period_timer, jiffies + msecs_to_jiffies(timer_period));
 }
 
@@ -267,10 +267,10 @@ static int ath_init_btcoex_timer(struct ath_softc *sc)
 {
         struct ath_btcoex *btcoex = &sc->btcoex;
 
-        btcoex->btcoex_period = ATH_BTCOEX_DEF_BT_PERIOD * 1000;
-        btcoex->btcoex_no_stomp = (100 - ATH_BTCOEX_DEF_DUTY_CYCLE) *
+        btcoex->btcoex_period = ATH_BTCOEX_DEF_BT_PERIOD;
+        btcoex->btcoex_no_stomp = (100 - ATH_BTCOEX_DEF_DUTY_CYCLE) * 1000 *
                 btcoex->btcoex_period / 100;
-        btcoex->btscan_no_stomp = (100 - ATH_BTCOEX_BTSCAN_DUTY_CYCLE) *
+        btcoex->btscan_no_stomp = (100 - ATH_BTCOEX_BTSCAN_DUTY_CYCLE) * 1000 *
                                    btcoex->btcoex_period / 100;
 
         setup_timer(&btcoex->period_timer, ath_btcoex_period_timer,
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 45e670087e1c..784baee5db84 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1348,6 +1348,9 @@ static bool ath9k_hw_set_reset(struct ath_hw *ah, int type)
                 }
         }
 
+        if (ath9k_hw_mci_is_enabled(ah))
+                ar9003_mci_check_gpm_offset(ah);
+
         REG_WRITE(ah, AR_RTC_RC, rst_flags);
 
         REGWRITE_BUFFER_FLUSH(ah);
@@ -1708,7 +1711,7 @@ static int ath9k_hw_do_fastcc(struct ath_hw *ah, struct ath9k_channel *chan)
         ath9k_hw_start_nfcal(ah, true);
 
         if (ath9k_hw_mci_is_enabled(ah))
-                ar9003_mci_2g5g_switch(ah, true);
+                ar9003_mci_2g5g_switch(ah, false);
 
         if (AR_SREV_9271(ah))
                 ar9002_hw_load_ani_reg(ah, chan);
@@ -1912,7 +1915,8 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
         ath9k_hw_set_dma(ah);
 
-        REG_WRITE(ah, AR_OBS, 8);
+        if (!ath9k_hw_mci_is_enabled(ah))
+                REG_WRITE(ah, AR_OBS, 8);
 
         if (ah->config.rx_intr_mitigation) {
                 REG_RMW_FIELD(ah, AR_RIMT, AR_RIMT_LAST, 500);
@@ -2111,6 +2115,9 @@ static bool ath9k_hw_set_power_awake(struct ath_hw *ah)
                     AR_RTC_FORCE_WAKE_EN);
         udelay(50);
 
+        if (ath9k_hw_mci_is_enabled(ah))
+                ar9003_mci_set_power_awake(ah);
+
         for (i = POWER_UP_TIME / 50; i > 0; i--) {
                 val = REG_READ(ah, AR_RTC_STATUS) & AR_RTC_STATUS_M;
                 if (val == AR_RTC_STATUS_ON)
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 0cc4c70f7f0c..a105c9426251 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -136,6 +136,14 @@ void ath_hw_pll_work(struct work_struct *work)
         u32 pll_sqsum;
         struct ath_softc *sc = container_of(work, struct ath_softc,
                                             hw_pll_work.work);
+        /*
+         * ensure that the PLL WAR is executed only
+         * after the STA is associated (or) if the
+         * beaconing had started in interfaces that
+         * uses beacons.
+         */
+        if (!test_bit(SC_OP_BEACONS, &sc->sc_flags))
+                return;
 
         ath9k_ps_wakeup(sc);
         pll_sqsum = ar9003_get_pll_sqsum_dvc(sc->sc_ah);
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index c0f478b0a9a2..52561b341d68 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -150,6 +150,9 @@ static void __ath_cancel_work(struct ath_softc *sc)
         cancel_work_sync(&sc->hw_check_work);
         cancel_delayed_work_sync(&sc->tx_complete_work);
         cancel_delayed_work_sync(&sc->hw_pll_work);
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+        cancel_work_sync(&sc->mci_work);
+#endif
 }
 
 static void ath_cancel_work(struct ath_softc *sc)
@@ -1033,15 +1036,6 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
                 }
         }
 
-        if ((ah->opmode == NL80211_IFTYPE_ADHOC) ||
-            ((vif->type == NL80211_IFTYPE_ADHOC) &&
-             sc->nvifs > 0)) {
-                ath_err(common, "Cannot create ADHOC interface when other"
-                        " interfaces already exist.\n");
-                ret = -EINVAL;
-                goto out;
-        }
-
         ath_dbg(common, CONFIG, "Attach a VIF of type: %d\n", vif->type);
 
         sc->nvifs++;
@@ -1066,15 +1060,6 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
         mutex_lock(&sc->mutex);
         ath9k_ps_wakeup(sc);
 
-        /* See if new interface type is valid. */
-        if ((new_type == NL80211_IFTYPE_ADHOC) &&
-            (sc->nvifs > 1)) {
-                ath_err(common, "When using ADHOC, it must be the only"
-                        " interface.\n");
-                ret = -EINVAL;
-                goto out;
-        }
-
         if (ath9k_uses_beacons(new_type) &&
             !ath9k_uses_beacons(vif->type)) {
                 if (sc->nbcnvifs >= ATH_BCBUF) {
@@ -1258,6 +1243,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
                 if (ath_set_channel(sc, hw, &sc->sc_ah->channels[pos]) < 0) {
                         ath_err(common, "Unable to set channel\n");
                         mutex_unlock(&sc->mutex);
+                        ath9k_ps_restore(sc);
                         return -EINVAL;
                 }
 
diff --git a/drivers/net/wireless/ath/ath9k/mci.c b/drivers/net/wireless/ath/ath9k/mci.c
index 49137f477b05..7d34a504d617 100644
--- a/drivers/net/wireless/ath/ath9k/mci.c
+++ b/drivers/net/wireless/ath/ath9k/mci.c
@@ -20,7 +20,7 @@
 #include "ath9k.h"
 #include "mci.h"
 
-static const u8 ath_mci_duty_cycle[] = { 0, 50, 60, 70, 80, 85, 90, 95, 98 };
+static const u8 ath_mci_duty_cycle[] = { 55, 50, 60, 70, 80, 85, 90, 95, 98 };
 
 static struct ath_mci_profile_info*
 ath_mci_find_profile(struct ath_mci_profile *mci,
@@ -28,11 +28,14 @@ ath_mci_find_profile(struct ath_mci_profile *mci,
 {
         struct ath_mci_profile_info *entry;
 
+        if (list_empty(&mci->info))
+                return NULL;
+
         list_for_each_entry(entry, &mci->info, list) {
                 if (entry->conn_handle == info->conn_handle)
-                        break;
+                        return entry;
         }
-        return entry;
+        return NULL;
 }
 
 static bool ath_mci_add_profile(struct ath_common *common,
@@ -49,31 +52,21 @@ static bool ath_mci_add_profile(struct ath_common *common,
             (info->type != MCI_GPM_COEX_PROFILE_VOICE))
                 return false;
 
-        entry = ath_mci_find_profile(mci, info);
-
-        if (entry) {
-                memcpy(entry, info, 10);
-        } else {
-                entry = kzalloc(sizeof(*entry), GFP_KERNEL);
-                if (!entry)
-                        return false;
+        entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
+        if (!entry)
+                return false;
 
-                memcpy(entry, info, 10);
-                INC_PROF(mci, info);
-                list_add_tail(&info->list, &mci->info);
-        }
+        memcpy(entry, info, 10);
+        INC_PROF(mci, info);
+        list_add_tail(&entry->list, &mci->info);
 
         return true;
 }
 
 static void ath_mci_del_profile(struct ath_common *common,
                                 struct ath_mci_profile *mci,
-                                struct ath_mci_profile_info *info)
+                                struct ath_mci_profile_info *entry)
 {
-        struct ath_mci_profile_info *entry;
-
-        entry = ath_mci_find_profile(mci, info);
-
         if (!entry)
                 return;
 
@@ -86,12 +79,16 @@ void ath_mci_flush_profile(struct ath_mci_profile *mci)
 {
         struct ath_mci_profile_info *info, *tinfo;
 
+        mci->aggr_limit = 0;
+
+        if (list_empty(&mci->info))
+                return;
+
         list_for_each_entry_safe(info, tinfo, &mci->info, list) {
                 list_del(&info->list);
                 DEC_PROF(mci, info);
                 kfree(info);
         }
-        mci->aggr_limit = 0;
 }
 
 static void ath_mci_adjust_aggr_limit(struct ath_btcoex *btcoex)
@@ -123,6 +120,8 @@ static void ath_mci_update_scheme(struct ath_softc *sc)
         if (mci_hw->config & ATH_MCI_CONFIG_DISABLE_TUNING)
                 goto skip_tuning;
 
+        btcoex->duty_cycle = ath_mci_duty_cycle[num_profile];
+
         if (num_profile == 1) {
                 info = list_first_entry(&mci->info,
                                         struct ath_mci_profile_info,
@@ -181,12 +180,11 @@ skip_tuning:
         if (IS_CHAN_5GHZ(sc->sc_ah->curchan))
                 return;
 
-        btcoex->duty_cycle += (mci->num_bdr ? ATH_MCI_MAX_DUTY_CYCLE : 0);
+        btcoex->duty_cycle += (mci->num_bdr ? ATH_MCI_BDR_DUTY_CYCLE : 0);
         if (btcoex->duty_cycle > ATH_MCI_MAX_DUTY_CYCLE)
                 btcoex->duty_cycle = ATH_MCI_MAX_DUTY_CYCLE;
 
-        btcoex->btcoex_period *= 1000;
-        btcoex->btcoex_no_stomp =  btcoex->btcoex_period *
+        btcoex->btcoex_no_stomp =  btcoex->btcoex_period * 1000 *
                 (100 - btcoex->duty_cycle) / 100;
 
         ath9k_hw_btcoex_enable(sc->sc_ah);
@@ -197,20 +195,16 @@ static void ath_mci_cal_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
 {
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
+        struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
         u32 payload[4] = {0, 0, 0, 0};
 
         switch (opcode) {
         case MCI_GPM_BT_CAL_REQ:
-                if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_AWAKE) {
-                        ar9003_mci_state(ah, MCI_STATE_SET_BT_CAL_START, NULL);
+                if (mci_hw->bt_state == MCI_BT_AWAKE) {
+                        ar9003_mci_state(ah, MCI_STATE_SET_BT_CAL_START);
                         ieee80211_queue_work(sc->hw, &sc->hw_reset_work);
-                } else {
-                        ath_dbg(common, MCI, "MCI State mismatch: %d\n",
-                                ar9003_mci_state(ah, MCI_STATE_BT, NULL));
                 }
-                break;
-        case MCI_GPM_BT_CAL_DONE:
-                ar9003_mci_state(ah, MCI_STATE_BT, NULL);
+                ath_dbg(common, MCI, "MCI State : %d\n", mci_hw->bt_state);
                 break;
         case MCI_GPM_BT_CAL_GRANT:
                 MCI_GPM_SET_CAL_TYPE(payload, MCI_GPM_WLAN_CAL_DONE);
@@ -223,32 +217,42 @@ static void ath_mci_cal_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
         }
 }
 
+static void ath9k_mci_work(struct work_struct *work)
+{
+        struct ath_softc *sc = container_of(work, struct ath_softc, mci_work);
+
+        ath_mci_update_scheme(sc);
+}
+
 static void ath_mci_process_profile(struct ath_softc *sc,
                                     struct ath_mci_profile_info *info)
 {
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
         struct ath_btcoex *btcoex = &sc->btcoex;
         struct ath_mci_profile *mci = &btcoex->mci;
+        struct ath_mci_profile_info *entry = NULL;
+
+        entry = ath_mci_find_profile(mci, info);
+        if (entry)
+                memcpy(entry, info, 10);
 
         if (info->start) {
-                if (!ath_mci_add_profile(common, mci, info))
+                if (!entry && !ath_mci_add_profile(common, mci, info))
                         return;
         } else
-                ath_mci_del_profile(common, mci, info);
+                ath_mci_del_profile(common, mci, entry);
 
         btcoex->btcoex_period = ATH_MCI_DEF_BT_PERIOD;
         mci->aggr_limit = mci->num_sco ? 6 : 0;
 
-        if (NUM_PROF(mci)) {
+        btcoex->duty_cycle = ath_mci_duty_cycle[NUM_PROF(mci)];
+        if (NUM_PROF(mci))
                 btcoex->bt_stomp_type = ATH_BTCOEX_STOMP_LOW;
-                btcoex->duty_cycle = ath_mci_duty_cycle[NUM_PROF(mci)];
-        } else {
+        else
                 btcoex->bt_stomp_type = mci->num_mgmt ? ATH_BTCOEX_STOMP_ALL :
                                                         ATH_BTCOEX_STOMP_LOW;
-                btcoex->duty_cycle = ATH_BTCOEX_DEF_DUTY_CYCLE;
-        }
 
-        ath_mci_update_scheme(sc);
+        ieee80211_queue_work(sc->hw, &sc->mci_work);
 }
 
 static void ath_mci_process_status(struct ath_softc *sc,
@@ -263,8 +267,6 @@ static void ath_mci_process_status(struct ath_softc *sc,
         if (status->is_link)
                 return;
 
-        memset(&info, 0, sizeof(struct ath_mci_profile_info));
-
         info.conn_handle = status->conn_handle;
         if (ath_mci_find_profile(mci, &info))
                 return;
@@ -284,7 +286,7 @@ static void ath_mci_process_status(struct ath_softc *sc,
         } while (++i < ATH_MCI_MAX_PROFILE);
 
         if (old_num_mgmt != mci->num_mgmt)
-                ath_mci_update_scheme(sc);
+                ieee80211_queue_work(sc->hw, &sc->mci_work);
 }
 
 static void ath_mci_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
@@ -293,25 +295,20 @@ static void ath_mci_msg(struct ath_softc *sc, u8 opcode, u8 *rx_payload)
         struct ath_mci_profile_info profile_info;
         struct ath_mci_profile_status profile_status;
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-        u32 version;
-        u8 major;
-        u8 minor;
+        u8 major, minor;
         u32 seq_num;
 
         switch (opcode) {
         case MCI_GPM_COEX_VERSION_QUERY:
-                version = ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_COEX_VERSION,
-                                           NULL);
+                ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_COEX_VERSION);
                 break;
         case MCI_GPM_COEX_VERSION_RESPONSE:
                 major = *(rx_payload + MCI_GPM_COEX_B_MAJOR_VERSION);
                 minor = *(rx_payload + MCI_GPM_COEX_B_MINOR_VERSION);
-                version = (major << 8) + minor;
-                version = ar9003_mci_state(ah, MCI_STATE_SET_BT_COEX_VERSION,
-                                           &version);
+                ar9003_mci_set_bt_version(ah, major, minor);
                 break;
         case MCI_GPM_COEX_STATUS_QUERY:
-                ar9003_mci_state(ah, MCI_STATE_SEND_WLAN_CHANNELS, NULL);
+                ar9003_mci_send_wlan_channels(ah);
                 break;
         case MCI_GPM_COEX_BT_PROFILE_INFO:
                 memcpy(&profile_info,
@@ -378,6 +375,7 @@ int ath_mci_setup(struct ath_softc *sc)
                          mci->gpm_buf.bf_addr, (mci->gpm_buf.bf_len >> 4),
                          mci->sched_buf.bf_paddr);
 
+        INIT_WORK(&sc->mci_work, ath9k_mci_work);
         ath_dbg(common, MCI, "MCI Initialized\n");
 
         return 0;
@@ -405,6 +403,7 @@ void ath_mci_intr(struct ath_softc *sc)
         struct ath_mci_coex *mci = &sc->mci_coex;
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
+        struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
         u32 mci_int, mci_int_rxmsg;
         u32 offset, subtype, opcode;
         u32 *pgpm;
@@ -413,8 +412,8 @@ void ath_mci_intr(struct ath_softc *sc)
 
         ar9003_mci_get_interrupt(sc->sc_ah, &mci_int, &mci_int_rxmsg);
 
-        if (ar9003_mci_state(ah, MCI_STATE_ENABLE, NULL) == 0) {
-                ar9003_mci_state(ah, MCI_STATE_INIT_GPM_OFFSET, NULL);
+        if (ar9003_mci_state(ah, MCI_STATE_ENABLE) == 0) {
+                ar9003_mci_get_next_gpm_offset(ah, true, NULL);
                 return;
         }
 
@@ -433,46 +432,41 @@ void ath_mci_intr(struct ath_softc *sc)
                                         NULL, 0, true, false);
 
                 mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_REQ_WAKE;
-                ar9003_mci_state(ah, MCI_STATE_RESET_REQ_WAKE, NULL);
+                ar9003_mci_state(ah, MCI_STATE_RESET_REQ_WAKE);
 
                 /*
                  * always do this for recovery and 2G/5G toggling and LNA_TRANS
                  */
-                ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE, NULL);
+                ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE);
         }
 
         if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING) {
                 mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SYS_WAKING;
 
-                if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_SLEEP) {
-                        if (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL) !=
-                            MCI_BT_SLEEP)
-                                ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE,
-                                                 NULL);
-                }
+                if ((mci_hw->bt_state == MCI_BT_SLEEP) &&
+                    (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP) !=
+                     MCI_BT_SLEEP))
+                        ar9003_mci_state(ah, MCI_STATE_SET_BT_AWAKE);
         }
 
         if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING) {
                 mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SYS_SLEEPING;
 
-                if (ar9003_mci_state(ah, MCI_STATE_BT, NULL) == MCI_BT_AWAKE) {
-                        if (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP, NULL) !=
-                            MCI_BT_AWAKE)
-                                ar9003_mci_state(ah, MCI_STATE_SET_BT_SLEEP,
-                                                 NULL);
-                }
+                if ((mci_hw->bt_state == MCI_BT_AWAKE) &&
+                    (ar9003_mci_state(ah, MCI_STATE_REMOTE_SLEEP) !=
+                     MCI_BT_AWAKE))
+                        mci_hw->bt_state = MCI_BT_SLEEP;
         }
 
         if ((mci_int & AR_MCI_INTERRUPT_RX_INVALID_HDR) ||
             (mci_int & AR_MCI_INTERRUPT_CONT_INFO_TIMEOUT)) {
-                ar9003_mci_state(ah, MCI_STATE_RECOVER_RX, NULL);
+                ar9003_mci_state(ah, MCI_STATE_RECOVER_RX);
                 skip_gpm = true;
         }
 
         if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO) {
                 mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_SCHD_INFO;
-                offset = ar9003_mci_state(ah, MCI_STATE_LAST_SCHD_MSG_OFFSET,
-                                          NULL);
+                offset = ar9003_mci_state(ah, MCI_STATE_LAST_SCHD_MSG_OFFSET);
         }
 
         if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_GPM) {
@@ -481,8 +475,8 @@ void ath_mci_intr(struct ath_softc *sc)
                 while (more_data == MCI_GPM_MORE) {
 
                         pgpm = mci->gpm_buf.bf_addr;
-                        offset = ar9003_mci_state(ah, MCI_STATE_NEXT_GPM_OFFSET,
-                                                  &more_data);
+                        offset = ar9003_mci_get_next_gpm_offset(ah, false,
+                                                                &more_data);
 
                         if (offset == MCI_GPM_INVALID)
                                 break;
@@ -523,23 +517,17 @@ void ath_mci_intr(struct ath_softc *sc)
                         mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_LNA_INFO;
 
                 if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_CONT_INFO) {
-                        int value_dbm = ar9003_mci_state(ah,
-                                                 MCI_STATE_CONT_RSSI_POWER, NULL);
+                        int value_dbm = MS(mci_hw->cont_status,
+                                           AR_MCI_CONT_RSSI_POWER);
 
                         mci_int_rxmsg &= ~AR_MCI_INTERRUPT_RX_MSG_CONT_INFO;
 
-                        if (ar9003_mci_state(ah, MCI_STATE_CONT_TXRX, NULL))
-                                ath_dbg(common, MCI,
-                                        "MCI CONT_INFO: (tx) pri = %d, pwr = %d dBm\n",
-                                        ar9003_mci_state(ah,
-                                                 MCI_STATE_CONT_PRIORITY, NULL),
-                                        value_dbm);
-                        else
-                                ath_dbg(common, MCI,
-                                        "MCI CONT_INFO: (rx) pri = %d,pwr = %d dBm\n",
-                                        ar9003_mci_state(ah,
-                                                 MCI_STATE_CONT_PRIORITY, NULL),
-                                        value_dbm);
+                        ath_dbg(common, MCI,
+                                "MCI CONT_INFO: (%s) pri = %d pwr = %d dBm\n",
+                                MS(mci_hw->cont_status, AR_MCI_CONT_TXRX) ?
+                                "tx" : "rx",
+                                MS(mci_hw->cont_status, AR_MCI_CONT_PRIORITY),
+                                value_dbm);
                 }
 
                 if (mci_int_rxmsg & AR_MCI_INTERRUPT_RX_MSG_CONT_NACK)
diff --git a/drivers/net/wireless/ath/ath9k/rc.c b/drivers/net/wireless/ath/ath9k/rc.c
index 92a6c0a87f89..e034add9cd5a 100644
--- a/drivers/net/wireless/ath/ath9k/rc.c
+++ b/drivers/net/wireless/ath/ath9k/rc.c
@@ -770,7 +770,7 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
         struct ieee80211_tx_rate *rates = tx_info->control.rates;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         __le16 fc = hdr->frame_control;
-        u8 try_per_rate, i = 0, rix, high_rix;
+        u8 try_per_rate, i = 0, rix;
         int is_probe = 0;
 
         if (rate_control_send_low(sta, priv_sta, txrc))
@@ -791,7 +791,6 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
         rate_table = ath_rc_priv->rate_table;
         rix = ath_rc_get_highest_rix(sc, ath_rc_priv, rate_table,
                                      &is_probe, false);
-        high_rix = rix;
 
         /*
          * If we're in HT mode and both us and our peer supports LDPC.
@@ -839,16 +838,16 @@ static void ath_get_rate(void *priv, struct ieee80211_sta *sta, void *priv_sta,
         try_per_rate = 8;
 
         /*
-         * Use a legacy rate as last retry to ensure that the frame
-         * is tried in both MCS and legacy rates.
+         * If the last rate in the rate series is MCS and has
+         * more than 80% of per thresh, then use a legacy rate
+         * as last retry to ensure that the frame is tried in both
+         * MCS and legacy rate.
          */
-        if ((rates[2].flags & IEEE80211_TX_RC_MCS) &&
-            (!(tx_info->flags & IEEE80211_TX_CTL_AMPDU) ||
-            (ath_rc_priv->per[high_rix] > 45)))
+        ath_rc_get_lower_rix(rate_table, ath_rc_priv, rix, &rix);
+        if (WLAN_RC_PHY_HT(rate_table->info[rix].phy) &&
+            (ath_rc_priv->per[rix] > 45))
                 rix = ath_rc_get_highest_rix(sc, ath_rc_priv, rate_table,
                                 &is_probe, true);
-        else
-                ath_rc_get_lower_rix(rate_table, ath_rc_priv, rix, &rix);
 
         /* All other rates in the series have RTS enabled */
         ath_rc_rate_set_series(rate_table, &rates[i], txrc,
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 560d6effac7a..75acefbd4937 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -2098,8 +2098,8 @@ enum {
 #define AR_MCI_CONT_STATUS                      0x1848
 #define AR_MCI_CONT_RSSI_POWER                  0x000000FF
 #define AR_MCI_CONT_RSSI_POWER_S                0
-#define AR_MCI_CONT_RRIORITY                    0x0000FF00
-#define AR_MCI_CONT_RRIORITY_S                  8
+#define AR_MCI_CONT_PRIORITY                    0x0000FF00
+#define AR_MCI_CONT_PRIORITY_S                  8
 #define AR_MCI_CONT_TXRX                        0x00010000
 #define AR_MCI_CONT_TXRX_S                      16
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index abb48032753b..9d5170b6df50 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -34,3 +34,5 @@ brcmfmac-$(CONFIG_BRCMFMAC_SDIO) += \
                 sdio_chip.o
 brcmfmac-$(CONFIG_BRCMFMAC_USB) += \
                 usb.o
+brcmfmac-$(CONFIG_BRCMDBG) += \
+                dhd_dbg.o
\ No newline at end of file
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index 9f637014486e..a11fe54f5950 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
@@ -613,6 +613,9 @@ struct brcmf_pub {
         struct work_struct multicast_work;
         u8 macvalue[ETH_ALEN];
         atomic_t pend_8021x_cnt;
+#ifdef DEBUG
+        struct dentry *dbgfs_dir;
+#endif
 };
 
 struct brcmf_if_event {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
new file mode 100644
index 000000000000..7f89540b56da
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
@@ -0,0 +1,126 @@
+/*
+ * Copyright (c) 2012 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/debugfs.h>
+#include <linux/if_ether.h>
+#include <linux/if.h>
+#include <linux/ieee80211.h>
+#include <linux/module.h>
+
+#include <defs.h>
+#include <brcmu_wifi.h>
+#include <brcmu_utils.h>
+#include "dhd.h"
+#include "dhd_bus.h"
+#include "dhd_dbg.h"
+
+static struct dentry *root_folder;
+
+void brcmf_debugfs_init(void)
+{
+        root_folder = debugfs_create_dir(KBUILD_MODNAME, NULL);
+        if (IS_ERR(root_folder))
+                root_folder = NULL;
+}
+
+void brcmf_debugfs_exit(void)
+{
+        if (!root_folder)
+                return;
+
+        debugfs_remove_recursive(root_folder);
+        root_folder = NULL;
+}
+
+int brcmf_debugfs_attach(struct brcmf_pub *drvr)
+{
+        if (!root_folder)
+                return -ENODEV;
+
+        drvr->dbgfs_dir = debugfs_create_dir(dev_name(drvr->dev), root_folder);
+        return PTR_RET(drvr->dbgfs_dir);
+}
+
+void brcmf_debugfs_detach(struct brcmf_pub *drvr)
+{
+        if (!IS_ERR_OR_NULL(drvr->dbgfs_dir))
+                debugfs_remove_recursive(drvr->dbgfs_dir);
+}
+
+struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr)
+{
+        return drvr->dbgfs_dir;
+}
+
+static
+ssize_t brcmf_debugfs_sdio_counter_read(struct file *f, char __user *data,
+                                        size_t count, loff_t *ppos)
+{
+        struct brcmf_sdio_count *sdcnt = f->private_data;
+        char buf[750];
+        int res;
+
+        /* only allow read from start */
+        if (*ppos > 0)
+                return 0;
+
+        res = scnprintf(buf, sizeof(buf),
+                        "intrcount:    %u\nlastintrs:    %u\n"
+                        "pollcnt:      %u\nregfails:     %u\n"
+                        "tx_sderrs:    %u\nfcqueued:     %u\n"
+                        "rxrtx:        %u\nrx_toolong:   %u\n"
+                        "rxc_errors:   %u\nrx_hdrfail:   %u\n"
+                        "rx_badhdr:    %u\nrx_badseq:    %u\n"
+                        "fc_rcvd:      %u\nfc_xoff:      %u\n"
+                        "fc_xon:       %u\nrxglomfail:   %u\n"
+                        "rxglomframes: %u\nrxglompkts:   %u\n"
+                        "f2rxhdrs:     %u\nf2rxdata:     %u\n"
+                        "f2txdata:     %u\nf1regdata:    %u\n"
+                        "tickcnt:      %u\ntx_ctlerrs:   %lu\n"
+                        "tx_ctlpkts:   %lu\nrx_ctlerrs:   %lu\n"
+                        "rx_ctlpkts:   %lu\nrx_readahead: %lu\n",
+                        sdcnt->intrcount, sdcnt->lastintrs,
+                        sdcnt->pollcnt, sdcnt->regfails,
+                        sdcnt->tx_sderrs, sdcnt->fcqueued,
+                        sdcnt->rxrtx, sdcnt->rx_toolong,
+                        sdcnt->rxc_errors, sdcnt->rx_hdrfail,
+                        sdcnt->rx_badhdr, sdcnt->rx_badseq,
+                        sdcnt->fc_rcvd, sdcnt->fc_xoff,
+                        sdcnt->fc_xon, sdcnt->rxglomfail,
+                        sdcnt->rxglomframes, sdcnt->rxglompkts,
+                        sdcnt->f2rxhdrs, sdcnt->f2rxdata,
+                        sdcnt->f2txdata, sdcnt->f1regdata,
+                        sdcnt->tickcnt, sdcnt->tx_ctlerrs,
+                        sdcnt->tx_ctlpkts, sdcnt->rx_ctlerrs,
+                        sdcnt->rx_ctlpkts, sdcnt->rx_readahead_cnt);
+
+        return simple_read_from_buffer(data, count, ppos, buf, res);
+}
+
+static const struct file_operations brcmf_debugfs_sdio_counter_ops = {
+        .owner = THIS_MODULE,
+        .open = simple_open,
+        .read = brcmf_debugfs_sdio_counter_read
+};
+
+void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
+                                     struct brcmf_sdio_count *sdcnt)
+{
+        struct dentry *dentry = drvr->dbgfs_dir;
+
+        if (!IS_ERR_OR_NULL(dentry))
+                debugfs_create_file("counters", S_IRUGO, dentry,
+                                    sdcnt, &brcmf_debugfs_sdio_counter_ops);
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
index a2c4576cf9ff..b784920532d3 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
@@ -76,4 +76,63 @@ do {									\
 
 extern int brcmf_msg_level;
 
+/*
+ * hold counter variables used in brcmfmac sdio driver.
+ */
+struct brcmf_sdio_count {
+        uint intrcount;         /* Count of device interrupt callbacks */
+        uint lastintrs;         /* Count as of last watchdog timer */
+        uint pollcnt;           /* Count of active polls */
+        uint regfails;          /* Count of R_REG failures */
+        uint tx_sderrs;         /* Count of tx attempts with sd errors */
+        uint fcqueued;          /* Tx packets that got queued */
+        uint rxrtx;             /* Count of rtx requests (NAK to dongle) */
+        uint rx_toolong;        /* Receive frames too long to receive */
+        uint rxc_errors;        /* SDIO errors when reading control frames */
+        uint rx_hdrfail;        /* SDIO errors on header reads */
+        uint rx_badhdr;         /* Bad received headers (roosync?) */
+        uint rx_badseq;         /* Mismatched rx sequence number */
+        uint fc_rcvd;           /* Number of flow-control events received */
+        uint fc_xoff;           /* Number which turned on flow-control */
+        uint fc_xon;            /* Number which turned off flow-control */
+        uint rxglomfail;        /* Failed deglom attempts */
+        uint rxglomframes;      /* Number of glom frames (superframes) */
+        uint rxglompkts;        /* Number of packets from glom frames */
+        uint f2rxhdrs;          /* Number of header reads */
+        uint f2rxdata;          /* Number of frame data reads */
+        uint f2txdata;          /* Number of f2 frame writes */
+        uint f1regdata;         /* Number of f1 register accesses */
+        uint tickcnt;           /* Number of watchdog been schedule */
+        ulong tx_ctlerrs;       /* Err of sending ctrl frames */
+        ulong tx_ctlpkts;       /* Ctrl frames sent to dongle */
+        ulong rx_ctlerrs;       /* Err of processing rx ctrl frames */
+        ulong rx_ctlpkts;       /* Ctrl frames processed from dongle */
+        ulong rx_readahead_cnt; /* packets where header read-ahead was used */
+};
+
+struct brcmf_pub;
+#ifdef DEBUG
+void brcmf_debugfs_init(void);
+void brcmf_debugfs_exit(void);
+int brcmf_debugfs_attach(struct brcmf_pub *drvr);
+void brcmf_debugfs_detach(struct brcmf_pub *drvr);
+struct dentry *brcmf_debugfs_get_devdir(struct brcmf_pub *drvr);
+void brcmf_debugfs_create_sdio_count(struct brcmf_pub *drvr,
+                                     struct brcmf_sdio_count *sdcnt);
+#else
+static inline void brcmf_debugfs_init(void)
+{
+}
+static inline void brcmf_debugfs_exit(void)
+{
+}
+static inline int brcmf_debugfs_attach(struct brcmf_pub *drvr)
+{
+        return 0;
+}
+static inline void brcmf_debugfs_detach(struct brcmf_pub *drvr)
+{
+}
+#endif
+
 #endif                          /* _BRCMF_DBG_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index 8933f9b31a9a..01cf6c03390b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -1007,6 +1007,9 @@ int brcmf_attach(uint bus_hdrlen, struct device *dev)
         drvr->bus_if->drvr = drvr;
         drvr->dev = dev;
 
+        /* create device debugfs folder */
+        brcmf_debugfs_attach(drvr);
+
         /* Attach and link in the protocol */
         ret = brcmf_proto_attach(drvr);
         if (ret != 0) {
@@ -1123,6 +1126,7 @@ void brcmf_detach(struct device *dev)
                 brcmf_proto_detach(drvr);
         }
 
+        brcmf_debugfs_detach(drvr);
         bus_if->drvr = NULL;
         kfree(drvr);
 }
@@ -1192,6 +1196,8 @@ exit:
 
 static void brcmf_driver_init(struct work_struct *work)
 {
+        brcmf_debugfs_init();
+
 #ifdef CONFIG_BRCMFMAC_SDIO
         brcmf_sdio_init();
 #endif
@@ -1219,6 +1225,7 @@ static void __exit brcmfmac_module_exit(void)
 #ifdef CONFIG_BRCMFMAC_USB
         brcmf_usb_exit();
 #endif
+        brcmf_debugfs_exit();
 }
 
 module_init(brcmfmac_module_init);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 4deae28fc211..b023766954a6 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -502,12 +502,9 @@ struct brcmf_sdio {
         bool intr;              /* Use interrupts */
         bool poll;              /* Use polling */
         bool ipend;             /* Device interrupt is pending */
-        uint intrcount;         /* Count of device interrupt callbacks */
-        uint lastintrs;         /* Count as of last watchdog timer */
         uint spurious;          /* Count of spurious interrupts */
         uint pollrate;          /* Ticks between device polls */
         uint polltick;          /* Tick counter */
-        uint pollcnt;           /* Count of active polls */
 
 #ifdef DEBUG
         uint console_interval;
@@ -515,8 +512,6 @@ struct brcmf_sdio {
         uint console_addr;      /* Console address from shared struct */
 #endif                          /* DEBUG */
 
-        uint regfails;          /* Count of R_REG failures */
-
         uint clkstate;          /* State of sd and backplane clock(s) */
         bool activity;          /* Activity flag for clock down */
         s32 idletime;           /* Control for activity timeout */
@@ -531,33 +526,6 @@ struct brcmf_sdio {
 /* Field to decide if rx of control frames happen in rxbuf or lb-pool */
         bool usebufpool;
 
-        /* Some additional counters */
-        uint tx_sderrs;         /* Count of tx attempts with sd errors */
-        uint fcqueued;          /* Tx packets that got queued */
-        uint rxrtx;             /* Count of rtx requests (NAK to dongle) */
-        uint rx_toolong;        /* Receive frames too long to receive */
-        uint rxc_errors;        /* SDIO errors when reading control frames */
-        uint rx_hdrfail;        /* SDIO errors on header reads */
-        uint rx_badhdr;         /* Bad received headers (roosync?) */
-        uint rx_badseq;         /* Mismatched rx sequence number */
-        uint fc_rcvd;           /* Number of flow-control events received */
-        uint fc_xoff;           /* Number which turned on flow-control */
-        uint fc_xon;            /* Number which turned off flow-control */
-        uint rxglomfail;        /* Failed deglom attempts */
-        uint rxglomframes;      /* Number of glom frames (superframes) */
-        uint rxglompkts;        /* Number of packets from glom frames */
-        uint f2rxhdrs;          /* Number of header reads */
-        uint f2rxdata;          /* Number of frame data reads */
-        uint f2txdata;          /* Number of f2 frame writes */
-        uint f1regdata;         /* Number of f1 register accesses */
-        uint tickcnt;           /* Number of watchdog been schedule */
-        unsigned long tx_ctlerrs;       /* Err of sending ctrl frames */
-        unsigned long tx_ctlpkts;       /* Ctrl frames sent to dongle */
-        unsigned long rx_ctlerrs;       /* Err of processing rx ctrl frames */
-        unsigned long rx_ctlpkts;       /* Ctrl frames processed from dongle */
-        unsigned long rx_readahead_cnt; /* Number of packets where header
-                                         * read-ahead was used. */
-
         u8 *ctrl_frame_buf;
         u32 ctrl_frame_len;
         bool ctrl_frame_stat;
@@ -583,6 +551,7 @@ struct brcmf_sdio {
         u32 fw_ptr;
 
         bool txoff;             /* Transmit flow-controlled */
+        struct brcmf_sdio_count sdcnt;
 };
 
 /* clkstate */
@@ -945,7 +914,7 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
         if (ret == 0)
                 w_sdreg32(bus, SMB_INT_ACK,
                           offsetof(struct sdpcmd_regs, tosbmailbox));
-        bus->f1regdata += 2;
+        bus->sdcnt.f1regdata += 2;
 
         /* Dongle recomposed rx frames, accept them again */
         if (hmb_data & HMB_DATA_NAKHANDLED) {
@@ -984,12 +953,12 @@ static u32 brcmf_sdbrcm_hostmail(struct brcmf_sdio *bus)
                                                         HMB_DATA_FCDATA_SHIFT;
 
                 if (fcbits & ~bus->flowcontrol)
-                        bus->fc_xoff++;
+                        bus->sdcnt.fc_xoff++;
 
                 if (bus->flowcontrol & ~fcbits)
-                        bus->fc_xon++;
+                        bus->sdcnt.fc_xon++;
 
-                bus->fc_rcvd++;
+                bus->sdcnt.fc_rcvd++;
                 bus->flowcontrol = fcbits;
         }
 
@@ -1021,7 +990,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
 
         brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
                          SFC_RF_TERM, &err);
-        bus->f1regdata++;
+        bus->sdcnt.f1regdata++;
 
         /* Wait until the packet has been flushed (device/FIFO stable) */
         for (lastrbc = retries = 0xffff; retries > 0; retries--) {
@@ -1029,7 +998,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
                                       SBSDIO_FUNC1_RFRAMEBCHI, &err);
                 lo = brcmf_sdio_regrb(bus->sdiodev,
                                       SBSDIO_FUNC1_RFRAMEBCLO, &err);
-                bus->f1regdata += 2;
+                bus->sdcnt.f1regdata += 2;
 
                 if ((hi == 0) && (lo == 0))
                         break;
@@ -1047,11 +1016,11 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
                 brcmf_dbg(INFO, "flush took %d iterations\n", 0xffff - retries);
 
         if (rtx) {
-                bus->rxrtx++;
+                bus->sdcnt.rxrtx++;
                 err = w_sdreg32(bus, SMB_NAK,
                                 offsetof(struct sdpcmd_regs, tosbmailbox));
 
-                bus->f1regdata++;
+                bus->sdcnt.f1regdata++;
                 if (err == 0)
                         bus->rxskip = true;
         }
@@ -1243,7 +1212,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                                   dlen);
                         errcode = -1;
                 }
-                bus->f2rxdata++;
+                bus->sdcnt.f2rxdata++;
 
                 /* On failure, kill the superframe, allow a couple retries */
                 if (errcode < 0) {
@@ -1256,7 +1225,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                         } else {
                                 bus->glomerr = 0;
                                 brcmf_sdbrcm_rxfail(bus, true, false);
-                                bus->rxglomfail++;
+                                bus->sdcnt.rxglomfail++;
                                 brcmf_sdbrcm_free_glom(bus);
                         }
                         return 0;
@@ -1312,7 +1281,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                 if (rxseq != seq) {
                         brcmf_dbg(INFO, "(superframe) rx_seq %d, expected %d\n",
                                   seq, rxseq);
-                        bus->rx_badseq++;
+                        bus->sdcnt.rx_badseq++;
                         rxseq = seq;
                 }
 
@@ -1376,7 +1345,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                         } else {
                                 bus->glomerr = 0;
                                 brcmf_sdbrcm_rxfail(bus, true, false);
-                                bus->rxglomfail++;
+                                bus->sdcnt.rxglomfail++;
                                 brcmf_sdbrcm_free_glom(bus);
                         }
                         bus->nextlen = 0;
@@ -1402,7 +1371,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                         if (rxseq != seq) {
                                 brcmf_dbg(GLOM, "rx_seq %d, expected %d\n",
                                           seq, rxseq);
-                                bus->rx_badseq++;
+                                bus->sdcnt.rx_badseq++;
                                 rxseq = seq;
                         }
                         rxseq++;
@@ -1441,8 +1410,8 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                         down(&bus->sdsem);
                 }
 
-                bus->rxglomframes++;
-                bus->rxglompkts += bus->glom.qlen;
+                bus->sdcnt.rxglomframes++;
+                bus->sdcnt.rxglompkts += bus->glom.qlen;
         }
         return num;
 }
@@ -1526,7 +1495,7 @@ brcmf_sdbrcm_read_control(struct brcmf_sdio *bus, u8 *hdr, uint len, uint doff)
                 brcmf_dbg(ERROR, "%d-byte ctl frame (%d-byte ctl data) exceeds %d-byte limit\n",
                           len, len - doff, bus->sdiodev->bus_if->maxctl);
                 bus->sdiodev->bus_if->dstats.rx_errors++;
-                bus->rx_toolong++;
+                bus->sdcnt.rx_toolong++;
                 brcmf_sdbrcm_rxfail(bus, false, false);
                 goto done;
         }
@@ -1536,13 +1505,13 @@ brcmf_sdbrcm_read_control(struct brcmf_sdio *bus, u8 *hdr, uint len, uint doff)
                                 bus->sdiodev->sbwad,
                                 SDIO_FUNC_2,
                                 F2SYNC, (bus->rxctl + BRCMF_FIRSTREAD), rdlen);
-        bus->f2rxdata++;
+        bus->sdcnt.f2rxdata++;
 
         /* Control frame failures need retransmission */
         if (sdret < 0) {
                 brcmf_dbg(ERROR, "read %d control bytes failed: %d\n",
                           rdlen, sdret);
-                bus->rxc_errors++;
+                bus->sdcnt.rxc_errors++;
                 brcmf_sdbrcm_rxfail(bus, true, true);
                 goto done;
         }
@@ -1589,7 +1558,7 @@ brcmf_alloc_pkt_and_read(struct brcmf_sdio *bus, u16 rdlen,
         /* Read the entire frame */
         sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
                                       SDIO_FUNC_2, F2SYNC, *pkt);
-        bus->f2rxdata++;
+        bus->sdcnt.f2rxdata++;
 
         if (sdret < 0) {
                 brcmf_dbg(ERROR, "(nextlen): read %d bytes failed: %d\n",
@@ -1630,7 +1599,7 @@ brcmf_check_rxbuf(struct brcmf_sdio *bus, struct sk_buff *pkt, u8 *rxbuf,
         if ((u16)~(*len ^ check)) {
                 brcmf_dbg(ERROR, "(nextlen): HW hdr error: nextlen/len/check 0x%04x/0x%04x/0x%04x\n",
                           nextlen, *len, check);
-                bus->rx_badhdr++;
+                bus->sdcnt.rx_badhdr++;
                 brcmf_sdbrcm_rxfail(bus, false, false);
                 goto fail;
         }
@@ -1746,7 +1715,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                                 bus->nextlen = 0;
                         }
 
-                        bus->rx_readahead_cnt++;
+                        bus->sdcnt.rx_readahead_cnt++;
 
                         /* Handle Flow Control */
                         fcbits = SDPCM_FCMASK_VALUE(
@@ -1754,12 +1723,12 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 
                         if (bus->flowcontrol != fcbits) {
                                 if (~bus->flowcontrol & fcbits)
-                                        bus->fc_xoff++;
+                                        bus->sdcnt.fc_xoff++;
 
                                 if (bus->flowcontrol & ~fcbits)
-                                        bus->fc_xon++;
+                                        bus->sdcnt.fc_xon++;
 
-                                bus->fc_rcvd++;
+                                bus->sdcnt.fc_rcvd++;
                                 bus->flowcontrol = fcbits;
                         }
 
@@ -1767,7 +1736,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                         if (rxseq != seq) {
                                 brcmf_dbg(INFO, "(nextlen): rx_seq %d, expected %d\n",
                                           seq, rxseq);
-                                bus->rx_badseq++;
+                                bus->sdcnt.rx_badseq++;
                                 rxseq = seq;
                         }
 
@@ -1814,11 +1783,11 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                 sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
                                               SDIO_FUNC_2, F2SYNC, bus->rxhdr,
                                               BRCMF_FIRSTREAD);
-                bus->f2rxhdrs++;
+                bus->sdcnt.f2rxhdrs++;
 
                 if (sdret < 0) {
                         brcmf_dbg(ERROR, "RXHEADER FAILED: %d\n", sdret);
-                        bus->rx_hdrfail++;
+                        bus->sdcnt.rx_hdrfail++;
                         brcmf_sdbrcm_rxfail(bus, true, true);
                         continue;
                 }
@@ -1840,7 +1809,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                 if ((u16) ~(len ^ check)) {
                         brcmf_dbg(ERROR, "HW hdr err: len/check 0x%04x/0x%04x\n",
                                   len, check);
-                        bus->rx_badhdr++;
+                        bus->sdcnt.rx_badhdr++;
                         brcmf_sdbrcm_rxfail(bus, false, false);
                         continue;
                 }
@@ -1861,7 +1830,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                 if ((doff < SDPCM_HDRLEN) || (doff > len)) {
                         brcmf_dbg(ERROR, "Bad data offset %d: HW len %d, min %d seq %d\n",
                                   doff, len, SDPCM_HDRLEN, seq);
-                        bus->rx_badhdr++;
+                        bus->sdcnt.rx_badhdr++;
                         brcmf_sdbrcm_rxfail(bus, false, false);
                         continue;
                 }
@@ -1880,19 +1849,19 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
 
                 if (bus->flowcontrol != fcbits) {
                         if (~bus->flowcontrol & fcbits)
-                                bus->fc_xoff++;
+                                bus->sdcnt.fc_xoff++;
 
                         if (bus->flowcontrol & ~fcbits)
-                                bus->fc_xon++;
+                                bus->sdcnt.fc_xon++;
 
-                        bus->fc_rcvd++;
+                        bus->sdcnt.fc_rcvd++;
                         bus->flowcontrol = fcbits;
                 }
 
                 /* Check and update sequence number */
                 if (rxseq != seq) {
                         brcmf_dbg(INFO, "rx_seq %d, expected %d\n", seq, rxseq);
-                        bus->rx_badseq++;
+                        bus->sdcnt.rx_badseq++;
                         rxseq = seq;
                 }
 
@@ -1937,7 +1906,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                         brcmf_dbg(ERROR, "too long: len %d rdlen %d\n",
                                   len, rdlen);
                         bus->sdiodev->bus_if->dstats.rx_errors++;
-                        bus->rx_toolong++;
+                        bus->sdcnt.rx_toolong++;
                         brcmf_sdbrcm_rxfail(bus, false, false);
                         continue;
                 }
@@ -1960,7 +1929,7 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio *bus, uint maxframes, bool *finished)
                 /* Read the remaining frame data */
                 sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
                                               SDIO_FUNC_2, F2SYNC, pkt);
-                bus->f2rxdata++;
+                bus->sdcnt.f2rxdata++;
 
                 if (sdret < 0) {
                         brcmf_dbg(ERROR, "read %d %s bytes failed: %d\n", rdlen,
@@ -2147,18 +2116,18 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
 
         ret = brcmf_sdcard_send_pkt(bus->sdiodev, bus->sdiodev->sbwad,
                                     SDIO_FUNC_2, F2SYNC, pkt);
-        bus->f2txdata++;
+        bus->sdcnt.f2txdata++;
 
         if (ret < 0) {
                 /* On failure, abort the command and terminate the frame */
                 brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
                           ret);
-                bus->tx_sderrs++;
+                bus->sdcnt.tx_sderrs++;
 
                 brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
                 brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
                                  SFC_WF_TERM, NULL);
-                bus->f1regdata++;
+                bus->sdcnt.f1regdata++;
 
                 for (i = 0; i < 3; i++) {
                         u8 hi, lo;
@@ -2166,7 +2135,7 @@ static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
                                               SBSDIO_FUNC1_WFRAMEBCHI, NULL);
                         lo = brcmf_sdio_regrb(bus->sdiodev,
                                               SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-                        bus->f1regdata += 2;
+                        bus->sdcnt.f1regdata += 2;
                         if ((hi == 0) && (lo == 0))
                                 break;
                 }
@@ -2224,7 +2193,7 @@ static uint brcmf_sdbrcm_sendfromq(struct brcmf_sdio *bus, uint maxframes)
                         ret = r_sdreg32(bus, &intstatus,
                                         offsetof(struct sdpcmd_regs,
                                                  intstatus));
-                        bus->f2txdata++;
+                        bus->sdcnt.f2txdata++;
                         if (ret != 0)
                                 break;
                         if (intstatus & bus->hostintmask)
@@ -2417,7 +2386,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
                 bus->ipend = false;
                 err = r_sdreg32(bus, &newstatus,
                                 offsetof(struct sdpcmd_regs, intstatus));
-                bus->f1regdata++;
+                bus->sdcnt.f1regdata++;
                 if (err != 0)
                         newstatus = 0;
                 newstatus &= bus->hostintmask;
@@ -2426,7 +2395,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
                         err = w_sdreg32(bus, newstatus,
                                         offsetof(struct sdpcmd_regs,
                                                  intstatus));
-                        bus->f1regdata++;
+                        bus->sdcnt.f1regdata++;
                 }
         }
 
@@ -2445,7 +2414,7 @@ static bool brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
 
                 err = r_sdreg32(bus, &newstatus,
                                 offsetof(struct sdpcmd_regs, intstatus));
-                bus->f1regdata += 2;
+                bus->sdcnt.f1regdata += 2;
                 bus->fcstate =
                     !!(newstatus & (I_HMB_FC_STATE | I_HMB_FC_CHANGE));
                 intstatus |= (newstatus & bus->hostintmask);
@@ -2510,13 +2479,13 @@ clkwait:
                                 terminate the frame */
                         brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
                                   ret);
-                        bus->tx_sderrs++;
+                        bus->sdcnt.tx_sderrs++;
 
                         brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
                         brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
                                          SFC_WF_TERM, &err);
-                        bus->f1regdata++;
+                        bus->sdcnt.f1regdata++;
 
                         for (i = 0; i < 3; i++) {
                                 u8 hi, lo;
@@ -2526,7 +2495,7 @@ clkwait:
                                 lo = brcmf_sdio_regrb(bus->sdiodev,
                                                       SBSDIO_FUNC1_WFRAMEBCLO,
                                                       &err);
-                                bus->f1regdata += 2;
+                                bus->sdcnt.f1regdata += 2;
                                 if ((hi == 0) && (lo == 0))
                                         break;
                         }
@@ -2657,7 +2626,7 @@ static int brcmf_sdbrcm_bus_txdata(struct device *dev, struct sk_buff *pkt)
         /* Check for existing queue, current flow-control,
                          pending event, or pending clock */
         brcmf_dbg(TRACE, "deferring pktq len %d\n", pktq_len(&bus->txq));
-        bus->fcqueued++;
+        bus->sdcnt.fcqueued++;
 
         /* Priority based enq */
         spin_lock_bh(&bus->txqlock);
@@ -2845,13 +2814,13 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
                 /* On failure, abort the command and terminate the frame */
                 brcmf_dbg(INFO, "sdio error %d, abort command and terminate frame\n",
                           ret);
-                bus->tx_sderrs++;
+                bus->sdcnt.tx_sderrs++;
 
                 brcmf_sdcard_abort(bus->sdiodev, SDIO_FUNC_2);
 
                 brcmf_sdio_regwb(bus->sdiodev, SBSDIO_FUNC1_FRAMECTRL,
                                  SFC_WF_TERM, NULL);
-                bus->f1regdata++;
+                bus->sdcnt.f1regdata++;
 
                 for (i = 0; i < 3; i++) {
                         u8 hi, lo;
@@ -2859,7 +2828,7 @@ static int brcmf_tx_frame(struct brcmf_sdio *bus, u8 *frame, u16 len)
                                               SBSDIO_FUNC1_WFRAMEBCHI, NULL);
                         lo = brcmf_sdio_regrb(bus->sdiodev,
                                               SBSDIO_FUNC1_WFRAMEBCLO, NULL);
-                        bus->f1regdata += 2;
+                        bus->sdcnt.f1regdata += 2;
                         if (hi == 0 && lo == 0)
                                 break;
                 }
@@ -2976,13 +2945,26 @@ brcmf_sdbrcm_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
         up(&bus->sdsem);
 
         if (ret)
-                bus->tx_ctlerrs++;
+                bus->sdcnt.tx_ctlerrs++;
         else
-                bus->tx_ctlpkts++;
+                bus->sdcnt.tx_ctlpkts++;
 
         return ret ? -EIO : 0;
 }
 
+#ifdef DEBUG
+static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
+{
+        struct brcmf_pub *drvr = bus->sdiodev->bus_if->drvr;
+
+        brcmf_debugfs_create_sdio_count(drvr, &bus->sdcnt);
+}
+#else
+static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
+{
+}
+#endif /* DEBUG */
+
 static int
 brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
 {
@@ -3017,9 +2999,9 @@ brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
         }
 
         if (rxlen)
-                bus->rx_ctlpkts++;
+                bus->sdcnt.rx_ctlpkts++;
         else
-                bus->rx_ctlerrs++;
+                bus->sdcnt.rx_ctlerrs++;
 
         return rxlen ? (int)rxlen : -ETIMEDOUT;
 }
@@ -3419,7 +3401,7 @@ static int brcmf_sdbrcm_bus_init(struct device *dev)
                 return 0;
 
         /* Start the watchdog timer */
-        bus->tickcnt = 0;
+        bus->sdcnt.tickcnt = 0;
         brcmf_sdbrcm_wd_timer(bus, BRCMF_WD_POLL_MS);
 
         down(&bus->sdsem);
@@ -3512,7 +3494,7 @@ void brcmf_sdbrcm_isr(void *arg)
                 return;
         }
         /* Count the interrupt call */
-        bus->intrcount++;
+        bus->sdcnt.intrcount++;
         bus->ipend = true;
 
         /* Shouldn't get this interrupt if we're sleeping? */
@@ -3554,7 +3536,8 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
                 bus->polltick = 0;
 
                 /* Check device if no interrupts */
-                if (!bus->intr || (bus->intrcount == bus->lastintrs)) {
+                if (!bus->intr ||
+                    (bus->sdcnt.intrcount == bus->sdcnt.lastintrs)) {
 
                         if (!bus->dpc_sched) {
                                 u8 devpend;
@@ -3569,7 +3552,7 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
                         /* If there is something, make like the ISR and
                                  schedule the DPC */
                         if (intstatus) {
-                                bus->pollcnt++;
+                                bus->sdcnt.pollcnt++;
                                 bus->ipend = true;
 
                                 bus->dpc_sched = true;
@@ -3581,7 +3564,7 @@ static bool brcmf_sdbrcm_bus_watchdog(struct brcmf_sdio *bus)
                 }
 
                 /* Update interrupt tracking */
-                bus->lastintrs = bus->intrcount;
+                bus->sdcnt.lastintrs = bus->sdcnt.intrcount;
         }
 #ifdef DEBUG
         /* Poll for console output periodically */
@@ -3793,7 +3776,7 @@ brcmf_sdbrcm_watchdog_thread(void *data)
                 if (!wait_for_completion_interruptible(&bus->watchdog_wait)) {
                         brcmf_sdbrcm_bus_watchdog(bus);
                         /* Count the tick for reference */
-                        bus->tickcnt++;
+                        bus->sdcnt.tickcnt++;
                 } else
                         break;
         }
@@ -3834,7 +3817,6 @@ static void brcmf_sdbrcm_release_dongle(struct brcmf_sdio *bus)
 static void brcmf_sdbrcm_release(struct brcmf_sdio *bus)
 {
         brcmf_dbg(TRACE, "Enter\n");
-
         if (bus) {
                 /* De-register interrupt handler */
                 brcmf_sdio_intr_unregister(bus->sdiodev);
@@ -3938,6 +3920,7 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
                 goto fail;
         }
 
+        brcmf_sdio_debugfs_create(bus);
         brcmf_dbg(INFO, "completed!!\n");
 
         /* if firmware path present try to download and bring up bus */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c b/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
index 95b5902bc4b3..01b190a25d94 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
@@ -735,10 +735,8 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
                  * a candidate for aggregation
                  */
                 p = pktq_ppeek(&qi->q, prec);
-                /* tx_info must be checked with current p */
-                tx_info = IEEE80211_SKB_CB(p);
-
                 if (p) {
+                        tx_info = IEEE80211_SKB_CB(p);
                         if ((tx_info->flags & IEEE80211_TX_CTL_AMPDU) &&
                             ((u8) (p->priority) == tid)) {
                                 plen = p->len + AMPDU_MAX_MPDU_OVERHEAD;
@@ -759,6 +757,7 @@ brcms_c_sendampdu(struct ampdu_info *ampdu, struct brcms_txq_info *qi,
                                         p = NULL;
                                         continue;
                                 }
+                                /* next packet fit for aggregation so dequeue */
                                 p = brcmu_pktq_pdeq(&qi->q, prec);
                         } else {
                                 p = NULL;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 50f92a0b7c41..341e06a0d6ec 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -721,14 +721,6 @@ static const struct ieee80211_ops brcms_ops = {
         .flush = brcms_ops_flush,
 };
 
-/*
- * is called in brcms_bcma_probe() context, therefore no locking required.
- */
-static int brcms_set_hint(struct brcms_info *wl, char *abbrev)
-{
-        return regulatory_hint(wl->pub->ieee_hw->wiphy, abbrev);
-}
-
 void brcms_dpc(unsigned long data)
 {
         struct brcms_info *wl;
@@ -1068,9 +1060,9 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
                 wiphy_err(wl->wiphy, "%s: ieee80211_register_hw failed, status"
                           "%d\n", __func__, err);
 
-        if (wl->pub->srom_ccode[0] && brcms_set_hint(wl, wl->pub->srom_ccode))
-                wiphy_err(wl->wiphy, "%s: regulatory_hint failed, status %d\n",
-                          __func__, err);
+        if (wl->pub->srom_ccode[0] &&
+            regulatory_hint(wl->wiphy, wl->pub->srom_ccode))
+                wiphy_err(wl->wiphy, "%s: regulatory hint failed\n", __func__);
 
         n_adapters_found++;
         return wl;
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 98c8f6449649..170ec330d2a9 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,7 +1,3 @@
-obj-$(CONFIG_IWLDVM)    += dvm/
-
-CFLAGS_iwl-devtrace.o := -I$(src)
-
 # common
 obj-$(CONFIG_IWLWIFI)   += iwlwifi.o
 iwlwifi-objs            += iwl-io.o
@@ -13,5 +9,11 @@ iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
 iwlwifi-objs            += pcie/1000.o pcie/2000.o pcie/5000.o pcie/6000.o
 
 iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TRACING) += iwl-devtrace.o
+iwlwifi-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-test.o
 
 ccflags-y += -D__CHECK_ENDIAN__ -I$(src)
+
+
+obj-$(CONFIG_IWLDVM)    += dvm/
+
+CFLAGS_iwl-devtrace.o := -I$(src)
diff --git a/drivers/net/wireless/iwlwifi/dvm/agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 2ae3608472a6..6d102413dd94 100644
--- a/drivers/net/wireless/iwlwifi/dvm/agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -395,8 +395,10 @@ static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u32 flags)
 }
 
 extern int iwl_alive_start(struct iwl_priv *priv);
-/* svtool */
+
+/* testmode support */
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
+
 extern int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data,
                                    int len);
 extern int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw,
@@ -404,13 +406,16 @@ extern int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw,
                                     struct netlink_callback *cb,
                                     void *data, int len);
 extern void iwl_testmode_init(struct iwl_priv *priv);
-extern void iwl_testmode_cleanup(struct iwl_priv *priv);
+extern void iwl_testmode_free(struct iwl_priv *priv);
+
 #else
+
 static inline
 int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
 {
         return -ENOSYS;
 }
+
 static inline
 int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
                       struct netlink_callback *cb,
@@ -418,12 +423,12 @@ int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
 {
         return -ENOSYS;
 }
-static inline
-void iwl_testmode_init(struct iwl_priv *priv)
+
+static inline void iwl_testmode_init(struct iwl_priv *priv)
 {
 }
-static inline
-void iwl_testmode_cleanup(struct iwl_priv *priv)
+
+static inline void iwl_testmode_free(struct iwl_priv *priv)
 {
 }
 #endif
diff --git a/drivers/net/wireless/iwlwifi/dvm/dev.h b/drivers/net/wireless/iwlwifi/dvm/dev.h
index 89f2e1040e7f..4620b657948a 100644
--- a/drivers/net/wireless/iwlwifi/dvm/dev.h
+++ b/drivers/net/wireless/iwlwifi/dvm/dev.h
@@ -52,6 +52,8 @@
 #include "rs.h"
 #include "tt.h"
 
+#include "iwl-test.h"
+
 /* CT-KILL constants */
 #define CT_KILL_THRESHOLD_LEGACY   110 /* in Celsius */
 #define CT_KILL_THRESHOLD          114 /* in Celsius */
@@ -596,24 +598,6 @@ struct iwl_lib_ops {
         void (*temperature)(struct iwl_priv *priv);
 };
 
-#ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
-struct iwl_testmode_trace {
-        u32 buff_size;
-        u32 total_size;
-        u32 num_chunks;
-        u8 *cpu_addr;
-        u8 *trace_addr;
-        dma_addr_t dma_addr;
-        bool trace_enabled;
-};
-struct iwl_testmode_mem {
-        u32 buff_size;
-        u32 num_chunks;
-        u8 *buff_addr;
-        bool read_in_progress;
-};
-#endif
-
 struct iwl_wipan_noa_data {
         struct rcu_head rcu_head;
         u32 length;
@@ -670,8 +654,6 @@ struct iwl_priv {
         enum ieee80211_band band;
         u8 valid_contexts;
 
-        void (*pre_rx_handler)(struct iwl_priv *priv,
-                               struct iwl_rx_cmd_buffer *rxb);
         int (*rx_handlers[REPLY_MAX])(struct iwl_priv *priv,
                                        struct iwl_rx_cmd_buffer *rxb,
                                        struct iwl_device_cmd *cmd);
@@ -895,9 +877,9 @@ struct iwl_priv {
         struct led_classdev led;
         unsigned long blink_on, blink_off;
         bool led_registered;
+
 #ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
-        struct iwl_testmode_trace testmode_trace;
-        struct iwl_testmode_mem testmode_mem;
+        struct iwl_test tst;
         u32 tm_fixed_rate;
 #endif
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index cb1ca7a25dd5..76f259283c3a 100644
--- a/drivers/net/wireless/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -1265,7 +1265,7 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
          * the mutex, this ensures we don't try to send two
          * (or more) synchronous commands at a time.
          */
-        if (cmd->flags & CMD_SYNC)
+        if (!(cmd->flags & CMD_ASYNC))
                 lockdep_assert_held(&priv->mutex);
 
         if (priv->ucode_owner == IWL_OWNERSHIP_TM &&
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index 599e8b41f5a8..9d2374862314 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -476,7 +476,7 @@ static int iwlagn_mac_resume(struct ieee80211_hw *hw)
                         }
 
                         if (priv->wowlan_sram)
-                                _iwl_read_targ_mem_words(
+                                _iwl_read_targ_mem_dwords(
                                       priv->trans, 0x800000,
                                       priv->wowlan_sram,
                                       img->sec[IWL_UCODE_SECTION_DATA].len / 4);
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index 1c2d0233a405..e620af3d592d 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -406,7 +406,7 @@ static void iwl_continuous_event_trace(struct iwl_priv *priv)
 
         base = priv->device_pointers.log_event_table;
         if (iwlagn_hw_valid_rtc_data_addr(base)) {
-                iwl_read_targ_mem_words(priv->trans, base, &read, sizeof(read));
+                iwl_read_targ_mem_bytes(priv->trans, base, &read, sizeof(read));
                 capacity = read.capacity;
                 mode = read.mode;
                 num_wraps = read.wrap_counter;
@@ -1548,7 +1548,7 @@ static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 
         iwl_dbgfs_unregister(priv);
 
-        iwl_testmode_cleanup(priv);
+        iwl_testmode_free(priv);
         iwlagn_mac_unregister(priv);
 
         iwl_tt_exit(priv);
@@ -1671,7 +1671,7 @@ static void iwl_dump_nic_error_log(struct iwl_priv *priv)
         }
 
         /*TODO: Update dbgfs with ISR error stats obtained below */
-        iwl_read_targ_mem_words(trans, base, &table, sizeof(table));
+        iwl_read_targ_mem_bytes(trans, base, &table, sizeof(table));
 
         if (ERROR_START_OFFSET <= table.valid * ERROR_ELEM_SIZE) {
                 IWL_ERR(trans, "Start IWL Error Log Dump:\n");
diff --git a/drivers/net/wireless/iwlwifi/dvm/rx.c b/drivers/net/wireless/iwlwifi/dvm/rx.c
index 0ed90bb8b56a..c1f7a18e08dd 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rx.c
@@ -1124,8 +1124,6 @@ int iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
 {
         struct iwl_rx_packet *pkt = rxb_addr(rxb);
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
-        void (*pre_rx_handler)(struct iwl_priv *,
-                               struct iwl_rx_cmd_buffer *);
         int err = 0;
 
         /*
@@ -1135,19 +1133,19 @@ int iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct iwl_rx_cmd_buffer *rxb,
          */
         iwl_notification_wait_notify(&priv->notif_wait, pkt);
 
-        /* RX data may be forwarded to userspace (using pre_rx_handler) in one
-         * of two cases: the first, that the user owns the uCode through
-         * testmode - in such case the pre_rx_handler is set and no further
-         * processing takes place. The other case is when the user want to
-         * monitor the rx w/o affecting the regular flow - the pre_rx_handler
-         * will be set but the ownership flag != IWL_OWNERSHIP_TM and the flow
+#ifdef CONFIG_IWLWIFI_DEVICE_TESTMODE
+        /*
+         * RX data may be forwarded to userspace in one
+         * of two cases: the user owns the fw through testmode or when
+         * the user requested to monitor the rx w/o affecting the regular flow.
+         * In these cases the iwl_test object will handle forwarding the rx
+         * data to user space.
+         * Note that if the ownership flag != IWL_OWNERSHIP_TM the flow
          * continues.
-         * We need to use ACCESS_ONCE to prevent a case where the handler
-         * changes between the check and the call.
          */
-        pre_rx_handler = ACCESS_ONCE(priv->pre_rx_handler);
-        if (pre_rx_handler)
-                pre_rx_handler(priv, rxb);
+        iwl_test_rx(&priv->tst, rxb);
+#endif
+
         if (priv->ucode_owner != IWL_OWNERSHIP_TM) {
                 /* Based on type of command response or notification,
                  *   handle those that need handling via function in
diff --git a/drivers/net/wireless/iwlwifi/dvm/testmode.c b/drivers/net/wireless/iwlwifi/dvm/testmode.c
index e08b1a383594..57b918ce3b5f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/testmode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/testmode.c
@@ -60,6 +60,7 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  *****************************************************************************/
+
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -69,354 +70,83 @@
 #include <net/cfg80211.h>
 #include <net/mac80211.h>
 #include <net/netlink.h>
+
 #include "iwl-debug.h"
-#include "iwl-io.h"
 #include "iwl-trans.h"
-#include "iwl-fh.h"
-#include "iwl-prph.h"
 #include "dev.h"
 #include "agn.h"
-#include "testmode.h"
-
-
-/* Periphery registers absolute lower bound. This is used in order to
- * differentiate registery access through HBUS_TARG_PRPH_* and
- * HBUS_TARG_MEM_* accesses.
- */
-#define IWL_TM_ABS_PRPH_START (0xA00000)
-
-/* The TLVs used in the gnl message policy between the kernel module and
- * user space application. iwl_testmode_gnl_msg_policy is to be carried
- * through the NL80211_CMD_TESTMODE channel regulated by nl80211.
- * See testmode.h
- */
-static
-struct nla_policy iwl_testmode_gnl_msg_policy[IWL_TM_ATTR_MAX] = {
-        [IWL_TM_ATTR_COMMAND] = { .type = NLA_U32, },
-
-        [IWL_TM_ATTR_UCODE_CMD_ID] = { .type = NLA_U8, },
-        [IWL_TM_ATTR_UCODE_CMD_DATA] = { .type = NLA_UNSPEC, },
-
-        [IWL_TM_ATTR_REG_OFFSET] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_REG_VALUE8] = { .type = NLA_U8, },
-        [IWL_TM_ATTR_REG_VALUE32] = { .type = NLA_U32, },
-
-        [IWL_TM_ATTR_SYNC_RSP] = { .type = NLA_UNSPEC, },
-        [IWL_TM_ATTR_UCODE_RX_PKT] = { .type = NLA_UNSPEC, },
+#include "iwl-test.h"
+#include "iwl-testmode.h"
 
-        [IWL_TM_ATTR_EEPROM] = { .type = NLA_UNSPEC, },
-
-        [IWL_TM_ATTR_TRACE_ADDR] = { .type = NLA_UNSPEC, },
-        [IWL_TM_ATTR_TRACE_DUMP] = { .type = NLA_UNSPEC, },
-        [IWL_TM_ATTR_TRACE_SIZE] = { .type = NLA_U32, },
-
-        [IWL_TM_ATTR_FIXRATE] = { .type = NLA_U32, },
-
-        [IWL_TM_ATTR_UCODE_OWNER] = { .type = NLA_U8, },
-
-        [IWL_TM_ATTR_MEM_ADDR] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_BUFFER_SIZE] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_BUFFER_DUMP] = { .type = NLA_UNSPEC, },
-
-        [IWL_TM_ATTR_FW_VERSION] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_DEVICE_ID] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_FW_TYPE] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_FW_INST_SIZE] = { .type = NLA_U32, },
-        [IWL_TM_ATTR_FW_DATA_SIZE] = { .type = NLA_U32, },
-
-        [IWL_TM_ATTR_ENABLE_NOTIFICATION] = {.type = NLA_FLAG, },
-};
-
-/*
- * See the struct iwl_rx_packet in commands.h for the format of the
- * received events from the device
- */
-static inline int get_event_length(struct iwl_rx_cmd_buffer *rxb)
+static int iwl_testmode_send_cmd(struct iwl_op_mode *op_mode,
+                                 struct iwl_host_cmd *cmd)
 {
-        struct iwl_rx_packet *pkt = rxb_addr(rxb);
-        if (pkt)
-                return le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-        else
-                return 0;
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return iwl_dvm_send_cmd(priv, cmd);
 }
 
-
-/*
- * This function multicasts the spontaneous messages from the device to the
- * user space. It is invoked whenever there is a received messages
- * from the device. This function is called within the ISR of the rx handlers
- * in iwlagn driver.
- *
- * The parsing of the message content is left to the user space application,
- * The message content is treated as unattacked raw data and is encapsulated
- * with IWL_TM_ATTR_UCODE_RX_PKT multicasting to the user space.
- *
- * @priv: the instance of iwlwifi device
- * @rxb: pointer to rx data content received by the ISR
- *
- * See the message policies and TLVs in iwl_testmode_gnl_msg_policy[].
- * For the messages multicasting to the user application, the mandatory
- * TLV fields are :
- *      IWL_TM_ATTR_COMMAND must be IWL_TM_CMD_DEV2APP_UCODE_RX_PKT
- *      IWL_TM_ATTR_UCODE_RX_PKT for carrying the message content
- */
-
-static void iwl_testmode_ucode_rx_pkt(struct iwl_priv *priv,
-                                      struct iwl_rx_cmd_buffer *rxb)
+static bool iwl_testmode_valid_hw_addr(u32 addr)
 {
-        struct ieee80211_hw *hw = priv->hw;
-        struct sk_buff *skb;
-        void *data;
-        int length;
+        if (iwlagn_hw_valid_rtc_data_addr(addr))
+                return true;
 
-        data = rxb_addr(rxb);
-        length = get_event_length(rxb);
+        if (IWLAGN_RTC_INST_LOWER_BOUND <= addr &&
+            addr < IWLAGN_RTC_INST_UPPER_BOUND)
+                return true;
 
-        if (!data || length == 0)
-                return;
-
-        skb = cfg80211_testmode_alloc_event_skb(hw->wiphy, 20 + length,
-                                                                GFP_ATOMIC);
-        if (skb == NULL) {
-                IWL_ERR(priv,
-                         "Run out of memory for messages to user space ?\n");
-                return;
-        }
-        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
-            /* the length doesn't include len_n_flags field, so add it manually */
-            nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, length + sizeof(__le32), data))
-                goto nla_put_failure;
-        cfg80211_testmode_event(skb, GFP_ATOMIC);
-        return;
-
-nla_put_failure:
-        kfree_skb(skb);
-        IWL_ERR(priv, "Ouch, overran buffer, check allocation!\n");
+        return false;
 }
 
-void iwl_testmode_init(struct iwl_priv *priv)
+static u32 iwl_testmode_get_fw_ver(struct iwl_op_mode *op_mode)
 {
-        priv->pre_rx_handler = NULL;
-        priv->testmode_trace.trace_enabled = false;
-        priv->testmode_mem.read_in_progress = false;
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return priv->fw->ucode_ver;
 }
 
-static void iwl_mem_cleanup(struct iwl_priv *priv)
+static struct sk_buff*
+iwl_testmode_alloc_reply(struct iwl_op_mode *op_mode, int len)
 {
-        if (priv->testmode_mem.read_in_progress) {
-                kfree(priv->testmode_mem.buff_addr);
-                priv->testmode_mem.buff_addr = NULL;
-                priv->testmode_mem.buff_size = 0;
-                priv->testmode_mem.num_chunks = 0;
-                priv->testmode_mem.read_in_progress = false;
-        }
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return cfg80211_testmode_alloc_reply_skb(priv->hw->wiphy, len);
 }
 
-static void iwl_trace_cleanup(struct iwl_priv *priv)
+static int iwl_testmode_reply(struct iwl_op_mode *op_mode, struct sk_buff *skb)
 {
-        if (priv->testmode_trace.trace_enabled) {
-                if (priv->testmode_trace.cpu_addr &&
-                    priv->testmode_trace.dma_addr)
-                        dma_free_coherent(priv->trans->dev,
-                                        priv->testmode_trace.total_size,
-                                        priv->testmode_trace.cpu_addr,
-                                        priv->testmode_trace.dma_addr);
-                priv->testmode_trace.trace_enabled = false;
-                priv->testmode_trace.cpu_addr = NULL;
-                priv->testmode_trace.trace_addr = NULL;
-                priv->testmode_trace.dma_addr = 0;
-                priv->testmode_trace.buff_size = 0;
-                priv->testmode_trace.total_size = 0;
-        }
+        return cfg80211_testmode_reply(skb);
 }
 
-
-void iwl_testmode_cleanup(struct iwl_priv *priv)
+static struct sk_buff *iwl_testmode_alloc_event(struct iwl_op_mode *op_mode,
+                                                int len)
 {
-        iwl_trace_cleanup(priv);
-        iwl_mem_cleanup(priv);
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return cfg80211_testmode_alloc_event_skb(priv->hw->wiphy, len,
+                                                 GFP_ATOMIC);
 }
 
-
-/*
- * This function handles the user application commands to the ucode.
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_CMD_ID and
- * IWL_TM_ATTR_UCODE_CMD_DATA and calls to the handler to send the
- * host command to the ucode.
- *
- * If any mandatory field is missing, -ENOMSG is replied to the user space
- * application; otherwise, waits for the host command to be sent and checks
- * the return code. In case or error, it is returned, otherwise a reply is
- * allocated and the reply RX packet
- * is returned.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_ucode(struct ieee80211_hw *hw, struct nlattr **tb)
+static void iwl_testmode_event(struct iwl_op_mode *op_mode, struct sk_buff *skb)
 {
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        struct iwl_host_cmd cmd;
-        struct iwl_rx_packet *pkt;
-        struct sk_buff *skb;
-        void *reply_buf;
-        u32 reply_len;
-        int ret;
-        bool cmd_want_skb;
-
-        memset(&cmd, 0, sizeof(struct iwl_host_cmd));
-
-        if (!tb[IWL_TM_ATTR_UCODE_CMD_ID] ||
-            !tb[IWL_TM_ATTR_UCODE_CMD_DATA]) {
-                IWL_ERR(priv, "Missing ucode command mandatory fields\n");
-                return -ENOMSG;
-        }
-
-        cmd.flags = CMD_ON_DEMAND | CMD_SYNC;
-        cmd_want_skb = nla_get_flag(tb[IWL_TM_ATTR_UCODE_CMD_SKB]);
-        if (cmd_want_skb)
-                cmd.flags |= CMD_WANT_SKB;
-
-        cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
-        cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
-        cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
-        cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
-        IWL_DEBUG_INFO(priv, "testmode ucode command ID 0x%x, flags 0x%x,"
-                                " len %d\n", cmd.id, cmd.flags, cmd.len[0]);
-
-        ret = iwl_dvm_send_cmd(priv, &cmd);
-        if (ret) {
-                IWL_ERR(priv, "Failed to send hcmd\n");
-                return ret;
-        }
-        if (!cmd_want_skb)
-                return ret;
-
-        /* Handling return of SKB to the user */
-        pkt = cmd.resp_pkt;
-        if (!pkt) {
-                IWL_ERR(priv, "HCMD received a null response packet\n");
-                return ret;
-        }
-
-        reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-        skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, reply_len + 20);
-        reply_buf = kmalloc(reply_len, GFP_KERNEL);
-        if (!skb || !reply_buf) {
-                kfree_skb(skb);
-                kfree(reply_buf);
-                return -ENOMEM;
-        }
-
-        /* The reply is in a page, that we cannot send to user space. */
-        memcpy(reply_buf, &(pkt->hdr), reply_len);
-        iwl_free_resp(&cmd);
-
-        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND, IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
-            nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf))
-                goto nla_put_failure;
-        return cfg80211_testmode_reply(skb);
-
-nla_put_failure:
-        IWL_DEBUG_INFO(priv, "Failed creating NL attributes\n");
-        return -ENOMSG;
+        return cfg80211_testmode_event(skb, GFP_ATOMIC);
 }
 
+static struct iwl_test_ops tst_ops = {
+        .send_cmd = iwl_testmode_send_cmd,
+        .valid_hw_addr = iwl_testmode_valid_hw_addr,
+        .get_fw_ver = iwl_testmode_get_fw_ver,
+        .alloc_reply = iwl_testmode_alloc_reply,
+        .reply = iwl_testmode_reply,
+        .alloc_event = iwl_testmode_alloc_event,
+        .event = iwl_testmode_event,
+};
 
-/*
- * This function handles the user application commands for register access.
- *
- * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
- * handlers respectively.
- *
- * If it's an unknown commdn ID, -ENOSYS is returned; or -ENOMSG if the
- * mandatory fields(IWL_TM_ATTR_REG_OFFSET,IWL_TM_ATTR_REG_VALUE32,
- * IWL_TM_ATTR_REG_VALUE8) are missing; Otherwise 0 is replied indicating
- * the success of the command execution.
- *
- * If IWL_TM_ATTR_COMMAND is IWL_TM_CMD_APP2DEV_REG_READ32, the register read
- * value is returned with IWL_TM_ATTR_REG_VALUE32.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_reg(struct ieee80211_hw *hw, struct nlattr **tb)
+void iwl_testmode_init(struct iwl_priv *priv)
 {
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        u32 ofs, val32, cmd;
-        u8 val8;
-        struct sk_buff *skb;
-        int status = 0;
-
-        if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
-                IWL_ERR(priv, "Missing register offset\n");
-                return -ENOMSG;
-        }
-        ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
-        IWL_INFO(priv, "testmode register access command offset 0x%x\n", ofs);
-
-        /* Allow access only to FH/CSR/HBUS in direct mode.
-        Since we don't have the upper bounds for the CSR and HBUS segments,
-        we will use only the upper bound of FH for sanity check. */
-        cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
-        if ((cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32 ||
-                cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32 ||
-                cmd == IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8) &&
-                (ofs >= FH_MEM_UPPER_BOUND)) {
-                IWL_ERR(priv, "offset out of segment (0x0 - 0x%x)\n",
-                        FH_MEM_UPPER_BOUND);
-                return -EINVAL;
-        }
-
-        switch (cmd) {
-        case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
-                val32 = iwl_read_direct32(priv->trans, ofs);
-                IWL_INFO(priv, "32bit value to read 0x%x\n", val32);
-
-                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-                if (!skb) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        return -ENOMEM;
-                }
-                if (nla_put_u32(skb, IWL_TM_ATTR_REG_VALUE32, val32))
-                        goto nla_put_failure;
-                status = cfg80211_testmode_reply(skb);
-                if (status < 0)
-                        IWL_ERR(priv, "Error sending msg : %d\n", status);
-                break;
-        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
-                if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
-                        IWL_ERR(priv, "Missing value to write\n");
-                        return -ENOMSG;
-                } else {
-                        val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
-                        IWL_INFO(priv, "32bit value to write 0x%x\n", val32);
-                        iwl_write_direct32(priv->trans, ofs, val32);
-                }
-                break;
-        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
-                if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
-                        IWL_ERR(priv, "Missing value to write\n");
-                        return -ENOMSG;
-                } else {
-                        val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
-                        IWL_INFO(priv, "8bit value to write 0x%x\n", val8);
-                        iwl_write8(priv->trans, ofs, val8);
-                }
-                break;
-        default:
-                IWL_ERR(priv, "Unknown testmode register command ID\n");
-                return -ENOSYS;
-        }
-
-        return status;
-
-nla_put_failure:
-        kfree_skb(skb);
-        return -EMSGSIZE;
+        iwl_test_init(&priv->tst, priv->trans, &tst_ops);
 }
 
+void iwl_testmode_free(struct iwl_priv *priv)
+{
+        iwl_test_free(&priv->tst);
+}
 
 static int iwl_testmode_cfg_init_calib(struct iwl_priv *priv)
 {
@@ -469,7 +199,7 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
         struct sk_buff *skb;
         unsigned char *rsp_data_ptr = NULL;
         int status = 0, rsp_data_len = 0;
-        u32 devid, inst_size = 0, data_size = 0;
+        u32 inst_size = 0, data_size = 0;
         const struct fw_img *img;
 
         switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
@@ -563,39 +293,6 @@ static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
                 priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
                 break;
 
-        case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
-                IWL_INFO(priv, "uCode version raw: 0x%x\n",
-                         priv->fw->ucode_ver);
-
-                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-                if (!skb) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        return -ENOMEM;
-                }
-                if (nla_put_u32(skb, IWL_TM_ATTR_FW_VERSION,
-                                priv->fw->ucode_ver))
-                        goto nla_put_failure;
-                status = cfg80211_testmode_reply(skb);
-                if (status < 0)
-                        IWL_ERR(priv, "Error sending msg : %d\n", status);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
-                devid = priv->trans->hw_id;
-                IWL_INFO(priv, "hw version: 0x%x\n", devid);
-
-                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20);
-                if (!skb) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        return -ENOMEM;
-                }
-                if (nla_put_u32(skb, IWL_TM_ATTR_DEVICE_ID, devid))
-                        goto nla_put_failure;
-                status = cfg80211_testmode_reply(skb);
-                if (status < 0)
-                        IWL_ERR(priv, "Error sending msg : %d\n", status);
-                break;
-
         case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
                 skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20 + 8);
                 if (!skb) {
@@ -630,125 +327,6 @@ nla_put_failure:
         return -EMSGSIZE;
 }
 
-
-/*
- * This function handles the user application commands for uCode trace
- *
- * It retrieves command ID carried with IWL_TM_ATTR_COMMAND and calls to the
- * handlers respectively.
- *
- * If it's an unknown commdn ID, -ENOSYS is replied; otherwise, the returned
- * value of the actual command execution is replied to the user application.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_trace(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        struct sk_buff *skb;
-        int status = 0;
-        struct device *dev = priv->trans->dev;
-
-        switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
-        case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
-                if (priv->testmode_trace.trace_enabled)
-                        return -EBUSY;
-
-                if (!tb[IWL_TM_ATTR_TRACE_SIZE])
-                        priv->testmode_trace.buff_size = TRACE_BUFF_SIZE_DEF;
-                else
-                        priv->testmode_trace.buff_size =
-                                nla_get_u32(tb[IWL_TM_ATTR_TRACE_SIZE]);
-                if (!priv->testmode_trace.buff_size)
-                        return -EINVAL;
-                if (priv->testmode_trace.buff_size < TRACE_BUFF_SIZE_MIN ||
-                    priv->testmode_trace.buff_size > TRACE_BUFF_SIZE_MAX)
-                        return -EINVAL;
-
-                priv->testmode_trace.total_size =
-                        priv->testmode_trace.buff_size + TRACE_BUFF_PADD;
-                priv->testmode_trace.cpu_addr =
-                        dma_alloc_coherent(dev,
-                                           priv->testmode_trace.total_size,
-                                           &priv->testmode_trace.dma_addr,
-                                           GFP_KERNEL);
-                if (!priv->testmode_trace.cpu_addr)
-                        return -ENOMEM;
-                priv->testmode_trace.trace_enabled = true;
-                priv->testmode_trace.trace_addr = (u8 *)PTR_ALIGN(
-                        priv->testmode_trace.cpu_addr, 0x100);
-                memset(priv->testmode_trace.trace_addr, 0x03B,
-                        priv->testmode_trace.buff_size);
-                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-                        sizeof(priv->testmode_trace.dma_addr) + 20);
-                if (!skb) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        iwl_trace_cleanup(priv);
-                        return -ENOMEM;
-                }
-                if (nla_put(skb, IWL_TM_ATTR_TRACE_ADDR,
-                            sizeof(priv->testmode_trace.dma_addr),
-                            (u64 *)&priv->testmode_trace.dma_addr))
-                        goto nla_put_failure;
-                status = cfg80211_testmode_reply(skb);
-                if (status < 0) {
-                        IWL_ERR(priv, "Error sending msg : %d\n", status);
-                }
-                priv->testmode_trace.num_chunks =
-                        DIV_ROUND_UP(priv->testmode_trace.buff_size,
-                                     DUMP_CHUNK_SIZE);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_END_TRACE:
-                iwl_trace_cleanup(priv);
-                break;
-        default:
-                IWL_ERR(priv, "Unknown testmode mem command ID\n");
-                return -ENOSYS;
-        }
-        return status;
-
-nla_put_failure:
-        kfree_skb(skb);
-        if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
-            IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
-                iwl_trace_cleanup(priv);
-        return -EMSGSIZE;
-}
-
-static int iwl_testmode_trace_dump(struct ieee80211_hw *hw,
-                                   struct sk_buff *skb,
-                                   struct netlink_callback *cb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        int idx, length;
-
-        if (priv->testmode_trace.trace_enabled &&
-            priv->testmode_trace.trace_addr) {
-                idx = cb->args[4];
-                if (idx >= priv->testmode_trace.num_chunks)
-                        return -ENOENT;
-                length = DUMP_CHUNK_SIZE;
-                if (((idx + 1) == priv->testmode_trace.num_chunks) &&
-                    (priv->testmode_trace.buff_size % DUMP_CHUNK_SIZE))
-                        length = priv->testmode_trace.buff_size %
-                                DUMP_CHUNK_SIZE;
-
-                if (nla_put(skb, IWL_TM_ATTR_TRACE_DUMP, length,
-                            priv->testmode_trace.trace_addr +
-                            (DUMP_CHUNK_SIZE * idx)))
-                        goto nla_put_failure;
-                idx++;
-                cb->args[4] = idx;
-                return 0;
-        } else
-                return -EFAULT;
-
- nla_put_failure:
-        return -ENOBUFS;
-}
-
 /*
  * This function handles the user application switch ucode ownership.
  *
@@ -777,10 +355,10 @@ static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
         owner = nla_get_u8(tb[IWL_TM_ATTR_UCODE_OWNER]);
         if (owner == IWL_OWNERSHIP_DRIVER) {
                 priv->ucode_owner = owner;
-                priv->pre_rx_handler = NULL;
+                iwl_test_enable_notifications(&priv->tst, false);
         } else if (owner == IWL_OWNERSHIP_TM) {
-                priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
                 priv->ucode_owner = owner;
+                iwl_test_enable_notifications(&priv->tst, true);
         } else {
                 IWL_ERR(priv, "Invalid owner\n");
                 return -EINVAL;
@@ -788,180 +366,6 @@ static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
         return 0;
 }
 
-static int iwl_testmode_indirect_read(struct iwl_priv *priv, u32 addr, u32 size)
-{
-        struct iwl_trans *trans = priv->trans;
-        unsigned long flags;
-        int i;
-
-        if (size & 0x3)
-                return -EINVAL;
-        priv->testmode_mem.buff_size = size;
-        priv->testmode_mem.buff_addr =
-                kmalloc(priv->testmode_mem.buff_size, GFP_KERNEL);
-        if (priv->testmode_mem.buff_addr == NULL)
-                return -ENOMEM;
-
-        /* Hard-coded periphery absolute address */
-        if (IWL_TM_ABS_PRPH_START <= addr &&
-                addr < IWL_TM_ABS_PRPH_START + PRPH_END) {
-                        spin_lock_irqsave(&trans->reg_lock, flags);
-                        iwl_grab_nic_access(trans);
-                        iwl_write32(trans, HBUS_TARG_PRPH_RADDR,
-                                addr | (3 << 24));
-                        for (i = 0; i < size; i += 4)
-                                *(u32 *)(priv->testmode_mem.buff_addr + i) =
-                                        iwl_read32(trans, HBUS_TARG_PRPH_RDAT);
-                        iwl_release_nic_access(trans);
-                        spin_unlock_irqrestore(&trans->reg_lock, flags);
-        } else { /* target memory (SRAM) */
-                _iwl_read_targ_mem_words(trans, addr,
-                        priv->testmode_mem.buff_addr,
-                        priv->testmode_mem.buff_size / 4);
-        }
-
-        priv->testmode_mem.num_chunks =
-                DIV_ROUND_UP(priv->testmode_mem.buff_size, DUMP_CHUNK_SIZE);
-        priv->testmode_mem.read_in_progress = true;
-        return 0;
-
-}
-
-static int iwl_testmode_indirect_write(struct iwl_priv *priv, u32 addr,
-        u32 size, unsigned char *buf)
-{
-        struct iwl_trans *trans = priv->trans;
-        u32 val, i;
-        unsigned long flags;
-
-        if (IWL_TM_ABS_PRPH_START <= addr &&
-                addr < IWL_TM_ABS_PRPH_START + PRPH_END) {
-                        /* Periphery writes can be 1-3 bytes long, or DWORDs */
-                        if (size < 4) {
-                                memcpy(&val, buf, size);
-                                spin_lock_irqsave(&trans->reg_lock, flags);
-                                iwl_grab_nic_access(trans);
-                                iwl_write32(trans, HBUS_TARG_PRPH_WADDR,
-                                            (addr & 0x0000FFFF) |
-                                            ((size - 1) << 24));
-                                iwl_write32(trans, HBUS_TARG_PRPH_WDAT, val);
-                                iwl_release_nic_access(trans);
-                                /* needed after consecutive writes w/o read */
-                                mmiowb();
-                                spin_unlock_irqrestore(&trans->reg_lock, flags);
-                        } else {
-                                if (size % 4)
-                                        return -EINVAL;
-                                for (i = 0; i < size; i += 4)
-                                        iwl_write_prph(trans, addr+i,
-                                                *(u32 *)(buf+i));
-                        }
-        } else if (iwlagn_hw_valid_rtc_data_addr(addr) ||
-                (IWLAGN_RTC_INST_LOWER_BOUND <= addr &&
-                addr < IWLAGN_RTC_INST_UPPER_BOUND)) {
-                        _iwl_write_targ_mem_words(trans, addr, buf, size/4);
-        } else
-                return -EINVAL;
-        return 0;
-}
-
-/*
- * This function handles the user application commands for SRAM data dump
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_SRAM_ADDR and
- * IWL_TM_ATTR_SRAM_SIZE to decide the memory area for SRAM data reading
- *
- * Several error will be retured, -EBUSY if the SRAM data retrieved by
- * previous command has not been delivered to userspace, or -ENOMSG if
- * the mandatory fields (IWL_TM_ATTR_SRAM_ADDR,IWL_TM_ATTR_SRAM_SIZE)
- * are missing, or -ENOMEM if the buffer allocation fails.
- *
- * Otherwise 0 is replied indicating the success of the SRAM reading.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_indirect_mem(struct ieee80211_hw *hw,
-        struct nlattr **tb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        u32 addr, size, cmd;
-        unsigned char *buf;
-
-        /* Both read and write should be blocked, for atomicity */
-        if (priv->testmode_mem.read_in_progress)
-                return -EBUSY;
-
-        cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
-        if (!tb[IWL_TM_ATTR_MEM_ADDR]) {
-                IWL_ERR(priv, "Error finding memory offset address\n");
-                return -ENOMSG;
-        }
-        addr = nla_get_u32(tb[IWL_TM_ATTR_MEM_ADDR]);
-        if (!tb[IWL_TM_ATTR_BUFFER_SIZE]) {
-                IWL_ERR(priv, "Error finding size for memory reading\n");
-                return -ENOMSG;
-        }
-        size = nla_get_u32(tb[IWL_TM_ATTR_BUFFER_SIZE]);
-
-        if (cmd == IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ)
-                return iwl_testmode_indirect_read(priv, addr,  size);
-        else {
-                if (!tb[IWL_TM_ATTR_BUFFER_DUMP])
-                        return -EINVAL;
-                buf = (unsigned char *) nla_data(tb[IWL_TM_ATTR_BUFFER_DUMP]);
-                return iwl_testmode_indirect_write(priv, addr, size, buf);
-        }
-}
-
-static int iwl_testmode_buffer_dump(struct ieee80211_hw *hw,
-                                    struct sk_buff *skb,
-                                    struct netlink_callback *cb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        int idx, length;
-
-        if (priv->testmode_mem.read_in_progress) {
-                idx = cb->args[4];
-                if (idx >= priv->testmode_mem.num_chunks) {
-                        iwl_mem_cleanup(priv);
-                        return -ENOENT;
-                }
-                length = DUMP_CHUNK_SIZE;
-                if (((idx + 1) == priv->testmode_mem.num_chunks) &&
-                    (priv->testmode_mem.buff_size % DUMP_CHUNK_SIZE))
-                        length = priv->testmode_mem.buff_size %
-                                DUMP_CHUNK_SIZE;
-
-                if (nla_put(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
-                            priv->testmode_mem.buff_addr +
-                            (DUMP_CHUNK_SIZE * idx)))
-                        goto nla_put_failure;
-                idx++;
-                cb->args[4] = idx;
-                return 0;
-        } else
-                return -EFAULT;
-
- nla_put_failure:
-        return -ENOBUFS;
-}
-
-static int iwl_testmode_notifications(struct ieee80211_hw *hw,
-        struct nlattr **tb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        bool enable;
-
-        enable = nla_get_flag(tb[IWL_TM_ATTR_ENABLE_NOTIFICATION]);
-        if (enable)
-                priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
-        else
-                priv->pre_rx_handler = NULL;
-        return 0;
-}
-
-
 /* The testmode gnl message handler that takes the gnl message from the
  * user space and parses it per the policy iwl_testmode_gnl_msg_policy, then
  * invoke the corresponding handlers.
@@ -987,32 +391,27 @@ int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         int result;
 
-        result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
-                        iwl_testmode_gnl_msg_policy);
-        if (result != 0) {
-                IWL_ERR(priv, "Error parsing the gnl message : %d\n", result);
+        result = iwl_test_parse(&priv->tst, tb, data, len);
+        if (result)
                 return result;
-        }
 
-        /* IWL_TM_ATTR_COMMAND is absolutely mandatory */
-        if (!tb[IWL_TM_ATTR_COMMAND]) {
-                IWL_ERR(priv, "Missing testmode command type\n");
-                return -ENOMSG;
-        }
         /* in case multiple accesses to the device happens */
         mutex_lock(&priv->mutex);
-
         switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
         case IWL_TM_CMD_APP2DEV_UCODE:
-                IWL_DEBUG_INFO(priv, "testmode cmd to uCode\n");
-                result = iwl_testmode_ucode(hw, tb);
-                break;
         case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
         case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
         case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
-                IWL_DEBUG_INFO(priv, "testmode cmd to register\n");
-                result = iwl_testmode_reg(hw, tb);
+        case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+        case IWL_TM_CMD_APP2DEV_END_TRACE:
+        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
+        case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
+        case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
+        case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
+        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
+                result = iwl_test_handle_cmd(&priv->tst, tb);
                 break;
+
         case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
         case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
         case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
@@ -1020,45 +419,25 @@ int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
         case IWL_TM_CMD_APP2DEV_GET_EEPROM:
         case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
         case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
-        case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
-        case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
         case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
                 IWL_DEBUG_INFO(priv, "testmode cmd to driver\n");
                 result = iwl_testmode_driver(hw, tb);
                 break;
 
-        case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
-        case IWL_TM_CMD_APP2DEV_END_TRACE:
-        case IWL_TM_CMD_APP2DEV_READ_TRACE:
-                IWL_DEBUG_INFO(priv, "testmode uCode trace cmd to driver\n");
-                result = iwl_testmode_trace(hw, tb);
-                break;
-
         case IWL_TM_CMD_APP2DEV_OWNERSHIP:
                 IWL_DEBUG_INFO(priv, "testmode change uCode ownership\n");
                 result = iwl_testmode_ownership(hw, tb);
                 break;
 
-        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
-        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
-                IWL_DEBUG_INFO(priv, "testmode indirect memory cmd "
-                        "to driver\n");
-                result = iwl_testmode_indirect_mem(hw, tb);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
-                IWL_DEBUG_INFO(priv, "testmode notifications cmd "
-                        "to driver\n");
-                result = iwl_testmode_notifications(hw, tb);
-                break;
-
         default:
                 IWL_ERR(priv, "Unknown testmode command\n");
                 result = -ENOSYS;
                 break;
         }
-
         mutex_unlock(&priv->mutex);
+
+        if (result)
+                IWL_ERR(priv, "Test cmd failed result=%d\n", result);
         return result;
 }
 
@@ -1066,7 +445,6 @@ int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
                       struct netlink_callback *cb,
                       void *data, int len)
 {
-        struct nlattr *tb[IWL_TM_ATTR_MAX];
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         int result;
         u32 cmd;
@@ -1075,39 +453,19 @@ int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
                 /* offset by 1 since commands start at 0 */
                 cmd = cb->args[3] - 1;
         } else {
-                result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
-                                iwl_testmode_gnl_msg_policy);
-                if (result) {
-                        IWL_ERR(priv,
-                                "Error parsing the gnl message : %d\n", result);
+                struct nlattr *tb[IWL_TM_ATTR_MAX];
+
+                result = iwl_test_parse(&priv->tst, tb, data, len);
+                if (result)
                         return result;
-                }
 
-                /* IWL_TM_ATTR_COMMAND is absolutely mandatory */
-                if (!tb[IWL_TM_ATTR_COMMAND]) {
-                        IWL_ERR(priv, "Missing testmode command type\n");
-                        return -ENOMSG;
-                }
                 cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
                 cb->args[3] = cmd + 1;
         }
 
         /* in case multiple accesses to the device happens */
         mutex_lock(&priv->mutex);
-        switch (cmd) {
-        case IWL_TM_CMD_APP2DEV_READ_TRACE:
-                IWL_DEBUG_INFO(priv, "uCode trace cmd to driver\n");
-                result = iwl_testmode_trace_dump(hw, skb, cb);
-                break;
-        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_DUMP:
-                IWL_DEBUG_INFO(priv, "testmode sram dump cmd to driver\n");
-                result = iwl_testmode_buffer_dump(hw, skb, cb);
-                break;
-        default:
-                result = -EINVAL;
-                break;
-        }
-
+        result = iwl_test_dump(&priv->tst, cmd, skb, cb);
         mutex_unlock(&priv->mutex);
         return result;
 }
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 0dfaf649b257..5971a23aa47d 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -403,6 +403,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
 
         info->driver_data[0] = ctx;
         info->driver_data[1] = dev_cmd;
+        /* From now on, we cannot access info->control */
 
         spin_lock(&priv->sta_lock);
 
@@ -1182,7 +1183,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
                 }
 
                 /*we can free until ssn % q.n_bd not inclusive */
-                WARN_ON(iwl_reclaim(priv, sta_id, tid, txq_id, ssn, &skbs));
+                WARN_ON_ONCE(iwl_reclaim(priv, sta_id, tid,
+                                         txq_id, ssn, &skbs));
                 iwlagn_check_ratid_empty(priv, sta_id, tid);
                 freed = 0;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 49df0e9d5c5f..a175997e7829 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -131,6 +131,8 @@ struct iwl_drv {
 #define DVM_OP_MODE     0
 #define MVM_OP_MODE     1
 
+/* Protects the table contents, i.e. the ops pointer & drv list */
+static struct mutex iwlwifi_opmode_table_mtx;
 static struct iwlwifi_opmode_table {
         const char *name;                       /* name: iwldvm, iwlmvm, etc */
         const struct iwl_op_mode_ops *ops;      /* pointer to op_mode ops */
@@ -776,6 +778,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
         const unsigned int api_min = drv->cfg->ucode_api_min;
         u32 api_ver;
         int i;
+        bool load_module = false;
 
         fw->ucode_capa.max_probe_length = 200;
         fw->ucode_capa.standard_phy_calibration_size =
@@ -898,6 +901,7 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
         /* We have our copies now, allow OS release its copies */
         release_firmware(ucode_raw);
 
+        mutex_lock(&iwlwifi_opmode_table_mtx);
         op = &iwlwifi_opmode_table[DVM_OP_MODE];
 
         /* add this device to the list of devices using this op_mode */
@@ -907,11 +911,14 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
                 const struct iwl_op_mode_ops *ops = op->ops;
                 drv->op_mode = ops->start(drv->trans, drv->cfg, &drv->fw);
 
-                if (!drv->op_mode)
+                if (!drv->op_mode) {
+                        mutex_unlock(&iwlwifi_opmode_table_mtx);
                         goto out_unbind;
+                }
         } else {
-                request_module_nowait("%s", op->name);
+                load_module = true;
         }
+        mutex_unlock(&iwlwifi_opmode_table_mtx);
 
         /*
          * Complete the firmware request last so that
@@ -919,6 +926,14 @@ static void iwl_ucode_callback(const struct firmware *ucode_raw, void *context)
          * are doing the start() above.
          */
         complete(&drv->request_firmware_complete);
+
+        /*
+         * Load the module last so we don't block anything
+         * else from proceeding if the module fails to load
+         * or hangs loading.
+         */
+        if (load_module)
+                request_module("%s", op->name);
         return;
 
  try_again:
@@ -952,6 +967,7 @@ struct iwl_drv *iwl_drv_start(struct iwl_trans *trans,
         drv->cfg = cfg;
 
         init_completion(&drv->request_firmware_complete);
+        INIT_LIST_HEAD(&drv->list);
 
         ret = iwl_request_firmware(drv, true);
 
@@ -974,6 +990,16 @@ void iwl_drv_stop(struct iwl_drv *drv)
 
         iwl_dealloc_ucode(drv);
 
+        mutex_lock(&iwlwifi_opmode_table_mtx);
+        /*
+         * List is empty (this item wasn't added)
+         * when firmware loading failed -- in that
+         * case we can't remove it from any list.
+         */
+        if (!list_empty(&drv->list))
+                list_del(&drv->list);
+        mutex_unlock(&iwlwifi_opmode_table_mtx);
+
         kfree(drv);
 }
 
@@ -996,6 +1022,7 @@ int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops)
         int i;
         struct iwl_drv *drv;
 
+        mutex_lock(&iwlwifi_opmode_table_mtx);
         for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++) {
                 if (strcmp(iwlwifi_opmode_table[i].name, name))
                         continue;
@@ -1003,8 +1030,10 @@ int iwl_opmode_register(const char *name, const struct iwl_op_mode_ops *ops)
                 list_for_each_entry(drv, &iwlwifi_opmode_table[i].drv, list)
                         drv->op_mode = ops->start(drv->trans, drv->cfg,
                                                   &drv->fw);
+                mutex_unlock(&iwlwifi_opmode_table_mtx);
                 return 0;
         }
+        mutex_unlock(&iwlwifi_opmode_table_mtx);
         return -EIO;
 }
 EXPORT_SYMBOL_GPL(iwl_opmode_register);
@@ -1014,6 +1043,7 @@ void iwl_opmode_deregister(const char *name)
         int i;
         struct iwl_drv *drv;
 
+        mutex_lock(&iwlwifi_opmode_table_mtx);
         for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++) {
                 if (strcmp(iwlwifi_opmode_table[i].name, name))
                         continue;
@@ -1026,8 +1056,10 @@ void iwl_opmode_deregister(const char *name)
                                 drv->op_mode = NULL;
                         }
                 }
+                mutex_unlock(&iwlwifi_opmode_table_mtx);
                 return;
         }
+        mutex_unlock(&iwlwifi_opmode_table_mtx);
 }
 EXPORT_SYMBOL_GPL(iwl_opmode_deregister);
 
@@ -1035,6 +1067,8 @@ static int __init iwl_drv_init(void)
 {
         int i;
 
+        mutex_init(&iwlwifi_opmode_table_mtx);
+
         for (i = 0; i < ARRAY_SIZE(iwlwifi_opmode_table); i++)
                 INIT_LIST_HEAD(&iwlwifi_opmode_table[i].drv);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index 74bce97a8600..806046641747 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -421,6 +421,8 @@ static inline unsigned int FH_MEM_CBBC_QUEUE(unsigned int chnl)
                 (FH_SRVC_LOWER_BOUND + ((_chnl) - 9) * 0x4)
 
 #define FH_TX_CHICKEN_BITS_REG  (FH_MEM_LOWER_BOUND + 0xE98)
+#define FH_TX_TRB_REG(_chan)    (FH_MEM_LOWER_BOUND + 0x958 + (_chan) * 4)
+
 /* Instruct FH to increment the retry count of a packet when
  * it is brought from the memory to TX-FIFO
  */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 5f2df70b73c1..66c873399aba 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -298,8 +298,8 @@ void iwl_clear_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask)
 }
 EXPORT_SYMBOL_GPL(iwl_clear_bits_prph);
 
-void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
-                              void *buf, int words)
+void _iwl_read_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+                               void *buf, int dwords)
 {
         unsigned long flags;
         int offs;
@@ -308,26 +308,26 @@ void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
         spin_lock_irqsave(&trans->reg_lock, flags);
         if (likely(iwl_grab_nic_access(trans))) {
                 iwl_write32(trans, HBUS_TARG_MEM_RADDR, addr);
-                for (offs = 0; offs < words; offs++)
+                for (offs = 0; offs < dwords; offs++)
                         vals[offs] = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
                 iwl_release_nic_access(trans);
         }
         spin_unlock_irqrestore(&trans->reg_lock, flags);
 }
-EXPORT_SYMBOL_GPL(_iwl_read_targ_mem_words);
+EXPORT_SYMBOL_GPL(_iwl_read_targ_mem_dwords);
 
 u32 iwl_read_targ_mem(struct iwl_trans *trans, u32 addr)
 {
         u32 value;
 
-        _iwl_read_targ_mem_words(trans, addr, &value, 1);
+        _iwl_read_targ_mem_dwords(trans, addr, &value, 1);
 
         return value;
 }
 EXPORT_SYMBOL_GPL(iwl_read_targ_mem);
 
-int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
-                                void *buf, int words)
+int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+                               void *buf, int dwords)
 {
         unsigned long flags;
         int offs, result = 0;
@@ -336,7 +336,7 @@ int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
         spin_lock_irqsave(&trans->reg_lock, flags);
         if (likely(iwl_grab_nic_access(trans))) {
                 iwl_write32(trans, HBUS_TARG_MEM_WADDR, addr);
-                for (offs = 0; offs < words; offs++)
+                for (offs = 0; offs < dwords; offs++)
                         iwl_write32(trans, HBUS_TARG_MEM_WDAT, vals[offs]);
                 iwl_release_nic_access(trans);
         } else
@@ -345,10 +345,10 @@ int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
 
         return result;
 }
-EXPORT_SYMBOL_GPL(_iwl_write_targ_mem_words);
+EXPORT_SYMBOL_GPL(_iwl_write_targ_mem_dwords);
 
 int iwl_write_targ_mem(struct iwl_trans *trans, u32 addr, u32 val)
 {
-        return _iwl_write_targ_mem_words(trans, addr, &val, 1);
+        return _iwl_write_targ_mem_dwords(trans, addr, &val, 1);
 }
 EXPORT_SYMBOL_GPL(iwl_write_targ_mem);
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
index 4a9a45f771ed..50d3819739d1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -76,18 +76,18 @@ void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 reg,
                             u32 bits, u32 mask);
 void iwl_clear_bits_prph(struct iwl_trans *trans, u32 reg, u32 mask);
 
-void _iwl_read_targ_mem_words(struct iwl_trans *trans, u32 addr,
-                              void *buf, int words);
+void _iwl_read_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+                               void *buf, int dwords);
 
-#define iwl_read_targ_mem_words(trans, addr, buf, bufsize)      \
+#define iwl_read_targ_mem_bytes(trans, addr, buf, bufsize)      \
         do {                                                    \
                 BUILD_BUG_ON((bufsize) % sizeof(u32));          \
-                _iwl_read_targ_mem_words(trans, addr, buf,      \
-                                         (bufsize) / sizeof(u32));\
+                _iwl_read_targ_mem_dwords(trans, addr, buf,     \
+                                          (bufsize) / sizeof(u32));\
         } while (0)
 
-int _iwl_write_targ_mem_words(struct iwl_trans *trans, u32 addr,
-                              void *buf, int words);
+int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
+                               void *buf, int dwords);
 
 u32 iwl_read_targ_mem(struct iwl_trans *trans, u32 addr);
 int iwl_write_targ_mem(struct iwl_trans *trans, u32 addr, u32 val);
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.c b/drivers/net/wireless/iwlwifi/iwl-test.c
new file mode 100644
index 000000000000..81e8c7126d72
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-test.c
@@ -0,0 +1,856 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2010 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2010 - 2012 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/export.h>
+#include <net/netlink.h>
+
+#include "iwl-io.h"
+#include "iwl-fh.h"
+#include "iwl-prph.h"
+#include "iwl-trans.h"
+#include "iwl-test.h"
+#include "iwl-csr.h"
+#include "iwl-testmode.h"
+
+/*
+ * Periphery registers absolute lower bound. This is used in order to
+ * differentiate registery access through HBUS_TARG_PRPH_* and
+ * HBUS_TARG_MEM_* accesses.
+ */
+#define IWL_ABS_PRPH_START (0xA00000)
+
+/*
+ * The TLVs used in the gnl message policy between the kernel module and
+ * user space application. iwl_testmode_gnl_msg_policy is to be carried
+ * through the NL80211_CMD_TESTMODE channel regulated by nl80211.
+ * See iwl-testmode.h
+ */
+static
+struct nla_policy iwl_testmode_gnl_msg_policy[IWL_TM_ATTR_MAX] = {
+        [IWL_TM_ATTR_COMMAND] = { .type = NLA_U32, },
+
+        [IWL_TM_ATTR_UCODE_CMD_ID] = { .type = NLA_U8, },
+        [IWL_TM_ATTR_UCODE_CMD_DATA] = { .type = NLA_UNSPEC, },
+
+        [IWL_TM_ATTR_REG_OFFSET] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_REG_VALUE8] = { .type = NLA_U8, },
+        [IWL_TM_ATTR_REG_VALUE32] = { .type = NLA_U32, },
+
+        [IWL_TM_ATTR_SYNC_RSP] = { .type = NLA_UNSPEC, },
+        [IWL_TM_ATTR_UCODE_RX_PKT] = { .type = NLA_UNSPEC, },
+
+        [IWL_TM_ATTR_EEPROM] = { .type = NLA_UNSPEC, },
+
+        [IWL_TM_ATTR_TRACE_ADDR] = { .type = NLA_UNSPEC, },
+        [IWL_TM_ATTR_TRACE_DUMP] = { .type = NLA_UNSPEC, },
+        [IWL_TM_ATTR_TRACE_SIZE] = { .type = NLA_U32, },
+
+        [IWL_TM_ATTR_FIXRATE] = { .type = NLA_U32, },
+
+        [IWL_TM_ATTR_UCODE_OWNER] = { .type = NLA_U8, },
+
+        [IWL_TM_ATTR_MEM_ADDR] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_BUFFER_SIZE] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_BUFFER_DUMP] = { .type = NLA_UNSPEC, },
+
+        [IWL_TM_ATTR_FW_VERSION] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_DEVICE_ID] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_FW_TYPE] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_FW_INST_SIZE] = { .type = NLA_U32, },
+        [IWL_TM_ATTR_FW_DATA_SIZE] = { .type = NLA_U32, },
+
+        [IWL_TM_ATTR_ENABLE_NOTIFICATION] = {.type = NLA_FLAG, },
+};
+
+static inline void iwl_test_trace_clear(struct iwl_test *tst)
+{
+        memset(&tst->trace, 0, sizeof(struct iwl_test_trace));
+}
+
+static void iwl_test_trace_stop(struct iwl_test *tst)
+{
+        if (!tst->trace.enabled)
+                return;
+
+        if (tst->trace.cpu_addr && tst->trace.dma_addr)
+                dma_free_coherent(tst->trans->dev,
+                                  tst->trace.tsize,
+                                  tst->trace.cpu_addr,
+                                  tst->trace.dma_addr);
+
+        iwl_test_trace_clear(tst);
+}
+
+static inline void iwl_test_mem_clear(struct iwl_test *tst)
+{
+        memset(&tst->mem, 0, sizeof(struct iwl_test_mem));
+}
+
+static inline void iwl_test_mem_stop(struct iwl_test *tst)
+{
+        if (!tst->mem.in_read)
+                return;
+
+        iwl_test_mem_clear(tst);
+}
+
+/*
+ * Initializes the test object
+ * During the lifetime of the test object it is assumed that the transport is
+ * started. The test object should be stopped before the transport is stopped.
+ */
+void iwl_test_init(struct iwl_test *tst, struct iwl_trans *trans,
+                   struct iwl_test_ops *ops)
+{
+        tst->trans = trans;
+        tst->ops = ops;
+
+        iwl_test_trace_clear(tst);
+        iwl_test_mem_clear(tst);
+}
+EXPORT_SYMBOL_GPL(iwl_test_init);
+
+/*
+ * Stop the test object
+ */
+void iwl_test_free(struct iwl_test *tst)
+{
+        iwl_test_mem_stop(tst);
+        iwl_test_trace_stop(tst);
+}
+EXPORT_SYMBOL_GPL(iwl_test_free);
+
+static inline int iwl_test_send_cmd(struct iwl_test *tst,
+                                    struct iwl_host_cmd *cmd)
+{
+        return tst->ops->send_cmd(tst->trans->op_mode, cmd);
+}
+
+static inline bool iwl_test_valid_hw_addr(struct iwl_test *tst, u32 addr)
+{
+        return tst->ops->valid_hw_addr(addr);
+}
+
+static inline u32 iwl_test_fw_ver(struct iwl_test *tst)
+{
+        return tst->ops->get_fw_ver(tst->trans->op_mode);
+}
+
+static inline struct sk_buff*
+iwl_test_alloc_reply(struct iwl_test *tst, int len)
+{
+        return tst->ops->alloc_reply(tst->trans->op_mode, len);
+}
+
+static inline int iwl_test_reply(struct iwl_test *tst, struct sk_buff *skb)
+{
+        return tst->ops->reply(tst->trans->op_mode, skb);
+}
+
+static inline struct sk_buff*
+iwl_test_alloc_event(struct iwl_test *tst, int len)
+{
+        return tst->ops->alloc_event(tst->trans->op_mode, len);
+}
+
+static inline void
+iwl_test_event(struct iwl_test *tst, struct sk_buff *skb)
+{
+        return tst->ops->event(tst->trans->op_mode, skb);
+}
+
+/*
+ * This function handles the user application commands to the fw. The fw
+ * commands are sent in a synchronuous manner. In case that the user requested
+ * to get commands response, it is send to the user.
+ */
+static int iwl_test_fw_cmd(struct iwl_test *tst, struct nlattr **tb)
+{
+        struct iwl_host_cmd cmd;
+        struct iwl_rx_packet *pkt;
+        struct sk_buff *skb;
+        void *reply_buf;
+        u32 reply_len;
+        int ret;
+        bool cmd_want_skb;
+
+        memset(&cmd, 0, sizeof(struct iwl_host_cmd));
+
+        if (!tb[IWL_TM_ATTR_UCODE_CMD_ID] ||
+            !tb[IWL_TM_ATTR_UCODE_CMD_DATA]) {
+                IWL_ERR(tst->trans, "Missing fw command mandatory fields\n");
+                return -ENOMSG;
+        }
+
+        cmd.flags = CMD_ON_DEMAND | CMD_SYNC;
+        cmd_want_skb = nla_get_flag(tb[IWL_TM_ATTR_UCODE_CMD_SKB]);
+        if (cmd_want_skb)
+                cmd.flags |= CMD_WANT_SKB;
+
+        cmd.id = nla_get_u8(tb[IWL_TM_ATTR_UCODE_CMD_ID]);
+        cmd.data[0] = nla_data(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+        cmd.len[0] = nla_len(tb[IWL_TM_ATTR_UCODE_CMD_DATA]);
+        cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
+        IWL_DEBUG_INFO(tst->trans, "test fw cmd=0x%x, flags 0x%x, len %d\n",
+                       cmd.id, cmd.flags, cmd.len[0]);
+
+        ret = iwl_test_send_cmd(tst, &cmd);
+        if (ret) {
+                IWL_ERR(tst->trans, "Failed to send hcmd\n");
+                return ret;
+        }
+        if (!cmd_want_skb)
+                return ret;
+
+        /* Handling return of SKB to the user */
+        pkt = cmd.resp_pkt;
+        if (!pkt) {
+                IWL_ERR(tst->trans, "HCMD received a null response packet\n");
+                return ret;
+        }
+
+        reply_len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+        skb = iwl_test_alloc_reply(tst, reply_len + 20);
+        reply_buf = kmalloc(reply_len, GFP_KERNEL);
+        if (!skb || !reply_buf) {
+                kfree_skb(skb);
+                kfree(reply_buf);
+                return -ENOMEM;
+        }
+
+        /* The reply is in a page, that we cannot send to user space. */
+        memcpy(reply_buf, &(pkt->hdr), reply_len);
+        iwl_free_resp(&cmd);
+
+        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+                        IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+            nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, reply_len, reply_buf))
+                goto nla_put_failure;
+        return iwl_test_reply(tst, skb);
+
+nla_put_failure:
+        IWL_DEBUG_INFO(tst->trans, "Failed creating NL attributes\n");
+        kfree(reply_buf);
+        kfree_skb(skb);
+        return -ENOMSG;
+}
+
+/*
+ * Handles the user application commands for register access.
+ */
+static int iwl_test_reg(struct iwl_test *tst, struct nlattr **tb)
+{
+        u32 ofs, val32, cmd;
+        u8 val8;
+        struct sk_buff *skb;
+        int status = 0;
+        struct iwl_trans *trans = tst->trans;
+
+        if (!tb[IWL_TM_ATTR_REG_OFFSET]) {
+                IWL_ERR(trans, "Missing reg offset\n");
+                return -ENOMSG;
+        }
+
+        ofs = nla_get_u32(tb[IWL_TM_ATTR_REG_OFFSET]);
+        IWL_DEBUG_INFO(trans, "test reg access cmd offset=0x%x\n", ofs);
+
+        cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
+
+        /*
+         * Allow access only to FH/CSR/HBUS in direct mode.
+         * Since we don't have the upper bounds for the CSR and HBUS segments,
+         * we will use only the upper bound of FH for sanity check.
+         */
+        if (ofs >= FH_MEM_UPPER_BOUND) {
+                IWL_ERR(trans, "offset out of segment (0x0 - 0x%x)\n",
+                        FH_MEM_UPPER_BOUND);
+                return -EINVAL;
+        }
+
+        switch (cmd) {
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+                val32 = iwl_read_direct32(tst->trans, ofs);
+                IWL_DEBUG_INFO(trans, "32 value to read 0x%x\n", val32);
+
+                skb = iwl_test_alloc_reply(tst, 20);
+                if (!skb) {
+                        IWL_ERR(trans, "Memory allocation fail\n");
+                        return -ENOMEM;
+                }
+                if (nla_put_u32(skb, IWL_TM_ATTR_REG_VALUE32, val32))
+                        goto nla_put_failure;
+                status = iwl_test_reply(tst, skb);
+                if (status < 0)
+                        IWL_ERR(trans, "Error sending msg : %d\n", status);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+                if (!tb[IWL_TM_ATTR_REG_VALUE32]) {
+                        IWL_ERR(trans, "Missing value to write\n");
+                        return -ENOMSG;
+                } else {
+                        val32 = nla_get_u32(tb[IWL_TM_ATTR_REG_VALUE32]);
+                        IWL_DEBUG_INFO(trans, "32b write val=0x%x\n", val32);
+                        iwl_write_direct32(tst->trans, ofs, val32);
+                }
+                break;
+
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+                if (!tb[IWL_TM_ATTR_REG_VALUE8]) {
+                        IWL_ERR(trans, "Missing value to write\n");
+                        return -ENOMSG;
+                } else {
+                        val8 = nla_get_u8(tb[IWL_TM_ATTR_REG_VALUE8]);
+                        IWL_DEBUG_INFO(trans, "8b write val=0x%x\n", val8);
+                        iwl_write8(tst->trans, ofs, val8);
+                }
+                break;
+
+        default:
+                IWL_ERR(trans, "Unknown test register cmd ID\n");
+                return -ENOMSG;
+        }
+
+        return status;
+
+nla_put_failure:
+        kfree_skb(skb);
+        return -EMSGSIZE;
+}
+
+/*
+ * Handles the request to start FW tracing. Allocates of the trace buffer
+ * and sends a reply to user space with the address of the allocated buffer.
+ */
+static int iwl_test_trace_begin(struct iwl_test *tst, struct nlattr **tb)
+{
+        struct sk_buff *skb;
+        int status = 0;
+
+        if (tst->trace.enabled)
+                return -EBUSY;
+
+        if (!tb[IWL_TM_ATTR_TRACE_SIZE])
+                tst->trace.size = TRACE_BUFF_SIZE_DEF;
+        else
+                tst->trace.size =
+                        nla_get_u32(tb[IWL_TM_ATTR_TRACE_SIZE]);
+
+        if (!tst->trace.size)
+                return -EINVAL;
+
+        if (tst->trace.size < TRACE_BUFF_SIZE_MIN ||
+            tst->trace.size > TRACE_BUFF_SIZE_MAX)
+                return -EINVAL;
+
+        tst->trace.tsize = tst->trace.size + TRACE_BUFF_PADD;
+        tst->trace.cpu_addr = dma_alloc_coherent(tst->trans->dev,
+                                                 tst->trace.tsize,
+                                                 &tst->trace.dma_addr,
+                                                 GFP_KERNEL);
+        if (!tst->trace.cpu_addr)
+                return -ENOMEM;
+
+        tst->trace.enabled = true;
+        tst->trace.trace_addr = (u8 *)PTR_ALIGN(tst->trace.cpu_addr, 0x100);
+
+        memset(tst->trace.trace_addr, 0x03B, tst->trace.size);
+
+        skb = iwl_test_alloc_reply(tst, sizeof(tst->trace.dma_addr) + 20);
+        if (!skb) {
+                IWL_ERR(tst->trans, "Memory allocation fail\n");
+                iwl_test_trace_stop(tst);
+                return -ENOMEM;
+        }
+
+        if (nla_put(skb, IWL_TM_ATTR_TRACE_ADDR,
+                    sizeof(tst->trace.dma_addr),
+                    (u64 *)&tst->trace.dma_addr))
+                goto nla_put_failure;
+
+        status = iwl_test_reply(tst, skb);
+        if (status < 0)
+                IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+        tst->trace.nchunks = DIV_ROUND_UP(tst->trace.size,
+                                          DUMP_CHUNK_SIZE);
+
+        return status;
+
+nla_put_failure:
+        kfree_skb(skb);
+        if (nla_get_u32(tb[IWL_TM_ATTR_COMMAND]) ==
+            IWL_TM_CMD_APP2DEV_BEGIN_TRACE)
+                iwl_test_trace_stop(tst);
+        return -EMSGSIZE;
+}
+
+/*
+ * Handles indirect read from the periphery or the SRAM. The read is performed
+ * to a temporary buffer. The user space application should later issue a dump
+ */
+static int iwl_test_indirect_read(struct iwl_test *tst, u32 addr, u32 size)
+{
+        struct iwl_trans *trans = tst->trans;
+        unsigned long flags;
+        int i;
+
+        if (size & 0x3)
+                return -EINVAL;
+
+        tst->mem.size = size;
+        tst->mem.addr = kmalloc(tst->mem.size, GFP_KERNEL);
+        if (tst->mem.addr == NULL)
+                return -ENOMEM;
+
+        /* Hard-coded periphery absolute address */
+        if (IWL_ABS_PRPH_START <= addr &&
+            addr < IWL_ABS_PRPH_START + PRPH_END) {
+                        spin_lock_irqsave(&trans->reg_lock, flags);
+                        iwl_grab_nic_access(trans);
+                        iwl_write32(trans, HBUS_TARG_PRPH_RADDR,
+                                    addr | (3 << 24));
+                        for (i = 0; i < size; i += 4)
+                                *(u32 *)(tst->mem.addr + i) =
+                                        iwl_read32(trans, HBUS_TARG_PRPH_RDAT);
+                        iwl_release_nic_access(trans);
+                        spin_unlock_irqrestore(&trans->reg_lock, flags);
+        } else { /* target memory (SRAM) */
+                _iwl_read_targ_mem_dwords(trans, addr,
+                                          tst->mem.addr,
+                                          tst->mem.size / 4);
+        }
+
+        tst->mem.nchunks =
+                DIV_ROUND_UP(tst->mem.size, DUMP_CHUNK_SIZE);
+        tst->mem.in_read = true;
+        return 0;
+
+}
+
+/*
+ * Handles indirect write to the periphery or SRAM. The  is performed to a
+ * temporary buffer.
+ */
+static int iwl_test_indirect_write(struct iwl_test *tst, u32 addr,
+        u32 size, unsigned char *buf)
+{
+        struct iwl_trans *trans = tst->trans;
+        u32 val, i;
+        unsigned long flags;
+
+        if (IWL_ABS_PRPH_START <= addr &&
+            addr < IWL_ABS_PRPH_START + PRPH_END) {
+                        /* Periphery writes can be 1-3 bytes long, or DWORDs */
+                        if (size < 4) {
+                                memcpy(&val, buf, size);
+                                spin_lock_irqsave(&trans->reg_lock, flags);
+                                iwl_grab_nic_access(trans);
+                                iwl_write32(trans, HBUS_TARG_PRPH_WADDR,
+                                            (addr & 0x0000FFFF) |
+                                            ((size - 1) << 24));
+                                iwl_write32(trans, HBUS_TARG_PRPH_WDAT, val);
+                                iwl_release_nic_access(trans);
+                                /* needed after consecutive writes w/o read */
+                                mmiowb();
+                                spin_unlock_irqrestore(&trans->reg_lock, flags);
+                        } else {
+                                if (size % 4)
+                                        return -EINVAL;
+                                for (i = 0; i < size; i += 4)
+                                        iwl_write_prph(trans, addr+i,
+                                                       *(u32 *)(buf+i));
+                        }
+        } else if (iwl_test_valid_hw_addr(tst, addr)) {
+                _iwl_write_targ_mem_dwords(trans, addr, buf, size / 4);
+        } else {
+                return -EINVAL;
+        }
+        return 0;
+}
+
+/*
+ * Handles the user application commands for indirect read/write
+ * to/from the periphery or the SRAM.
+ */
+static int iwl_test_indirect_mem(struct iwl_test *tst, struct nlattr **tb)
+{
+        u32 addr, size, cmd;
+        unsigned char *buf;
+
+        /* Both read and write should be blocked, for atomicity */
+        if (tst->mem.in_read)
+                return -EBUSY;
+
+        cmd = nla_get_u32(tb[IWL_TM_ATTR_COMMAND]);
+        if (!tb[IWL_TM_ATTR_MEM_ADDR]) {
+                IWL_ERR(tst->trans, "Error finding memory offset address\n");
+                return -ENOMSG;
+        }
+        addr = nla_get_u32(tb[IWL_TM_ATTR_MEM_ADDR]);
+        if (!tb[IWL_TM_ATTR_BUFFER_SIZE]) {
+                IWL_ERR(tst->trans, "Error finding size for memory reading\n");
+                return -ENOMSG;
+        }
+        size = nla_get_u32(tb[IWL_TM_ATTR_BUFFER_SIZE]);
+
+        if (cmd == IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ) {
+                return iwl_test_indirect_read(tst, addr,  size);
+        } else {
+                if (!tb[IWL_TM_ATTR_BUFFER_DUMP])
+                        return -EINVAL;
+                buf = (unsigned char *)nla_data(tb[IWL_TM_ATTR_BUFFER_DUMP]);
+                return iwl_test_indirect_write(tst, addr, size, buf);
+        }
+}
+
+/*
+ * Enable notifications to user space
+ */
+static int iwl_test_notifications(struct iwl_test *tst,
+                                  struct nlattr **tb)
+{
+        tst->notify = nla_get_flag(tb[IWL_TM_ATTR_ENABLE_NOTIFICATION]);
+        return 0;
+}
+
+/*
+ * Handles the request to get the device id
+ */
+static int iwl_test_get_dev_id(struct iwl_test *tst, struct nlattr **tb)
+{
+        u32 devid = tst->trans->hw_id;
+        struct sk_buff *skb;
+        int status;
+
+        IWL_DEBUG_INFO(tst->trans, "hw version: 0x%x\n", devid);
+
+        skb = iwl_test_alloc_reply(tst, 20);
+        if (!skb) {
+                IWL_ERR(tst->trans, "Memory allocation fail\n");
+                return -ENOMEM;
+        }
+
+        if (nla_put_u32(skb, IWL_TM_ATTR_DEVICE_ID, devid))
+                goto nla_put_failure;
+        status = iwl_test_reply(tst, skb);
+        if (status < 0)
+                IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+        return 0;
+
+nla_put_failure:
+        kfree_skb(skb);
+        return -EMSGSIZE;
+}
+
+/*
+ * Handles the request to get the FW version
+ */
+static int iwl_test_get_fw_ver(struct iwl_test *tst, struct nlattr **tb)
+{
+        struct sk_buff *skb;
+        int status;
+        u32 ver = iwl_test_fw_ver(tst);
+
+        IWL_DEBUG_INFO(tst->trans, "uCode version raw: 0x%x\n", ver);
+
+        skb = iwl_test_alloc_reply(tst, 20);
+        if (!skb) {
+                IWL_ERR(tst->trans, "Memory allocation fail\n");
+                return -ENOMEM;
+        }
+
+        if (nla_put_u32(skb, IWL_TM_ATTR_FW_VERSION, ver))
+                goto nla_put_failure;
+
+        status = iwl_test_reply(tst, skb);
+        if (status < 0)
+                IWL_ERR(tst->trans, "Error sending msg : %d\n", status);
+
+        return 0;
+
+nla_put_failure:
+        kfree_skb(skb);
+        return -EMSGSIZE;
+}
+
+/*
+ * Parse the netlink message and validate that the IWL_TM_ATTR_CMD exists
+ */
+int iwl_test_parse(struct iwl_test *tst, struct nlattr **tb,
+                   void *data, int len)
+{
+        int result;
+
+        result = nla_parse(tb, IWL_TM_ATTR_MAX - 1, data, len,
+                        iwl_testmode_gnl_msg_policy);
+        if (result) {
+                IWL_ERR(tst->trans, "Fail parse gnl msg: %d\n", result);
+                return result;
+        }
+
+        /* IWL_TM_ATTR_COMMAND is absolutely mandatory */
+        if (!tb[IWL_TM_ATTR_COMMAND]) {
+                IWL_ERR(tst->trans, "Missing testmode command type\n");
+                return -ENOMSG;
+        }
+        return 0;
+}
+EXPORT_SYMBOL_GPL(iwl_test_parse);
+
+/*
+ * Handle test commands.
+ * Returns 1 for unknown commands (not handled by the test object); negative
+ * value in case of error.
+ */
+int iwl_test_handle_cmd(struct iwl_test *tst, struct nlattr **tb)
+{
+        int result;
+
+        switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+        case IWL_TM_CMD_APP2DEV_UCODE:
+                IWL_DEBUG_INFO(tst->trans, "test cmd to uCode\n");
+                result = iwl_test_fw_cmd(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+                IWL_DEBUG_INFO(tst->trans, "test cmd to register\n");
+                result = iwl_test_reg(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_BEGIN_TRACE:
+                IWL_DEBUG_INFO(tst->trans, "test uCode trace cmd to driver\n");
+                result = iwl_test_trace_begin(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_END_TRACE:
+                iwl_test_trace_stop(tst);
+                result = 0;
+                break;
+
+        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_READ:
+        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_WRITE:
+                IWL_DEBUG_INFO(tst->trans, "test indirect memory cmd\n");
+                result = iwl_test_indirect_mem(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_NOTIFICATIONS:
+                IWL_DEBUG_INFO(tst->trans, "test notifications cmd\n");
+                result = iwl_test_notifications(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_GET_FW_VERSION:
+                IWL_DEBUG_INFO(tst->trans, "test get FW ver cmd\n");
+                result = iwl_test_get_fw_ver(tst, tb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_GET_DEVICE_ID:
+                IWL_DEBUG_INFO(tst->trans, "test Get device ID cmd\n");
+                result = iwl_test_get_dev_id(tst, tb);
+                break;
+
+        default:
+                IWL_DEBUG_INFO(tst->trans, "Unknown test command\n");
+                result = 1;
+                break;
+        }
+        return result;
+}
+EXPORT_SYMBOL_GPL(iwl_test_handle_cmd);
+
+static int iwl_test_trace_dump(struct iwl_test *tst, struct sk_buff *skb,
+                               struct netlink_callback *cb)
+{
+        int idx, length;
+
+        if (!tst->trace.enabled || !tst->trace.trace_addr)
+                return -EFAULT;
+
+        idx = cb->args[4];
+        if (idx >= tst->trace.nchunks)
+                return -ENOENT;
+
+        length = DUMP_CHUNK_SIZE;
+        if (((idx + 1) == tst->trace.nchunks) &&
+            (tst->trace.size % DUMP_CHUNK_SIZE))
+                length = tst->trace.size %
+                        DUMP_CHUNK_SIZE;
+
+        if (nla_put(skb, IWL_TM_ATTR_TRACE_DUMP, length,
+                    tst->trace.trace_addr + (DUMP_CHUNK_SIZE * idx)))
+                goto nla_put_failure;
+
+        cb->args[4] = ++idx;
+        return 0;
+
+ nla_put_failure:
+        return -ENOBUFS;
+}
+
+static int iwl_test_buffer_dump(struct iwl_test *tst, struct sk_buff *skb,
+                                struct netlink_callback *cb)
+{
+        int idx, length;
+
+        if (!tst->mem.in_read)
+                return -EFAULT;
+
+        idx = cb->args[4];
+        if (idx >= tst->mem.nchunks) {
+                iwl_test_mem_stop(tst);
+                return -ENOENT;
+        }
+
+        length = DUMP_CHUNK_SIZE;
+        if (((idx + 1) == tst->mem.nchunks) &&
+            (tst->mem.size % DUMP_CHUNK_SIZE))
+                length = tst->mem.size % DUMP_CHUNK_SIZE;
+
+        if (nla_put(skb, IWL_TM_ATTR_BUFFER_DUMP, length,
+                    tst->mem.addr + (DUMP_CHUNK_SIZE * idx)))
+                goto nla_put_failure;
+
+        cb->args[4] = ++idx;
+        return 0;
+
+ nla_put_failure:
+        return -ENOBUFS;
+}
+
+/*
+ * Handle dump commands.
+ * Returns 1 for unknown commands (not handled by the test object); negative
+ * value in case of error.
+ */
+int iwl_test_dump(struct iwl_test *tst, u32 cmd, struct sk_buff *skb,
+                  struct netlink_callback *cb)
+{
+        int result;
+
+        switch (cmd) {
+        case IWL_TM_CMD_APP2DEV_READ_TRACE:
+                IWL_DEBUG_INFO(tst->trans, "uCode trace cmd\n");
+                result = iwl_test_trace_dump(tst, skb, cb);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_INDIRECT_BUFFER_DUMP:
+                IWL_DEBUG_INFO(tst->trans, "testmode sram dump cmd\n");
+                result = iwl_test_buffer_dump(tst, skb, cb);
+                break;
+
+        default:
+                result = 1;
+                break;
+        }
+        return result;
+}
+EXPORT_SYMBOL_GPL(iwl_test_dump);
+
+/*
+ * Multicast a spontaneous messages from the device to the user space.
+ */
+static void iwl_test_send_rx(struct iwl_test *tst,
+                             struct iwl_rx_cmd_buffer *rxb)
+{
+        struct sk_buff *skb;
+        struct iwl_rx_packet *data;
+        int length;
+
+        data = rxb_addr(rxb);
+        length = le32_to_cpu(data->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+
+        /* the length doesn't include len_n_flags field, so add it manually */
+        length += sizeof(__le32);
+
+        skb = iwl_test_alloc_event(tst, length + 20);
+        if (skb == NULL) {
+                IWL_ERR(tst->trans, "Out of memory for message to user\n");
+                return;
+        }
+
+        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+                        IWL_TM_CMD_DEV2APP_UCODE_RX_PKT) ||
+            nla_put(skb, IWL_TM_ATTR_UCODE_RX_PKT, length, data))
+                goto nla_put_failure;
+
+        iwl_test_event(tst, skb);
+        return;
+
+nla_put_failure:
+        kfree_skb(skb);
+        IWL_ERR(tst->trans, "Ouch, overran buffer, check allocation!\n");
+}
+
+/*
+ * Called whenever a Rx frames is recevied from the device. If notifications to
+ * the user space are requested, sends the frames to the user.
+ */
+void iwl_test_rx(struct iwl_test *tst, struct iwl_rx_cmd_buffer *rxb)
+{
+        if (tst->notify)
+                iwl_test_send_rx(tst, rxb);
+}
+EXPORT_SYMBOL_GPL(iwl_test_rx);
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.h b/drivers/net/wireless/iwlwifi/iwl-test.h
new file mode 100644
index 000000000000..e13ffa8acc02
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-test.h
@@ -0,0 +1,161 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2010 - 2012 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2010 - 2012 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __IWL_TEST_H__
+#define __IWL_TEST_H__
+
+#include <linux/types.h>
+#include "iwl-trans.h"
+
+struct iwl_test_trace {
+        u32 size;
+        u32 tsize;
+        u32 nchunks;
+        u8 *cpu_addr;
+        u8 *trace_addr;
+        dma_addr_t dma_addr;
+        bool enabled;
+};
+
+struct iwl_test_mem {
+        u32 size;
+        u32 nchunks;
+        u8 *addr;
+        bool in_read;
+};
+
+/*
+ * struct iwl_test_ops: callback to the op mode
+ *
+ * The structure defines the callbacks that the op_mode should handle,
+ * inorder to handle logic that is out of the scope of iwl_test. The
+ * op_mode must set all the callbacks.
+
+ * @send_cmd: handler that is used by the test object to request the
+ *  op_mode to send a command to the fw.
+ *
+ * @valid_hw_addr: handler that is used by the test object to request the
+ *  op_mode to check if the given address is a valid address.
+ *
+ * @get_fw_ver: handler used to get the FW version.
+ *
+ * @alloc_reply: handler used by the test object to request the op_mode
+ *  to allocate an skb for sending a reply to the user, and initialize
+ *  the skb. It is assumed that the test object only fills the required
+ *  attributes.
+ *
+ * @reply: handler used by the test object to request the op_mode to reply
+ *  to a request. The skb is an skb previously allocated by the the
+ *  alloc_reply callback.
+ I
+ * @alloc_event: handler used by the test object to request the op_mode
+ *  to allocate an skb for sending an event, and initialize
+ *  the skb. It is assumed that the test object only fills the required
+ *  attributes.
+ *
+ * @reply: handler used by the test object to request the op_mode to send
+ *  an event. The skb is an skb previously allocated by the the
+ *  alloc_event callback.
+ */
+struct iwl_test_ops {
+        int (*send_cmd)(struct iwl_op_mode *op_modes,
+                        struct iwl_host_cmd *cmd);
+        bool (*valid_hw_addr)(u32 addr);
+        u32 (*get_fw_ver)(struct iwl_op_mode *op_mode);
+
+        struct sk_buff *(*alloc_reply)(struct iwl_op_mode *op_mode, int len);
+        int (*reply)(struct iwl_op_mode *op_mode, struct sk_buff *skb);
+        struct sk_buff* (*alloc_event)(struct iwl_op_mode *op_mode, int len);
+        void (*event)(struct iwl_op_mode *op_mode, struct sk_buff *skb);
+};
+
+struct iwl_test {
+        struct iwl_trans *trans;
+        struct iwl_test_ops *ops;
+        struct iwl_test_trace trace;
+        struct iwl_test_mem mem;
+        bool notify;
+};
+
+void iwl_test_init(struct iwl_test *tst, struct iwl_trans *trans,
+                   struct iwl_test_ops *ops);
+
+void iwl_test_free(struct iwl_test *tst);
+
+int iwl_test_parse(struct iwl_test *tst, struct nlattr **tb,
+                   void *data, int len);
+
+int iwl_test_handle_cmd(struct iwl_test *tst, struct nlattr **tb);
+
+int iwl_test_dump(struct iwl_test *tst, u32 cmd, struct sk_buff *skb,
+                  struct netlink_callback *cb);
+
+void iwl_test_rx(struct iwl_test *tst, struct iwl_rx_cmd_buffer *rxb);
+
+static inline void iwl_test_enable_notifications(struct iwl_test *tst,
+                                                 bool enable)
+{
+        tst->notify = enable;
+}
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/dvm/testmode.h b/drivers/net/wireless/iwlwifi/iwl-testmode.h
index 6ba211b09426..6ba211b09426 100644
--- a/drivers/net/wireless/iwlwifi/dvm/testmode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-testmode.h
diff --git a/drivers/net/wireless/iwlwifi/pcie/6000.c b/drivers/net/wireless/iwlwifi/pcie/6000.c
index cb08ba03aae7..4a57624afc40 100644
--- a/drivers/net/wireless/iwlwifi/pcie/6000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/6000.c
@@ -258,6 +258,7 @@ const struct iwl_cfg iwl6030_2bg_cfg = {
         .eeprom_calib_ver = EEPROM_6030_TX_POWER_VERSION,       \
         .base_params = &iwl6000_g2_base_params,                 \
         .bt_params = &iwl6000_bt_params,                        \
+        .eeprom_params = &iwl6000_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .led_mode = IWL_LED_RF_STATE,                           \
         .adv_pm = true
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index 94201c4d6227..5024fb662bf6 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -339,16 +339,9 @@ void iwl_tx_cmd_complete(struct iwl_trans *trans,
 void iwl_trans_txq_update_byte_cnt_tbl(struct iwl_trans *trans,
                                        struct iwl_tx_queue *txq,
                                        u16 byte_cnt);
-void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue);
-void iwl_trans_set_wr_ptrs(struct iwl_trans *trans, int txq_id, u32 index);
-void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-                                   struct iwl_tx_queue *txq,
-                                   int tx_fifo_id, bool active);
-void __iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id,
-                                        int fifo, int sta_id, int tid,
-                                        int frame_limit, u16 ssn);
 void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
                                int sta_id, int tid, int frame_limit, u16 ssn);
+void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue);
 void iwl_txq_free_tfd(struct iwl_trans *trans, struct iwl_tx_queue *txq,
                       enum dma_data_direction dma_dir);
 int iwl_tx_queue_reclaim(struct iwl_trans *trans, int txq_id, int index,
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 7461a6a14338..32ab8ea56135 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -298,6 +298,10 @@ static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
         struct iwl_tx_queue *txq = (void *)data;
         struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
         struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
+        u32 scd_sram_addr = trans_pcie->scd_base_addr +
+                SCD_TX_STTS_MEM_LOWER_BOUND + (16 * txq->q.id);
+        u8 buf[16];
+        int i;
 
         spin_lock(&txq->lock);
         /* check if triggered erroneously */
@@ -307,15 +311,40 @@ static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
         }
         spin_unlock(&txq->lock);
 
-
         IWL_ERR(trans, "Queue %d stuck for %u ms.\n", txq->q.id,
                 jiffies_to_msecs(trans_pcie->wd_timeout));
         IWL_ERR(trans, "Current SW read_ptr %d write_ptr %d\n",
                 txq->q.read_ptr, txq->q.write_ptr);
-        IWL_ERR(trans, "Current HW read_ptr %d write_ptr %d\n",
-                iwl_read_prph(trans, SCD_QUEUE_RDPTR(txq->q.id))
-                                        & (TFD_QUEUE_SIZE_MAX - 1),
-                iwl_read_prph(trans, SCD_QUEUE_WRPTR(txq->q.id)));
+
+        iwl_read_targ_mem_bytes(trans, scd_sram_addr, buf, sizeof(buf));
+
+        iwl_print_hex_error(trans, buf, sizeof(buf));
+
+        for (i = 0; i < FH_TCSR_CHNL_NUM; i++)
+                IWL_ERR(trans, "FH TRBs(%d) = 0x%08x\n", i,
+                        iwl_read_direct32(trans, FH_TX_TRB_REG(i)));
+
+        for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
+                u32 status = iwl_read_prph(trans, SCD_QUEUE_STATUS_BITS(i));
+                u8 fifo = (status >> SCD_QUEUE_STTS_REG_POS_TXF) & 0x7;
+                bool active = !!(status & BIT(SCD_QUEUE_STTS_REG_POS_ACTIVE));
+                u32 tbl_dw =
+                        iwl_read_targ_mem(trans,
+                                          trans_pcie->scd_base_addr +
+                                          SCD_TRANS_TBL_OFFSET_QUEUE(i));
+
+                if (i & 0x1)
+                        tbl_dw = (tbl_dw & 0xFFFF0000) >> 16;
+                else
+                        tbl_dw = tbl_dw & 0x0000FFFF;
+
+                IWL_ERR(trans,
+                        "Q %d is %sactive and mapped to fifo %d ra_tid 0x%04x [%d,%d]\n",
+                        i, active ? "" : "in", fifo, tbl_dw,
+                        iwl_read_prph(trans,
+                                      SCD_QUEUE_RDPTR(i)) & (txq->q.n_bd - 1),
+                        iwl_read_prph(trans, SCD_QUEUE_WRPTR(i)));
+        }
 
         iwl_op_mode_nic_error(trans->op_mode);
 }
@@ -1054,23 +1083,21 @@ static void iwl_tx_start(struct iwl_trans *trans)
         iwl_write_prph(trans, SCD_DRAM_BASE_ADDR,
                        trans_pcie->scd_bc_tbls.dma >> 10);
 
+        /* The chain extension of the SCD doesn't work well. This feature is
+         * enabled by default by the HW, so we need to disable it manually.
+         */
+        iwl_write_prph(trans, SCD_CHAINEXT_EN, 0);
+
         for (i = 0; i < trans_pcie->n_q_to_fifo; i++) {
                 int fifo = trans_pcie->setup_q_to_fifo[i];
 
-                __iwl_trans_pcie_txq_enable(trans, i, fifo, IWL_INVALID_STATION,
-                                            IWL_TID_NON_QOS,
-                                            SCD_FRAME_LIMIT, 0);
+                iwl_trans_pcie_txq_enable(trans, i, fifo, IWL_INVALID_STATION,
+                                          IWL_TID_NON_QOS, SCD_FRAME_LIMIT, 0);
         }
 
         /* Activate all Tx DMA/FIFO channels */
         iwl_trans_txq_set_sched(trans, IWL_MASK(0, 7));
 
-        /* The chain extension of the SCD doesn't work well. This feature is
-         * enabled by default by the HW, so we need to disable it manually.
-         */
-        iwl_write_prph(trans, SCD_CHAINEXT_EN, 0);
-
-
         /* Enable DMA channel */
         for (chan = 0; chan < FH_TCSR_CHNL_NUM ; chan++)
                 iwl_write_direct32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(chan),
@@ -1239,6 +1266,19 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 
         spin_lock(&txq->lock);
 
+        /* In AGG mode, the index in the ring must correspond to the WiFi
+         * sequence number. This is a HW requirements to help the SCD to parse
+         * the BA.
+         * Check here that the packets are in the right place on the ring.
+         */
+#ifdef CONFIG_IWLWIFI_DEBUG
+        wifi_seq = SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl));
+        WARN_ONCE((iwl_read_prph(trans, SCD_AGGR_SEL) & BIT(txq_id)) &&
+                  ((wifi_seq & 0xff) != q->write_ptr),
+                  "Q: %d WiFi Seq %d tfdNum %d",
+                  txq_id, wifi_seq, q->write_ptr);
+#endif
+
         /* Set up driver data for this TFD */
         txq->entries[q->write_ptr].skb = skb;
         txq->entries[q->write_ptr].cmd = dev_cmd;
@@ -1332,7 +1372,8 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
                              skb->data + hdr_len, secondlen);
 
         /* start timer if queue currently empty */
-        if (q->read_ptr == q->write_ptr && trans_pcie->wd_timeout)
+        if (txq->need_update && q->read_ptr == q->write_ptr &&
+            trans_pcie->wd_timeout)
                 mod_timer(&txq->stuck_timer, jiffies + trans_pcie->wd_timeout);
 
         /* Tell device the write index *just past* this latest filled TFD */
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 35e82161ca43..6baf8deef519 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -380,8 +380,8 @@ static void iwlagn_txq_inval_byte_cnt_tbl(struct iwl_trans *trans,
                         tfd_offset[TFD_QUEUE_SIZE_MAX + read_ptr] = bc_ent;
 }
 
-static int iwlagn_tx_queue_set_q2ratid(struct iwl_trans *trans, u16 ra_tid,
-                                       u16 txq_id)
+static int iwl_txq_set_ratid_map(struct iwl_trans *trans, u16 ra_tid,
+                                 u16 txq_id)
 {
         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
         u32 tbl_dw_addr;
@@ -405,7 +405,7 @@ static int iwlagn_tx_queue_set_q2ratid(struct iwl_trans *trans, u16 ra_tid,
         return 0;
 }
 
-static void iwlagn_tx_queue_stop_scheduler(struct iwl_trans *trans, u16 txq_id)
+static inline void iwl_txq_set_inactive(struct iwl_trans *trans, u16 txq_id)
 {
         /* Simply stop the queue, but don't change any configuration;
          * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
@@ -415,46 +415,16 @@ static void iwlagn_tx_queue_stop_scheduler(struct iwl_trans *trans, u16 txq_id)
                 (1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
 }
 
-void iwl_trans_set_wr_ptrs(struct iwl_trans *trans, int txq_id, u32 index)
-{
-        IWL_DEBUG_TX_QUEUES(trans, "Q %d  WrPtr: %d\n", txq_id, index & 0xff);
-        iwl_write_direct32(trans, HBUS_TARG_WRPTR,
-                           (index & 0xff) | (txq_id << 8));
-        iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), index);
-}
-
-void iwl_trans_tx_queue_set_status(struct iwl_trans *trans,
-                                   struct iwl_tx_queue *txq,
-                                   int tx_fifo_id, bool active)
-{
-        int txq_id = txq->q.id;
-
-        iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
-                        (active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
-                        (tx_fifo_id << SCD_QUEUE_STTS_REG_POS_TXF) |
-                        (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
-                        SCD_QUEUE_STTS_REG_MSK);
-
-        if (active)
-                IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d\n",
-                                    txq_id, tx_fifo_id);
-        else
-                IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
-}
-
-void __iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id,
-                                        int fifo, int sta_id, int tid,
-                                        int frame_limit, u16 ssn)
+void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
+                               int sta_id, int tid, int frame_limit, u16 ssn)
 {
         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-        lockdep_assert_held(&trans_pcie->irq_lock);
-
         if (test_and_set_bit(txq_id, trans_pcie->queue_used))
                 WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
 
         /* Stop this Tx queue before configuring it */
-        iwlagn_tx_queue_stop_scheduler(trans, txq_id);
+        iwl_txq_set_inactive(trans, txq_id);
 
         /* Set this queue as a chain-building queue unless it is CMD queue */
         if (txq_id != trans_pcie->cmd_queue)
@@ -465,17 +435,27 @@ void __iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id,
                 u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
                 /* Map receiver-address / traffic-ID to this queue */
-                iwlagn_tx_queue_set_q2ratid(trans, ra_tid, txq_id);
+                iwl_txq_set_ratid_map(trans, ra_tid, txq_id);
 
                 /* enable aggregations for the queue */
                 iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+        } else {
+                /*
+                 * disable aggregations for the queue, this will also make the
+                 * ra_tid mapping configuration irrelevant since it is now a
+                 * non-AGG queue.
+                 */
+                iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
         }
 
         /* Place first TFD at index corresponding to start sequence number.
          * Assumes that ssn_idx is valid (!= 0xFFF) */
         trans_pcie->txq[txq_id].q.read_ptr = (ssn & 0xff);
         trans_pcie->txq[txq_id].q.write_ptr = (ssn & 0xff);
-        iwl_trans_set_wr_ptrs(trans, txq_id, ssn);
+
+        iwl_write_direct32(trans, HBUS_TARG_WRPTR,
+                           (ssn & 0xff) | (txq_id << 8));
+        iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), ssn);
 
         /* Set up Tx window size and frame limit for this queue */
         iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
@@ -488,43 +468,34 @@ void __iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id,
                                 SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
 
         /* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
-        iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
-                                      fifo, true);
-}
-
-void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
-                               int sta_id, int tid, int frame_limit, u16 ssn)
-{
-        struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-        unsigned long flags;
-
-        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
-
-        __iwl_trans_pcie_txq_enable(trans, txq_id, fifo, sta_id,
-                                    tid, frame_limit, ssn);
-
-        spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+        iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
+                       (1 << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+                       (fifo << SCD_QUEUE_STTS_REG_POS_TXF) |
+                       (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
+                       SCD_QUEUE_STTS_REG_MSK);
+        IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d WrPtr: %d\n",
+                            txq_id, fifo, ssn & 0xff);
 }
 
 void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id)
 {
         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+        u16 rd_ptr, wr_ptr;
+        int n_bd = trans_pcie->txq[txq_id].q.n_bd;
 
         if (!test_and_clear_bit(txq_id, trans_pcie->queue_used)) {
                 WARN_ONCE(1, "queue %d not used", txq_id);
                 return;
         }
 
-        iwlagn_tx_queue_stop_scheduler(trans, txq_id);
-
-        iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+        rd_ptr = iwl_read_prph(trans, SCD_QUEUE_RDPTR(txq_id)) & (n_bd - 1);
+        wr_ptr = iwl_read_prph(trans, SCD_QUEUE_WRPTR(txq_id));
 
-        trans_pcie->txq[txq_id].q.read_ptr = 0;
-        trans_pcie->txq[txq_id].q.write_ptr = 0;
-        iwl_trans_set_wr_ptrs(trans, txq_id, 0);
+        WARN_ONCE(rd_ptr != wr_ptr, "queue %d isn't empty: [%d,%d]",
+                  txq_id, rd_ptr, wr_ptr);
 
-        iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[txq_id],
-                                      0, false);
+        iwl_txq_set_inactive(trans, txq_id);
+        IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
 }
 
 /*************** HOST COMMAND QUEUE FUNCTIONS   *****/
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index 8173ab66066d..89f9a2a45de3 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -27,6 +27,17 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
                               struct cfg80211_ap_settings *params) {
         int i;
 
+        if (!params->privacy) {
+                bss_config->protocol = PROTOCOL_NO_SECURITY;
+                bss_config->key_mgmt = KEY_MGMT_NONE;
+                bss_config->wpa_cfg.length = 0;
+                priv->sec_info.wep_enabled = 0;
+                priv->sec_info.wpa_enabled = 0;
+                priv->sec_info.wpa2_enabled = 0;
+
+                return 0;
+        }
+
         switch (params->auth_type) {
         case NL80211_AUTHTYPE_OPEN_SYSTEM:
                 bss_config->auth_mode = WLAN_AUTH_OPEN;
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 2e9e6af21362..dfcd02ab6cae 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -2110,7 +2110,7 @@ resize_buf:
         while (check_bssid_list_item(bssid, bssid_len, buf, len)) {
                 if (rndis_bss_info_update(usbdev, bssid) && match_bssid &&
                     matched) {
-                        if (!ether_addr_equal(bssid->mac, match_bssid))
+                        if (ether_addr_equal(bssid->mac, match_bssid))
                                 *matched = true;
                 }