237 files changed, 12779 insertions, 9277 deletions

diff --git a/MAINTAINERS b/MAINTAINERS
index c5fd905206e..fe6dd3d7d2c 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -1595,6 +1595,7 @@ M:	Arend van Spriel <arend@broadcom.com>
 M:      Franky (Zhenhui) Lin <frankyl@broadcom.com>
 M:      Kan Yan <kanyan@broadcom.com>
 L:      linux-wireless@vger.kernel.org
+L:      brcm80211-dev-list@broadcom.com
 S:      Supported
 F:      drivers/net/wireless/brcm80211/
 
@@ -1800,6 +1801,9 @@ F:	include/linux/cfag12864b.h
 CFG80211 and NL80211
 M:      Johannes Berg <johannes@sipsolutions.net>
 L:      linux-wireless@vger.kernel.org
+W:      http://wireless.kernel.org/
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211-next.git
 S:      Maintained
 F:      include/linux/nl80211.h
 F:      include/net/cfg80211.h
@@ -4340,7 +4344,8 @@ MAC80211
 M:      Johannes Berg <johannes@sipsolutions.net>
 L:      linux-wireless@vger.kernel.org
 W:      http://wireless.kernel.org/
-T:      git git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211-next.git
 S:      Maintained
 F:      Documentation/networking/mac80211-injection.txt
 F:      include/net/mac80211.h
@@ -4351,7 +4356,8 @@ M:	Stefano Brivio <stefano.brivio@polimi.it>
 M:      Mattias Nissler <mattias.nissler@gmx.de>
 L:      linux-wireless@vger.kernel.org
 W:      http://wireless.kernel.org/en/developers/Documentation/mac80211/RateControl/PID
-T:      git git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211-next.git
 S:      Maintained
 F:      net/mac80211/rc80211_pid*
 
@@ -5695,6 +5701,9 @@ F:	include/linux/remoteproc.h
 RFKILL
 M:      Johannes Berg <johannes@sipsolutions.net>
 L:      linux-wireless@vger.kernel.org
+W:      http://wireless.kernel.org/
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211.git
+T:      git git://git.kernel.org/pub/scm/linux/kernel/git/jberg/mac80211-next.git
 S:      Maintained
 F:      Documentation/rfkill.txt
 F:      net/rfkill/
diff --git a/drivers/bcma/driver_pci.c b/drivers/bcma/driver_pci.c
index 9a96f14c8f4..c32ebd537ab 100644
--- a/drivers/bcma/driver_pci.c
+++ b/drivers/bcma/driver_pci.c
@@ -232,17 +232,19 @@ void __devinit bcma_core_pci_init(struct bcma_drv_pci *pc)
 int bcma_core_pci_irq_ctl(struct bcma_drv_pci *pc, struct bcma_device *core,
                           bool enable)
 {
-        struct pci_dev *pdev = pc->core->bus->host_pci;
+        struct pci_dev *pdev;
         u32 coremask, tmp;
         int err = 0;
 
-        if (core->bus->hosttype != BCMA_HOSTTYPE_PCI) {
+        if (!pc || core->bus->hosttype != BCMA_HOSTTYPE_PCI) {
                 /* This bcma device is not on a PCI host-bus. So the IRQs are
                  * not routed through the PCI core.
                  * So we must not enable routing through the PCI core. */
                 goto out;
         }
 
+        pdev = pc->core->bus->host_pci;
+
         err = pci_read_config_dword(pdev, BCMA_PCI_IRQMASK, &tmp);
         if (err)
                 goto out;
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 5ed0718fc66..a3420585d94 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -28,6 +28,12 @@ static const struct bcma_device_id_name bcma_arm_device_names[] = {
 
 static const struct bcma_device_id_name bcma_bcm_device_names[] = {
         { BCMA_CORE_OOB_ROUTER, "OOB Router" },
+        { BCMA_CORE_4706_CHIPCOMMON, "BCM4706 ChipCommon" },
+        { BCMA_CORE_4706_SOC_RAM, "BCM4706 SOC RAM" },
+        { BCMA_CORE_4706_MAC_GBIT, "BCM4706 GBit MAC" },
+        { BCMA_CORE_AMEMC, "AMEMC (DDR)" },
+        { BCMA_CORE_ALTA, "ALTA (I2S)" },
+        { BCMA_CORE_4706_MAC_GBIT_COMMON, "BCM4706 GBit MAC Common" },
         { BCMA_CORE_INVALID, "Invalid" },
         { BCMA_CORE_CHIPCOMMON, "ChipCommon" },
         { BCMA_CORE_ILINE20, "ILine 20" },
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 1fcd9238035..585c88e0189 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 609861a53c2..29caaed2d71 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 308c8599ab5..b2b0fbbb43b 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 a853244e7fd..2867499f725 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 c4fc2f3fc32..65b8d996840 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 c9463af8e56..a45e717f5f8 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 6e8d9618968..b1b37ccd3cd 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 661a8dc4d2f..57e502e0608 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 748329468d2..c60623f206d 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 e564579a611..2f9b796e106 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 b874c0efde2..ff6d589c34a 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/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index b8fce0d4d72..aca1d2689e9 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -53,6 +53,11 @@
 
 #define DEFAULT_BG_SCAN_PERIOD 60
 
+struct ath6kl_cfg80211_match_probe_ssid {
+        struct cfg80211_ssid ssid;
+        u8 flag;
+};
+
 static struct ieee80211_rate ath6kl_rates[] = {
         RATETAB_ENT(10, 0x1, 0),
         RATETAB_ENT(20, 0x2, 0),
@@ -576,6 +581,9 @@ static int ath6kl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 
         vif->nw_type = vif->next_mode;
 
+        /* enable enhanced bmiss detection if applicable */
+        ath6kl_cfg80211_sta_bmiss_enhance(vif, true);
+
         if (vif->wdev.iftype == NL80211_IFTYPE_P2P_CLIENT)
                 nw_subtype = SUBTYPE_P2PCLIENT;
 
@@ -852,20 +860,6 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
                 }
         }
 
-        /*
-         * Send a disconnect command to target when a disconnect event is
-         * received with reason code other than 3 (DISCONNECT_CMD - disconnect
-         * request from host) to make the firmware stop trying to connect even
-         * after giving disconnect event. There will be one more disconnect
-         * event for this disconnect command with reason code DISCONNECT_CMD
-         * which will be notified to cfg80211.
-         */
-
-        if (reason != DISCONNECT_CMD) {
-                ath6kl_wmi_disconnect_cmd(ar->wmi, vif->fw_vif_idx);
-                return;
-        }
-
         clear_bit(CONNECT_PEND, &vif->flags);
 
         if (vif->sme_state == SME_CONNECTING) {
@@ -875,32 +869,96 @@ void ath6kl_cfg80211_disconnect_event(struct ath6kl_vif *vif, u8 reason,
                                         WLAN_STATUS_UNSPECIFIED_FAILURE,
                                         GFP_KERNEL);
         } else if (vif->sme_state == SME_CONNECTED) {
-                cfg80211_disconnected(vif->ndev, reason,
+                cfg80211_disconnected(vif->ndev, proto_reason,
                                       NULL, 0, GFP_KERNEL);
         }
 
         vif->sme_state = SME_DISCONNECTED;
+
+        /*
+         * Send a disconnect command to target when a disconnect event is
+         * received with reason code other than 3 (DISCONNECT_CMD - disconnect
+         * request from host) to make the firmware stop trying to connect even
+         * after giving disconnect event. There will be one more disconnect
+         * event for this disconnect command with reason code DISCONNECT_CMD
+         * which won't be notified to cfg80211.
+         */
+        if (reason != DISCONNECT_CMD)
+                ath6kl_wmi_disconnect_cmd(ar->wmi, vif->fw_vif_idx);
 }
 
 static int ath6kl_set_probed_ssids(struct ath6kl *ar,
                                    struct ath6kl_vif *vif,
-                                   struct cfg80211_ssid *ssids, int n_ssids)
+                                   struct cfg80211_ssid *ssids, int n_ssids,
+                                   struct cfg80211_match_set *match_set,
+                                   int n_match_ssid)
 {
-        u8 i;
+        u8 i, j, index_to_add, ssid_found = false;
+        struct ath6kl_cfg80211_match_probe_ssid ssid_list[MAX_PROBED_SSIDS];
+
+        memset(ssid_list, 0, sizeof(ssid_list));
 
-        if (n_ssids > MAX_PROBED_SSID_INDEX)
+        if (n_ssids > MAX_PROBED_SSIDS ||
+            n_match_ssid > MAX_PROBED_SSIDS)
                 return -EINVAL;
 
         for (i = 0; i < n_ssids; i++) {
+                memcpy(ssid_list[i].ssid.ssid,
+                       ssids[i].ssid,
+                       ssids[i].ssid_len);
+                ssid_list[i].ssid.ssid_len = ssids[i].ssid_len;
+
+                if (ssids[i].ssid_len)
+                        ssid_list[i].flag = SPECIFIC_SSID_FLAG;
+                else
+                        ssid_list[i].flag = ANY_SSID_FLAG;
+
+                if (n_match_ssid == 0)
+                        ssid_list[i].flag |= MATCH_SSID_FLAG;
+        }
+
+        index_to_add = i;
+
+        for (i = 0; i < n_match_ssid; i++) {
+                ssid_found = false;
+
+                for (j = 0; j < n_ssids; j++) {
+                        if ((match_set[i].ssid.ssid_len ==
+                             ssid_list[j].ssid.ssid_len) &&
+                            (!memcmp(ssid_list[j].ssid.ssid,
+                                     match_set[i].ssid.ssid,
+                                     match_set[i].ssid.ssid_len))) {
+                                ssid_list[j].flag |= MATCH_SSID_FLAG;
+                                ssid_found = true;
+                                break;
+                        }
+                }
+
+                if (ssid_found)
+                        continue;
+
+                if (index_to_add >= MAX_PROBED_SSIDS)
+                        continue;
+
+                ssid_list[index_to_add].ssid.ssid_len =
+                        match_set[i].ssid.ssid_len;
+                memcpy(ssid_list[index_to_add].ssid.ssid,
+                       match_set[i].ssid.ssid,
+                       match_set[i].ssid.ssid_len);
+                ssid_list[index_to_add].flag |= MATCH_SSID_FLAG;
+                index_to_add++;
+        }
+
+        for (i = 0; i < index_to_add; i++) {
                 ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
-                                          ssids[i].ssid_len ?
-                                          SPECIFIC_SSID_FLAG : ANY_SSID_FLAG,
-                                          ssids[i].ssid_len,
-                                          ssids[i].ssid);
+                                          ssid_list[i].flag,
+                                          ssid_list[i].ssid.ssid_len,
+                                          ssid_list[i].ssid.ssid);
+
         }
 
         /* Make sure no old entries are left behind */
-        for (i = n_ssids; i < MAX_PROBED_SSID_INDEX; i++) {
+        for (i = index_to_add; i < MAX_PROBED_SSIDS; i++) {
                 ath6kl_wmi_probedssid_cmd(ar->wmi, vif->fw_vif_idx, i,
                                           DISABLE_SSID_FLAG, 0, NULL);
         }
@@ -934,7 +992,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
         }
 
         ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
-                                      request->n_ssids);
+                                      request->n_ssids, NULL, 0);
         if (ret < 0)
                 return ret;
 
@@ -943,7 +1001,7 @@ static int ath6kl_cfg80211_scan(struct wiphy *wiphy, struct net_device *ndev,
                                        WMI_FRAME_PROBE_REQ,
                                        request->ie, request->ie_len);
         if (ret) {
-                ath6kl_err("failed to set Probe Request appie for scan");
+                ath6kl_err("failed to set Probe Request appie for scan\n");
                 return ret;
         }
 
@@ -1512,6 +1570,9 @@ static int ath6kl_cfg80211_change_iface(struct wiphy *wiphy,
                 }
         }
 
+        /* need to clean up enhanced bmiss detection fw state */
+        ath6kl_cfg80211_sta_bmiss_enhance(vif, false);
+
 set_iface_type:
         switch (type) {
         case NL80211_IFTYPE_STATION:
@@ -2074,7 +2135,9 @@ static int ath6kl_wow_suspend(struct ath6kl *ar, struct cfg80211_wowlan *wow)
         if (wow && (wow->n_patterns > WOW_MAX_FILTERS_PER_LIST))
                 return -EINVAL;
 
-        if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags)) {
+        if (!test_bit(NETDEV_MCAST_ALL_ON, &vif->flags) &&
+            test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+                     ar->fw_capabilities)) {
                 ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
                                                 vif->fw_vif_idx, false);
                 if (ret)
@@ -2209,7 +2272,9 @@ static int ath6kl_wow_resume(struct ath6kl *ar)
 
         ar->state = ATH6KL_STATE_ON;
 
-        if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags)) {
+        if (!test_bit(NETDEV_MCAST_ALL_OFF, &vif->flags) &&
+            test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+                     ar->fw_capabilities)) {
                 ret = ath6kl_wmi_mcast_filter_cmd(vif->ar->wmi,
                                         vif->fw_vif_idx, true);
                 if (ret)
@@ -2475,7 +2540,7 @@ void ath6kl_check_wow_status(struct ath6kl *ar)
 static int ath6kl_set_htcap(struct ath6kl_vif *vif, enum ieee80211_band band,
                             bool ht_enable)
 {
-        struct ath6kl_htcap *htcap = &vif->htcap;
+        struct ath6kl_htcap *htcap = &vif->htcap[band];
 
         if (htcap->ht_enable == ht_enable)
                 return 0;
@@ -2585,6 +2650,30 @@ static int ath6kl_set_ies(struct ath6kl_vif *vif,
         return 0;
 }
 
+void ath6kl_cfg80211_sta_bmiss_enhance(struct ath6kl_vif *vif, bool enable)
+{
+        int err;
+
+        if (WARN_ON(!test_bit(WMI_READY, &vif->ar->flag)))
+                return;
+
+        if (vif->nw_type != INFRA_NETWORK)
+                return;
+
+        if (!test_bit(ATH6KL_FW_CAPABILITY_BMISS_ENHANCE,
+                      vif->ar->fw_capabilities))
+                return;
+
+        ath6kl_dbg(ATH6KL_DBG_WLAN_CFG, "%s fw bmiss enhance\n",
+                   enable ? "enable" : "disable");
+
+        err = ath6kl_wmi_sta_bmiss_enhance_cmd(vif->ar->wmi,
+                                               vif->fw_vif_idx, enable);
+        if (err)
+                ath6kl_err("failed to %s enhanced bmiss detection: %d\n",
+                           enable ? "enable" : "disable", err);
+}
+
 static int ath6kl_get_rsn_capab(struct cfg80211_beacon_data *beacon,
                                 u8 *rsn_capab)
 {
@@ -2665,9 +2754,15 @@ static int ath6kl_start_ap(struct wiphy *wiphy, struct net_device *dev,
 
         /* TODO:
          * info->interval
-         * info->dtim_period
          */
 
+        ret = ath6kl_wmi_ap_set_dtim_cmd(ar->wmi, vif->fw_vif_idx,
+                                         info->dtim_period);
+
+        /* ignore error, just print a warning and continue normally */
+        if (ret)
+                ath6kl_warn("Failed to set dtim_period in beacon: %d\n", ret);
+
         if (info->beacon.head == NULL)
                 return -EINVAL;
         mgmt = (struct ieee80211_mgmt *) info->beacon.head;
@@ -3131,10 +3226,24 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
         ath6kl_cfg80211_scan_complete_event(vif, true);
 
         ret = ath6kl_set_probed_ssids(ar, vif, request->ssids,
-                                      request->n_ssids);
+                                      request->n_ssids,
+                                      request->match_sets,
+                                      request->n_match_sets);
         if (ret < 0)
                 return ret;
 
+        if (!request->n_match_sets) {
+                ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+                                               ALL_BSS_FILTER, 0);
+                if (ret < 0)
+                        return ret;
+        } else {
+                 ret = ath6kl_wmi_bssfilter_cmd(ar->wmi, vif->fw_vif_idx,
+                                                MATCHED_SSID_FILTER, 0);
+                if (ret < 0)
+                        return ret;
+        }
+
         /* fw uses seconds, also make sure that it's >0 */
         interval = max_t(u16, 1, request->interval / 1000);
 
@@ -3156,7 +3265,7 @@ static int ath6kl_cfg80211_sscan_start(struct wiphy *wiphy,
                                        WMI_FRAME_PROBE_REQ,
                                        request->ie, request->ie_len);
         if (ret) {
-                ath6kl_warn("Failed to set probe request IE for scheduled scan: %d",
+                ath6kl_warn("Failed to set probe request IE for scheduled scan: %d\n",
                             ret);
                 return ret;
         }
@@ -3188,6 +3297,18 @@ static int ath6kl_cfg80211_sscan_stop(struct wiphy *wiphy,
         return 0;
 }
 
+static int ath6kl_cfg80211_set_bitrate(struct wiphy *wiphy,
+                                       struct net_device *dev,
+                                       const u8 *addr,
+                                       const struct cfg80211_bitrate_mask *mask)
+{
+        struct ath6kl *ar = ath6kl_priv(dev);
+        struct ath6kl_vif *vif = netdev_priv(dev);
+
+        return ath6kl_wmi_set_bitrate_mask(ar->wmi, vif->fw_vif_idx,
+                                           mask);
+}
+
 static const struct ieee80211_txrx_stypes
 ath6kl_mgmt_stypes[NUM_NL80211_IFTYPES] = {
         [NL80211_IFTYPE_STATION] = {
@@ -3253,6 +3374,7 @@ static struct cfg80211_ops ath6kl_cfg80211_ops = {
         .mgmt_frame_register = ath6kl_mgmt_frame_register,
         .sched_scan_start = ath6kl_cfg80211_sscan_start,
         .sched_scan_stop = ath6kl_cfg80211_sscan_stop,
+        .set_bitrate_mask = ath6kl_cfg80211_set_bitrate,
 };
 
 void ath6kl_cfg80211_stop(struct ath6kl_vif *vif)
@@ -3380,7 +3502,8 @@ struct net_device *ath6kl_interface_add(struct ath6kl *ar, char *name,
         vif->listen_intvl_t = ATH6KL_DEFAULT_LISTEN_INTVAL;
         vif->bmiss_time_t = ATH6KL_DEFAULT_BMISS_TIME;
         vif->bg_scan_period = 0;
-        vif->htcap.ht_enable = true;
+        vif->htcap[IEEE80211_BAND_2GHZ].ht_enable = true;
+        vif->htcap[IEEE80211_BAND_5GHZ].ht_enable = true;
 
         memcpy(ndev->dev_addr, ar->mac_addr, ETH_ALEN);
         if (fw_vif_idx != 0)
@@ -3440,7 +3563,13 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
         }
 
         /* max num of ssids that can be probed during scanning */
-        wiphy->max_scan_ssids = MAX_PROBED_SSID_INDEX;
+        wiphy->max_scan_ssids = MAX_PROBED_SSIDS;
+
+        /* max num of ssids that can be matched after scan */
+        if (test_bit(ATH6KL_FW_CAPABILITY_SCHED_SCAN_MATCH_LIST,
+                     ar->fw_capabilities))
+                wiphy->max_match_sets = MAX_PROBED_SSIDS;
+
         wiphy->max_scan_ie_len = 1000; /* FIX: what is correct limit? */
         switch (ar->hw.cap) {
         case WMI_11AN_CAP:
@@ -3477,6 +3606,17 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
                 ath6kl_band_5ghz.ht_cap.cap = 0;
                 ath6kl_band_5ghz.ht_cap.ht_supported = false;
         }
+
+        if (ar->hw.flags & ATH6KL_HW_FLAG_64BIT_RATES) {
+                ath6kl_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+                ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+                ath6kl_band_2ghz.ht_cap.mcs.rx_mask[1] = 0xff;
+                ath6kl_band_5ghz.ht_cap.mcs.rx_mask[1] = 0xff;
+        } else {
+                ath6kl_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+                ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
+        }
+
         if (band_2gig)
                 wiphy->bands[IEEE80211_BAND_2GHZ] = &ath6kl_band_2ghz;
         if (band_5gig)
@@ -3499,7 +3639,7 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
         wiphy->wowlan.pattern_max_len = WOW_PATTERN_SIZE;
 #endif
 
-        wiphy->max_sched_scan_ssids = MAX_PROBED_SSID_INDEX;
+        wiphy->max_sched_scan_ssids = MAX_PROBED_SSIDS;
 
         ar->wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM |
                             WIPHY_FLAG_HAVE_AP_SME |
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.h b/drivers/net/wireless/ath/ath6kl/cfg80211.h
index 5ea8cbb79f4..b992046a1b0 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.h
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.h
@@ -62,5 +62,7 @@ void ath6kl_cfg80211_cleanup(struct ath6kl *ar);
 
 struct ath6kl *ath6kl_cfg80211_create(void);
 void ath6kl_cfg80211_destroy(struct ath6kl *ar);
+/* TODO: remove this once ath6kl_vif_cleanup() is moved to cfg80211.c */
+void ath6kl_cfg80211_sta_bmiss_enhance(struct ath6kl_vif *vif, bool enable);
 
 #endif /* ATH6KL_CFG80211_H */
diff --git a/drivers/net/wireless/ath/ath6kl/core.h b/drivers/net/wireless/ath/ath6kl/core.h
index 8443b2a4133..d38a31de344 100644
--- a/drivers/net/wireless/ath/ath6kl/core.h
+++ b/drivers/net/wireless/ath/ath6kl/core.h
@@ -100,6 +100,21 @@ enum ath6kl_fw_capability {
         /* Firmware has support to override rsn cap of rsn ie */
         ATH6KL_FW_CAPABILITY_RSN_CAP_OVERRIDE,
 
+        /*
+         * Multicast support in WOW and host awake mode.
+         * Allow all multicast in host awake mode.
+         * Apply multicast filter in WOW mode.
+         */
+        ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+
+        /* Firmware supports enhanced bmiss detection */
+        ATH6KL_FW_CAPABILITY_BMISS_ENHANCE,
+
+        /*
+         * FW supports matching of ssid in schedule scan
+         */
+        ATH6KL_FW_CAPABILITY_SCHED_SCAN_MATCH_LIST,
+
         /* this needs to be last */
         ATH6KL_FW_CAPABILITY_MAX,
 };
@@ -112,6 +127,10 @@ struct ath6kl_fw_ie {
         u8 data[0];
 };
 
+enum ath6kl_hw_flags {
+        ATH6KL_HW_FLAG_64BIT_RATES      = BIT(0),
+};
+
 #define ATH6KL_FW_API2_FILE "fw-2.bin"
 #define ATH6KL_FW_API3_FILE "fw-3.bin"
 
@@ -196,7 +215,7 @@ struct ath6kl_fw_ie {
 
 #define AGGR_NUM_OF_FREE_NETBUFS    16
 
-#define AGGR_RX_TIMEOUT     400 /* in ms */
+#define AGGR_RX_TIMEOUT     100 /* in ms */
 
 #define WMI_TIMEOUT (2 * HZ)
 
@@ -245,7 +264,6 @@ struct skb_hold_q {
 
 struct rxtid {
         bool aggr;
-        bool progress;
         bool timer_mon;
         u16 win_sz;
         u16 seq_next;
@@ -254,9 +272,15 @@ struct rxtid {
         struct sk_buff_head q;
 
         /*
-         * FIXME: No clue what this should protect. Apparently it should
-         * protect some of the fields above but they are also accessed
-         * without taking the lock.
+         * lock mainly protects seq_next and hold_q. Movement of seq_next
+         * needs to be protected between aggr_timeout() and
+         * aggr_process_recv_frm(). hold_q will be holding the pending
+         * reorder frames and it's access should also be protected.
+         * Some of the other fields like hold_q_sz, win_sz and aggr are
+         * initialized/reset when receiving addba/delba req, also while
+         * deleting aggr state all the pending buffers are flushed before
+         * resetting these fields, so there should not be any race in accessing
+         * these fields.
          */
         spinlock_t lock;
 };
@@ -541,7 +565,7 @@ struct ath6kl_vif {
         struct ath6kl_wep_key wep_key_list[WMI_MAX_KEY_INDEX + 1];
         struct ath6kl_key keys[WMI_MAX_KEY_INDEX + 1];
         struct aggr_info *aggr_cntxt;
-        struct ath6kl_htcap htcap;
+        struct ath6kl_htcap htcap[IEEE80211_NUM_BANDS];
 
         struct timer_list disconnect_timer;
         struct timer_list sched_scan_timer;
@@ -684,6 +708,8 @@ struct ath6kl {
                 u32 testscript_addr;
                 enum wmi_phy_cap cap;
 
+                u32 flags;
+
                 struct ath6kl_hw_fw {
                         const char *dir;
                         const char *otp;
diff --git a/drivers/net/wireless/ath/ath6kl/htc_mbox.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index 2798624d3a9..cd0e1ba410d 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_mbox.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -1309,7 +1309,7 @@ static int ath6kl_htc_rx_packet(struct htc_target *target,
         }
 
         ath6kl_dbg(ATH6KL_DBG_HTC,
-                   "htc rx 0x%p hdr x%x len %d mbox 0x%x\n",
+                   "htc rx 0x%p hdr 0x%x len %d mbox 0x%x\n",
                    packet, packet->info.rx.exp_hdr,
                    padded_len, dev->ar->mbox_info.htc_addr);
 
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 7eb0515f458..f90b5db741c 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -42,6 +42,7 @@ static const struct ath6kl_hw hw_list[] = {
                 .reserved_ram_size              = 6912,
                 .refclk_hz                      = 26000000,
                 .uarttx_pin                     = 8,
+                .flags                          = 0,
 
                 /* hw2.0 needs override address hardcoded */
                 .app_start_override_addr        = 0x944C00,
@@ -67,6 +68,7 @@ static const struct ath6kl_hw hw_list[] = {
                 .refclk_hz                      = 26000000,
                 .uarttx_pin                     = 8,
                 .testscript_addr                = 0x57ef74,
+                .flags                          = 0,
 
                 .fw = {
                         .dir            = AR6003_HW_2_1_1_FW_DIR,
@@ -91,6 +93,7 @@ static const struct ath6kl_hw hw_list[] = {
                 .board_addr                     = 0x433900,
                 .refclk_hz                      = 26000000,
                 .uarttx_pin                     = 11,
+                .flags                          = ATH6KL_HW_FLAG_64BIT_RATES,
 
                 .fw = {
                         .dir            = AR6004_HW_1_0_FW_DIR,
@@ -110,6 +113,7 @@ static const struct ath6kl_hw hw_list[] = {
                 .board_addr                     = 0x43d400,
                 .refclk_hz                      = 40000000,
                 .uarttx_pin                     = 11,
+                .flags                          = ATH6KL_HW_FLAG_64BIT_RATES,
 
                 .fw = {
                         .dir            = AR6004_HW_1_1_FW_DIR,
@@ -129,6 +133,7 @@ static const struct ath6kl_hw hw_list[] = {
                 .board_addr                     = 0x435c00,
                 .refclk_hz                      = 40000000,
                 .uarttx_pin                     = 11,
+                .flags                          = ATH6KL_HW_FLAG_64BIT_RATES,
 
                 .fw = {
                         .dir            = AR6004_HW_1_2_FW_DIR,
@@ -938,6 +943,14 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
                 }
 
                 switch (ie_id) {
+                case ATH6KL_FW_IE_FW_VERSION:
+                        strlcpy(ar->wiphy->fw_version, data,
+                                sizeof(ar->wiphy->fw_version));
+
+                        ath6kl_dbg(ATH6KL_DBG_BOOT,
+                                   "found fw version %s\n",
+                                    ar->wiphy->fw_version);
+                        break;
                 case ATH6KL_FW_IE_OTP_IMAGE:
                         ath6kl_dbg(ATH6KL_DBG_BOOT, "found otp image ie (%zd B)\n",
                                    ie_len);
@@ -991,9 +1004,6 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
                                    ar->hw.reserved_ram_size);
                         break;
                 case ATH6KL_FW_IE_CAPABILITIES:
-                        if (ie_len < DIV_ROUND_UP(ATH6KL_FW_CAPABILITY_MAX, 8))
-                                break;
-
                         ath6kl_dbg(ATH6KL_DBG_BOOT,
                                    "found firmware capabilities ie (%zd B)\n",
                                    ie_len);
@@ -1002,6 +1012,9 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
                                 index = i / 8;
                                 bit = i % 8;
 
+                                if (index == ie_len)
+                                        break;
+
                                 if (data[index] & (1 << bit))
                                         __set_bit(i, ar->fw_capabilities);
                         }
@@ -1392,6 +1405,12 @@ static int ath6kl_init_upload(struct ath6kl *ar)
             ar->version.target_ver == AR6003_HW_2_1_1_VERSION) {
                 ath6kl_err("temporary war to avoid sdio crc error\n");
 
+                param = 0x28;
+                address = GPIO_BASE_ADDRESS + GPIO_PIN9_ADDRESS;
+                status = ath6kl_bmi_reg_write(ar, address, param);
+                if (status)
+                        return status;
+
                 param = 0x20;
 
                 address = GPIO_BASE_ADDRESS + GPIO_PIN10_ADDRESS;
@@ -1659,6 +1678,9 @@ void ath6kl_cleanup_vif(struct ath6kl_vif *vif, bool wmi_ready)
                 cfg80211_scan_done(vif->scan_req, true);
                 vif->scan_req = NULL;
         }
+
+        /* need to clean up enhanced bmiss detection fw state */
+        ath6kl_cfg80211_sta_bmiss_enhance(vif, false);
 }
 
 void ath6kl_stop_txrx(struct ath6kl *ar)
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index b836f279511..c189e28e86a 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -554,20 +554,24 @@ void ath6kl_ready_event(void *devt, u8 *datap, u32 sw_ver, u32 abi_ver,
         struct ath6kl *ar = devt;
 
         memcpy(ar->mac_addr, datap, ETH_ALEN);
-        ath6kl_dbg(ATH6KL_DBG_TRC, "%s: mac addr = %pM\n",
-                   __func__, ar->mac_addr);
+
+        ath6kl_dbg(ATH6KL_DBG_BOOT,
+                   "ready event mac addr %pM sw_ver 0x%x abi_ver 0x%x cap 0x%x\n",
+                   ar->mac_addr, sw_ver, abi_ver, cap);
 
         ar->version.wlan_ver = sw_ver;
         ar->version.abi_ver = abi_ver;
         ar->hw.cap = cap;
 
-        snprintf(ar->wiphy->fw_version,
-                 sizeof(ar->wiphy->fw_version),
-                 "%u.%u.%u.%u",
-                 (ar->version.wlan_ver & 0xf0000000) >> 28,
-                 (ar->version.wlan_ver & 0x0f000000) >> 24,
-                 (ar->version.wlan_ver & 0x00ff0000) >> 16,
-                 (ar->version.wlan_ver & 0x0000ffff));
+        if (strlen(ar->wiphy->fw_version) == 0) {
+                snprintf(ar->wiphy->fw_version,
+                         sizeof(ar->wiphy->fw_version),
+                         "%u.%u.%u.%u",
+                         (ar->version.wlan_ver & 0xf0000000) >> 28,
+                         (ar->version.wlan_ver & 0x0f000000) >> 24,
+                         (ar->version.wlan_ver & 0x00ff0000) >> 16,
+                         (ar->version.wlan_ver & 0x0000ffff));
+        }
 
         /* indicate to the waiting thread that the ready event was received */
         set_bit(WMI_READY, &ar->flag);
@@ -1166,7 +1170,10 @@ static void ath6kl_set_multicast_list(struct net_device *ndev)
         else
                 clear_bit(NETDEV_MCAST_ALL_ON, &vif->flags);
 
-        mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
+        if (test_bit(ATH6KL_FW_CAPABILITY_WOW_MULTICAST_FILTER,
+                     vif->ar->fw_capabilities)) {
+                mc_all_on = mc_all_on || (vif->ar->state == ATH6KL_STATE_ON);
+        }
 
         if (!(ndev->flags & IFF_MULTICAST)) {
                 mc_all_on = false;
diff --git a/drivers/net/wireless/ath/ath6kl/target.h b/drivers/net/wireless/ath/ath6kl/target.h
index 78e0ef4567a..a98c12ba70c 100644
--- a/drivers/net/wireless/ath/ath6kl/target.h
+++ b/drivers/net/wireless/ath/ath6kl/target.h
@@ -45,6 +45,7 @@
 #define LPO_CAL_ENABLE_S                20
 #define LPO_CAL_ENABLE                  0x00100000
 
+#define GPIO_PIN9_ADDRESS               0x0000004c
 #define GPIO_PIN10_ADDRESS              0x00000050
 #define GPIO_PIN11_ADDRESS              0x00000054
 #define GPIO_PIN12_ADDRESS              0x00000058
diff --git a/drivers/net/wireless/ath/ath6kl/txrx.c b/drivers/net/wireless/ath/ath6kl/txrx.c
index 67206aedea6..7dfa0fd86d7 100644
--- a/drivers/net/wireless/ath/ath6kl/txrx.c
+++ b/drivers/net/wireless/ath/ath6kl/txrx.c
@@ -1036,6 +1036,7 @@ static void aggr_deque_frms(struct aggr_info_conn *agg_conn, u8 tid,
         rxtid = &agg_conn->rx_tid[tid];
         stats = &agg_conn->stat[tid];
 
+        spin_lock_bh(&rxtid->lock);
         idx = AGGR_WIN_IDX(rxtid->seq_next, rxtid->hold_q_sz);
 
         /*
@@ -1054,8 +1055,6 @@ static void aggr_deque_frms(struct aggr_info_conn *agg_conn, u8 tid,
         seq_end = seq_no ? seq_no : rxtid->seq_next;
         idx_end = AGGR_WIN_IDX(seq_end, rxtid->hold_q_sz);
 
-        spin_lock_bh(&rxtid->lock);
-
         do {
                 node = &rxtid->hold_q[idx];
                 if ((order == 1) && (!node->skb))
@@ -1127,11 +1126,13 @@ static bool aggr_process_recv_frm(struct aggr_info_conn *agg_conn, u8 tid,
                     ((end > extended_end) && (cur > extended_end) &&
                      (cur < end))) {
                         aggr_deque_frms(agg_conn, tid, 0, 0);
+                        spin_lock_bh(&rxtid->lock);
                         if (cur >= rxtid->hold_q_sz - 1)
                                 rxtid->seq_next = cur - (rxtid->hold_q_sz - 1);
                         else
                                 rxtid->seq_next = ATH6KL_MAX_SEQ_NO -
                                                   (rxtid->hold_q_sz - 2 - cur);
+                        spin_unlock_bh(&rxtid->lock);
                 } else {
                         /*
                          * Dequeue only those frames that are outside the
@@ -1185,25 +1186,25 @@ static bool aggr_process_recv_frm(struct aggr_info_conn *agg_conn, u8 tid,
         aggr_deque_frms(agg_conn, tid, 0, 1);
 
         if (agg_conn->timer_scheduled)
-                rxtid->progress = true;
-        else
-                for (idx = 0 ; idx < rxtid->hold_q_sz; idx++) {
-                        if (rxtid->hold_q[idx].skb) {
-                                /*
-                                 * There is a frame in the queue and no
-                                 * timer so start a timer to ensure that
-                                 * the frame doesn't remain stuck
-                                 * forever.
-                                 */
-                                agg_conn->timer_scheduled = true;
-                                mod_timer(&agg_conn->timer,
-                                          (jiffies +
-                                           HZ * (AGGR_RX_TIMEOUT) / 1000));
-                                rxtid->progress = false;
-                                rxtid->timer_mon = true;
-                                break;
-                        }
+                return is_queued;
+
+        spin_lock_bh(&rxtid->lock);
+        for (idx = 0 ; idx < rxtid->hold_q_sz; idx++) {
+                if (rxtid->hold_q[idx].skb) {
+                        /*
+                         * There is a frame in the queue and no
+                         * timer so start a timer to ensure that
+                         * the frame doesn't remain stuck
+                         * forever.
+                         */
+                        agg_conn->timer_scheduled = true;
+                        mod_timer(&agg_conn->timer,
+                                  (jiffies + (HZ * AGGR_RX_TIMEOUT) / 1000));
+                        rxtid->timer_mon = true;
+                        break;
                 }
+        }
+        spin_unlock_bh(&rxtid->lock);
 
         return is_queued;
 }
@@ -1608,7 +1609,7 @@ static void aggr_timeout(unsigned long arg)
                 rxtid = &aggr_conn->rx_tid[i];
                 stats = &aggr_conn->stat[i];
 
-                if (!rxtid->aggr || !rxtid->timer_mon || rxtid->progress)
+                if (!rxtid->aggr || !rxtid->timer_mon)
                         continue;
 
                 stats->num_timeouts++;
@@ -1626,14 +1627,15 @@ static void aggr_timeout(unsigned long arg)
                 rxtid = &aggr_conn->rx_tid[i];
 
                 if (rxtid->aggr && rxtid->hold_q) {
+                        spin_lock_bh(&rxtid->lock);
                         for (j = 0; j < rxtid->hold_q_sz; j++) {
                                 if (rxtid->hold_q[j].skb) {
                                         aggr_conn->timer_scheduled = true;
                                         rxtid->timer_mon = true;
-                                        rxtid->progress = false;
                                         break;
                                 }
                         }
+                        spin_unlock_bh(&rxtid->lock);
 
                         if (j >= rxtid->hold_q_sz)
                                 rxtid->timer_mon = false;
@@ -1660,7 +1662,6 @@ static void aggr_delete_tid_state(struct aggr_info_conn *aggr_conn, u8 tid)
                 aggr_deque_frms(aggr_conn, tid, 0, 0);
 
         rxtid->aggr = false;
-        rxtid->progress = false;
         rxtid->timer_mon = false;
         rxtid->win_sz = 0;
         rxtid->seq_next = 0;
@@ -1739,7 +1740,6 @@ void aggr_conn_init(struct ath6kl_vif *vif, struct aggr_info *aggr_info,
         for (i = 0; i < NUM_OF_TIDS; i++) {
                 rxtid = &aggr_conn->rx_tid[i];
                 rxtid->aggr = false;
-                rxtid->progress = false;
                 rxtid->timer_mon = false;
                 skb_queue_head_init(&rxtid->q);
                 spin_lock_init(&rxtid->lock);
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index ee8ec2394c2..a6caa673e8a 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -743,7 +743,6 @@ int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid)
                 return -ENOMEM;
 
         cmd = (struct roam_ctrl_cmd *) skb->data;
-        memset(cmd, 0, sizeof(*cmd));
 
         memcpy(cmd->info.bssid, bssid, ETH_ALEN);
         cmd->roam_ctrl = WMI_FORCE_ROAM;
@@ -753,6 +752,22 @@ int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid)
                                    NO_SYNC_WMIFLAG);
 }
 
+int ath6kl_wmi_ap_set_dtim_cmd(struct wmi *wmi, u8 if_idx, u32 dtim_period)
+{
+        struct sk_buff *skb;
+        struct set_dtim_cmd *cmd;
+
+        skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+        if (!skb)
+                return -ENOMEM;
+
+        cmd = (struct set_dtim_cmd *) skb->data;
+
+        cmd->dtim_period = cpu_to_le32(dtim_period);
+        return ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+                                   WMI_AP_SET_DTIM_CMDID, NO_SYNC_WMIFLAG);
+}
+
 int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode)
 {
         struct sk_buff *skb;
@@ -763,7 +778,6 @@ int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode)
                 return -ENOMEM;
 
         cmd = (struct roam_ctrl_cmd *) skb->data;
-        memset(cmd, 0, sizeof(*cmd));
 
         cmd->info.roam_mode = mode;
         cmd->roam_ctrl = WMI_SET_ROAM_MODE;
@@ -1995,7 +2009,7 @@ int ath6kl_wmi_probedssid_cmd(struct wmi *wmi, u8 if_idx, u8 index, u8 flag,
         struct wmi_probed_ssid_cmd *cmd;
         int ret;
 
-        if (index > MAX_PROBED_SSID_INDEX)
+        if (index >= MAX_PROBED_SSIDS)
                 return -EINVAL;
 
         if (ssid_len > sizeof(cmd->ssid))
@@ -2599,6 +2613,115 @@ static void ath6kl_wmi_relinquish_implicit_pstream_credits(struct wmi *wmi)
         spin_unlock_bh(&wmi->lock);
 }
 
+static int ath6kl_set_bitrate_mask64(struct wmi *wmi, u8 if_idx,
+                                     const struct cfg80211_bitrate_mask *mask)
+{
+        struct sk_buff *skb;
+        int ret, mode, band;
+        u64 mcsrate, ratemask[IEEE80211_NUM_BANDS];
+        struct wmi_set_tx_select_rates64_cmd *cmd;
+
+        memset(&ratemask, 0, sizeof(ratemask));
+        for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+                /* copy legacy rate mask */
+                ratemask[band] = mask->control[band].legacy;
+                if (band == IEEE80211_BAND_5GHZ)
+                        ratemask[band] =
+                                mask->control[band].legacy << 4;
+
+                /* copy mcs rate mask */
+                mcsrate = mask->control[band].mcs[1];
+                mcsrate <<= 8;
+                mcsrate |= mask->control[band].mcs[0];
+                ratemask[band] |= mcsrate << 12;
+                ratemask[band] |= mcsrate << 28;
+        }
+
+        ath6kl_dbg(ATH6KL_DBG_WMI,
+                   "Ratemask 64 bit: 2.4:%llx 5:%llx\n",
+                   ratemask[0], ratemask[1]);
+
+        skb = ath6kl_wmi_get_new_buf(sizeof(*cmd) * WMI_RATES_MODE_MAX);
+        if (!skb)
+                return -ENOMEM;
+
+        cmd = (struct wmi_set_tx_select_rates64_cmd *) skb->data;
+        for (mode = 0; mode < WMI_RATES_MODE_MAX; mode++) {
+                /* A mode operate in 5GHZ band */
+                if (mode == WMI_RATES_MODE_11A ||
+                    mode == WMI_RATES_MODE_11A_HT20 ||
+                    mode == WMI_RATES_MODE_11A_HT40)
+                        band = IEEE80211_BAND_5GHZ;
+                else
+                        band = IEEE80211_BAND_2GHZ;
+                cmd->ratemask[mode] = cpu_to_le64(ratemask[band]);
+        }
+
+        ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+                                  WMI_SET_TX_SELECT_RATES_CMDID,
+                                  NO_SYNC_WMIFLAG);
+        return ret;
+}
+
+static int ath6kl_set_bitrate_mask32(struct wmi *wmi, u8 if_idx,
+                                     const struct cfg80211_bitrate_mask *mask)
+{
+        struct sk_buff *skb;
+        int ret, mode, band;
+        u32 mcsrate, ratemask[IEEE80211_NUM_BANDS];
+        struct wmi_set_tx_select_rates32_cmd *cmd;
+
+        memset(&ratemask, 0, sizeof(ratemask));
+        for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+                /* copy legacy rate mask */
+                ratemask[band] = mask->control[band].legacy;
+                if (band == IEEE80211_BAND_5GHZ)
+                        ratemask[band] =
+                                mask->control[band].legacy << 4;
+
+                /* copy mcs rate mask */
+                mcsrate = mask->control[band].mcs[0];
+                ratemask[band] |= mcsrate << 12;
+                ratemask[band] |= mcsrate << 20;
+        }
+
+        ath6kl_dbg(ATH6KL_DBG_WMI,
+                   "Ratemask 32 bit: 2.4:%x 5:%x\n",
+                   ratemask[0], ratemask[1]);
+
+        skb = ath6kl_wmi_get_new_buf(sizeof(*cmd) * WMI_RATES_MODE_MAX);
+        if (!skb)
+                return -ENOMEM;
+
+        cmd = (struct wmi_set_tx_select_rates32_cmd *) skb->data;
+        for (mode = 0; mode < WMI_RATES_MODE_MAX; mode++) {
+                /* A mode operate in 5GHZ band */
+                if (mode == WMI_RATES_MODE_11A ||
+                    mode == WMI_RATES_MODE_11A_HT20 ||
+                    mode == WMI_RATES_MODE_11A_HT40)
+                        band = IEEE80211_BAND_5GHZ;
+                else
+                        band = IEEE80211_BAND_2GHZ;
+                cmd->ratemask[mode] = cpu_to_le32(ratemask[band]);
+        }
+
+        ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+                                  WMI_SET_TX_SELECT_RATES_CMDID,
+                                  NO_SYNC_WMIFLAG);
+        return ret;
+}
+
+int ath6kl_wmi_set_bitrate_mask(struct wmi *wmi, u8 if_idx,
+                                const struct cfg80211_bitrate_mask *mask)
+{
+        struct ath6kl *ar = wmi->parent_dev;
+
+        if (ar->hw.flags & ATH6KL_HW_FLAG_64BIT_RATES)
+                return ath6kl_set_bitrate_mask64(wmi, if_idx, mask);
+        else
+                return ath6kl_set_bitrate_mask32(wmi, if_idx, mask);
+}
+
 int ath6kl_wmi_set_host_sleep_mode_cmd(struct wmi *wmi, u8 if_idx,
                                        enum ath6kl_host_mode host_mode)
 {
@@ -2997,6 +3120,25 @@ int ath6kl_wmi_add_del_mcast_filter_cmd(struct wmi *wmi, u8 if_idx,
         return ret;
 }
 
+int ath6kl_wmi_sta_bmiss_enhance_cmd(struct wmi *wmi, u8 if_idx, bool enhance)
+{
+        struct sk_buff *skb;
+        struct wmi_sta_bmiss_enhance_cmd *cmd;
+        int ret;
+
+        skb = ath6kl_wmi_get_new_buf(sizeof(*cmd));
+        if (!skb)
+                return -ENOMEM;
+
+        cmd = (struct wmi_sta_bmiss_enhance_cmd *) skb->data;
+        cmd->enable = enhance ? 1 : 0;
+
+        ret = ath6kl_wmi_cmd_send(wmi, if_idx, skb,
+                                  WMI_STA_BMISS_ENHANCE_CMDID,
+                                  NO_SYNC_WMIFLAG);
+        return ret;
+}
+
 s32 ath6kl_wmi_get_rate(s8 rate_index)
 {
         if (rate_index == RATE_AUTO)
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.h b/drivers/net/wireless/ath/ath6kl/wmi.h
index 9076bec3a2b..43339aca585 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.h
+++ b/drivers/net/wireless/ath/ath6kl/wmi.h
@@ -624,6 +624,10 @@ enum wmi_cmd_id {
         WMI_SEND_MGMT_CMDID,
         WMI_BEGIN_SCAN_CMDID,
 
+        WMI_SET_BLACK_LIST,
+        WMI_SET_MCASTRATE,
+
+        WMI_STA_BMISS_ENHANCE_CMDID,
 };
 
 enum wmi_mgmt_frame_type {
@@ -960,6 +964,9 @@ enum wmi_bss_filter {
         /* beacons matching probed ssid */
         PROBED_SSID_FILTER,
 
+        /* beacons matching matched ssid */
+        MATCHED_SSID_FILTER,
+
         /* marker only */
         LAST_BSS_FILTER,
 };
@@ -978,7 +985,7 @@ struct wmi_bss_filter_cmd {
 } __packed;
 
 /* WMI_SET_PROBED_SSID_CMDID */
-#define MAX_PROBED_SSID_INDEX   9
+#define MAX_PROBED_SSIDS   16
 
 enum wmi_ssid_flag {
         /* disables entry */
@@ -989,10 +996,13 @@ enum wmi_ssid_flag {
 
         /* probes for any ssid */
         ANY_SSID_FLAG = 0x02,
+
+        /* match for ssid */
+        MATCH_SSID_FLAG = 0x08,
 };
 
 struct wmi_probed_ssid_cmd {
-        /* 0 to MAX_PROBED_SSID_INDEX */
+        /* 0 to MAX_PROBED_SSIDS - 1 */
         u8 entry_index;
 
         /* see, enum wmi_ssid_flg */
@@ -1017,6 +1027,11 @@ struct wmi_bmiss_time_cmd {
         __le16 num_beacons;
 };
 
+/* WMI_STA_ENHANCE_BMISS_CMDID */
+struct wmi_sta_bmiss_enhance_cmd {
+        u8 enable;
+} __packed;
+
 /* WMI_SET_POWER_MODE_CMDID */
 enum wmi_power_mode {
         REC_POWER = 0x01,
@@ -1048,6 +1063,36 @@ struct wmi_power_params_cmd {
         __le16 ps_fail_event_policy;
 } __packed;
 
+/*
+ * Ratemask for below modes should be passed
+ * to WMI_SET_TX_SELECT_RATES_CMDID.
+ * AR6003 has 32 bit mask for each modes.
+ * First 12 bits for legacy rates, 13 to 20
+ * bits for HT 20 rates and 21 to 28 bits for
+ * HT 40 rates
+ */
+enum wmi_mode_phy {
+        WMI_RATES_MODE_11A = 0,
+        WMI_RATES_MODE_11G,
+        WMI_RATES_MODE_11B,
+        WMI_RATES_MODE_11GONLY,
+        WMI_RATES_MODE_11A_HT20,
+        WMI_RATES_MODE_11G_HT20,
+        WMI_RATES_MODE_11A_HT40,
+        WMI_RATES_MODE_11G_HT40,
+        WMI_RATES_MODE_MAX
+};
+
+/* WMI_SET_TX_SELECT_RATES_CMDID */
+struct wmi_set_tx_select_rates32_cmd {
+        __le32 ratemask[WMI_RATES_MODE_MAX];
+} __packed;
+
+/* WMI_SET_TX_SELECT_RATES_CMDID */
+struct wmi_set_tx_select_rates64_cmd {
+        __le64 ratemask[WMI_RATES_MODE_MAX];
+} __packed;
+
 /* WMI_SET_DISC_TIMEOUT_CMDID */
 struct wmi_disc_timeout_cmd {
         /* seconds */
@@ -1572,6 +1617,10 @@ struct roam_ctrl_cmd {
         u8 roam_ctrl;
 } __packed;
 
+struct set_dtim_cmd {
+        __le32 dtim_period;
+} __packed;
+
 /* BSS INFO HDR version 2.0 */
 struct wmi_bss_info_hdr2 {
         __le16 ch; /* frequency in MHz */
@@ -2532,6 +2581,8 @@ int ath6kl_wmi_set_ip_cmd(struct wmi *wmi, u8 if_idx,
                           __be32 ips0, __be32 ips1);
 int ath6kl_wmi_set_host_sleep_mode_cmd(struct wmi *wmi, u8 if_idx,
                                        enum ath6kl_host_mode host_mode);
+int ath6kl_wmi_set_bitrate_mask(struct wmi *wmi, u8 if_idx,
+                                const struct cfg80211_bitrate_mask *mask);
 int ath6kl_wmi_set_wow_mode_cmd(struct wmi *wmi, u8 if_idx,
                                 enum ath6kl_wow_mode wow_mode,
                                 u32 filter, u16 host_req_delay);
@@ -2542,11 +2593,14 @@ int ath6kl_wmi_add_wow_pattern_cmd(struct wmi *wmi, u8 if_idx,
 int ath6kl_wmi_del_wow_pattern_cmd(struct wmi *wmi, u8 if_idx,
                                    u16 list_id, u16 filter_id);
 int ath6kl_wmi_set_roam_lrssi_cmd(struct wmi *wmi, u8 lrssi);
+int ath6kl_wmi_ap_set_dtim_cmd(struct wmi *wmi, u8 if_idx, u32 dtim_period);
 int ath6kl_wmi_force_roam_cmd(struct wmi *wmi, const u8 *bssid);
 int ath6kl_wmi_set_roam_mode_cmd(struct wmi *wmi, enum wmi_roam_mode mode);
 int ath6kl_wmi_mcast_filter_cmd(struct wmi *wmi, u8 if_idx, bool mc_all_on);
 int ath6kl_wmi_add_del_mcast_filter_cmd(struct wmi *wmi, u8 if_idx,
                                         u8 *filter, bool add_filter);
+int ath6kl_wmi_sta_bmiss_enhance_cmd(struct wmi *wmi, u8 if_idx, bool enable);
+
 /* AP mode uAPSD */
 int ath6kl_wmi_ap_set_apsd(struct wmi *wmi, u8 if_idx, u8 enable);
 
diff --git a/drivers/net/wireless/ath/ath9k/ani.c b/drivers/net/wireless/ath/ath9k/ani.c
index b4c77f9d747..7ebc3465f22 100644
--- a/drivers/net/wireless/ath/ath9k/ani.c
+++ b/drivers/net/wireless/ath/ath9k/ani.c
@@ -104,11 +104,6 @@ static const struct ani_cck_level_entry cck_level_table[] = {
 #define ATH9K_ANI_CCK_DEF_LEVEL \
         2 /* default level - matches the INI settings */
 
-static bool use_new_ani(struct ath_hw *ah)
-{
-        return AR_SREV_9300_20_OR_LATER(ah) || modparam_force_new_ani;
-}
-
 static void ath9k_hw_update_mibstats(struct ath_hw *ah,
                                      struct ath9k_mib_stats *stats)
 {
@@ -122,8 +117,6 @@ static void ath9k_hw_update_mibstats(struct ath_hw *ah,
 static void ath9k_ani_restart(struct ath_hw *ah)
 {
         struct ar5416AniState *aniState;
-        struct ath_common *common = ath9k_hw_common(ah);
-        u32 ofdm_base = 0, cck_base = 0;
 
         if (!DO_ANI(ah))
                 return;
@@ -131,18 +124,10 @@ static void ath9k_ani_restart(struct ath_hw *ah)
         aniState = &ah->curchan->ani;
         aniState->listenTime = 0;
 
-        if (!use_new_ani(ah)) {
-                ofdm_base = AR_PHY_COUNTMAX - ah->config.ofdm_trig_high;
-                cck_base = AR_PHY_COUNTMAX - ah->config.cck_trig_high;
-        }
-
-        ath_dbg(common, ANI, "Writing ofdmbase=%u   cckbase=%u\n",
-                ofdm_base, cck_base);
-
         ENABLE_REGWRITE_BUFFER(ah);
 
-        REG_WRITE(ah, AR_PHY_ERR_1, ofdm_base);
-        REG_WRITE(ah, AR_PHY_ERR_2, cck_base);
+        REG_WRITE(ah, AR_PHY_ERR_1, 0);
+        REG_WRITE(ah, AR_PHY_ERR_2, 0);
         REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
         REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
 
@@ -154,110 +139,6 @@ static void ath9k_ani_restart(struct ath_hw *ah)
         aniState->cckPhyErrCount = 0;
 }
 
-static void ath9k_hw_ani_ofdm_err_trigger_old(struct ath_hw *ah)
-{
-        struct ieee80211_conf *conf = &ath9k_hw_common(ah)->hw->conf;
-        struct ar5416AniState *aniState;
-        int32_t rssi;
-
-        aniState = &ah->curchan->ani;
-
-        if (aniState->noiseImmunityLevel < HAL_NOISE_IMMUNE_MAX) {
-                if (ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-                                         aniState->noiseImmunityLevel + 1)) {
-                        return;
-                }
-        }
-
-        if (aniState->spurImmunityLevel < HAL_SPUR_IMMUNE_MAX) {
-                if (ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-                                         aniState->spurImmunityLevel + 1)) {
-                        return;
-                }
-        }
-
-        if (ah->opmode == NL80211_IFTYPE_AP) {
-                if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-                        ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                             aniState->firstepLevel + 1);
-                }
-                return;
-        }
-        rssi = BEACON_RSSI(ah);
-        if (rssi > aniState->rssiThrHigh) {
-                if (!aniState->ofdmWeakSigDetectOff) {
-                        if (ath9k_hw_ani_control(ah,
-                                         ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                         false)) {
-                                ath9k_hw_ani_control(ah,
-                                        ATH9K_ANI_SPUR_IMMUNITY_LEVEL, 0);
-                                return;
-                        }
-                }
-                if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-                        ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                             aniState->firstepLevel + 1);
-                        return;
-                }
-        } else if (rssi > aniState->rssiThrLow) {
-                if (aniState->ofdmWeakSigDetectOff)
-                        ath9k_hw_ani_control(ah,
-                                     ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                     true);
-                if (aniState->firstepLevel < HAL_FIRST_STEP_MAX)
-                        ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                             aniState->firstepLevel + 1);
-                return;
-        } else {
-                if ((conf->channel->band == IEEE80211_BAND_2GHZ) &&
-                    !conf_is_ht(conf)) {
-                        if (!aniState->ofdmWeakSigDetectOff)
-                                ath9k_hw_ani_control(ah,
-                                     ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                     false);
-                        if (aniState->firstepLevel > 0)
-                                ath9k_hw_ani_control(ah,
-                                             ATH9K_ANI_FIRSTEP_LEVEL, 0);
-                        return;
-                }
-        }
-}
-
-static void ath9k_hw_ani_cck_err_trigger_old(struct ath_hw *ah)
-{
-        struct ieee80211_conf *conf = &ath9k_hw_common(ah)->hw->conf;
-        struct ar5416AniState *aniState;
-        int32_t rssi;
-
-        aniState = &ah->curchan->ani;
-        if (aniState->noiseImmunityLevel < HAL_NOISE_IMMUNE_MAX) {
-                if (ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-                                         aniState->noiseImmunityLevel + 1)) {
-                        return;
-                }
-        }
-        if (ah->opmode == NL80211_IFTYPE_AP) {
-                if (aniState->firstepLevel < HAL_FIRST_STEP_MAX) {
-                        ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                             aniState->firstepLevel + 1);
-                }
-                return;
-        }
-        rssi = BEACON_RSSI(ah);
-        if (rssi > aniState->rssiThrLow) {
-                if (aniState->firstepLevel < HAL_FIRST_STEP_MAX)
-                        ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                             aniState->firstepLevel + 1);
-        } else {
-                if ((conf->channel->band == IEEE80211_BAND_2GHZ) &&
-                    !conf_is_ht(conf)) {
-                        if (aniState->firstepLevel > 0)
-                                ath9k_hw_ani_control(ah,
-                                             ATH9K_ANI_FIRSTEP_LEVEL, 0);
-                }
-        }
-}
-
 /* Adjust the OFDM Noise Immunity Level */
 static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
 {
@@ -265,18 +146,15 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
         struct ath_common *common = ath9k_hw_common(ah);
         const struct ani_ofdm_level_entry *entry_ofdm;
         const struct ani_cck_level_entry *entry_cck;
-
-        aniState->noiseFloor = BEACON_RSSI(ah);
+        bool weak_sig;
 
         ath_dbg(common, ANI, "**** ofdmlevel %d=>%d, rssi=%d[lo=%d hi=%d]\n",
                 aniState->ofdmNoiseImmunityLevel,
-                immunityLevel, aniState->noiseFloor,
+                immunityLevel, BEACON_RSSI(ah),
                 aniState->rssiThrLow, aniState->rssiThrHigh);
 
         if (aniState->update_ani)
-                aniState->ofdmNoiseImmunityLevel =
-                        (immunityLevel > ATH9K_ANI_OFDM_DEF_LEVEL) ?
-                        immunityLevel : ATH9K_ANI_OFDM_DEF_LEVEL;
+                aniState->ofdmNoiseImmunityLevel = immunityLevel;
 
         entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
         entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -292,12 +170,22 @@ static void ath9k_hw_set_ofdm_nil(struct ath_hw *ah, u8 immunityLevel)
                                      ATH9K_ANI_FIRSTEP_LEVEL,
                                      entry_ofdm->fir_step_level);
 
-        if ((aniState->noiseFloor >= aniState->rssiThrHigh) &&
-            (!aniState->ofdmWeakSigDetectOff !=
-             entry_ofdm->ofdm_weak_signal_on)) {
+        weak_sig = entry_ofdm->ofdm_weak_signal_on;
+        if (ah->opmode == NL80211_IFTYPE_STATION &&
+            BEACON_RSSI(ah) <= aniState->rssiThrHigh)
+                weak_sig = true;
+
+        if (aniState->ofdmWeakSigDetect != weak_sig)
                         ath9k_hw_ani_control(ah,
                                 ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
                                 entry_ofdm->ofdm_weak_signal_on);
+
+        if (aniState->ofdmNoiseImmunityLevel >= ATH9K_ANI_OFDM_DEF_LEVEL) {
+                ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
+                ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI;
+        } else {
+                ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI;
+                ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
         }
 }
 
@@ -308,11 +196,6 @@ static void ath9k_hw_ani_ofdm_err_trigger(struct ath_hw *ah)
         if (!DO_ANI(ah))
                 return;
 
-        if (!use_new_ani(ah)) {
-                ath9k_hw_ani_ofdm_err_trigger_old(ah);
-                return;
-        }
-
         aniState = &ah->curchan->ani;
 
         if (aniState->ofdmNoiseImmunityLevel < ATH9K_ANI_OFDM_MAX_LEVEL)
@@ -329,22 +212,18 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel)
         const struct ani_ofdm_level_entry *entry_ofdm;
         const struct ani_cck_level_entry *entry_cck;
 
-        aniState->noiseFloor = BEACON_RSSI(ah);
         ath_dbg(common, ANI, "**** ccklevel %d=>%d, rssi=%d[lo=%d hi=%d]\n",
                 aniState->cckNoiseImmunityLevel, immunityLevel,
-                aniState->noiseFloor, aniState->rssiThrLow,
+                BEACON_RSSI(ah), aniState->rssiThrLow,
                 aniState->rssiThrHigh);
 
-        if ((ah->opmode == NL80211_IFTYPE_STATION ||
-             ah->opmode == NL80211_IFTYPE_ADHOC) &&
-            aniState->noiseFloor <= aniState->rssiThrLow &&
+        if (ah->opmode == NL80211_IFTYPE_STATION &&
+            BEACON_RSSI(ah) <= aniState->rssiThrLow &&
             immunityLevel > ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI)
                 immunityLevel = ATH9K_ANI_CCK_MAX_LEVEL_LOW_RSSI;
 
         if (aniState->update_ani)
-                aniState->cckNoiseImmunityLevel =
-                        (immunityLevel > ATH9K_ANI_CCK_DEF_LEVEL) ?
-                        immunityLevel : ATH9K_ANI_CCK_DEF_LEVEL;
+                aniState->cckNoiseImmunityLevel = immunityLevel;
 
         entry_ofdm = &ofdm_level_table[aniState->ofdmNoiseImmunityLevel];
         entry_cck = &cck_level_table[aniState->cckNoiseImmunityLevel];
@@ -359,7 +238,7 @@ static void ath9k_hw_set_cck_nil(struct ath_hw *ah, u_int8_t immunityLevel)
         if (!AR_SREV_9300_20_OR_LATER(ah) || AR_SREV_9485(ah))
                 return;
 
-        if (aniState->mrcCCKOff == entry_cck->mrc_cck_on)
+        if (aniState->mrcCCK != entry_cck->mrc_cck_on)
                 ath9k_hw_ani_control(ah,
                                      ATH9K_ANI_MRC_CCK,
                                      entry_cck->mrc_cck_on);
@@ -372,70 +251,12 @@ static void ath9k_hw_ani_cck_err_trigger(struct ath_hw *ah)
         if (!DO_ANI(ah))
                 return;
 
-        if (!use_new_ani(ah)) {
-                ath9k_hw_ani_cck_err_trigger_old(ah);
-                return;
-        }
-
         aniState = &ah->curchan->ani;
 
         if (aniState->cckNoiseImmunityLevel < ATH9K_ANI_CCK_MAX_LEVEL)
                 ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel + 1);
 }
 
-static void ath9k_hw_ani_lower_immunity_old(struct ath_hw *ah)
-{
-        struct ar5416AniState *aniState;
-        int32_t rssi;
-
-        aniState = &ah->curchan->ani;
-
-        if (ah->opmode == NL80211_IFTYPE_AP) {
-                if (aniState->firstepLevel > 0) {
-                        if (ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                                 aniState->firstepLevel - 1))
-                                return;
-                }
-        } else {
-                rssi = BEACON_RSSI(ah);
-                if (rssi > aniState->rssiThrHigh) {
-                        /* XXX: Handle me */
-                } else if (rssi > aniState->rssiThrLow) {
-                        if (aniState->ofdmWeakSigDetectOff) {
-                                if (ath9k_hw_ani_control(ah,
-                                         ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                         true))
-                                        return;
-                        }
-                        if (aniState->firstepLevel > 0) {
-                                if (ath9k_hw_ani_control(ah,
-                                         ATH9K_ANI_FIRSTEP_LEVEL,
-                                         aniState->firstepLevel - 1))
-                                        return;
-                        }
-                } else {
-                        if (aniState->firstepLevel > 0) {
-                                if (ath9k_hw_ani_control(ah,
-                                         ATH9K_ANI_FIRSTEP_LEVEL,
-                                         aniState->firstepLevel - 1))
-                                        return;
-                        }
-                }
-        }
-
-        if (aniState->spurImmunityLevel > 0) {
-                if (ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-                                         aniState->spurImmunityLevel - 1))
-                        return;
-        }
-
-        if (aniState->noiseImmunityLevel > 0) {
-                ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-                                     aniState->noiseImmunityLevel - 1);
-                return;
-        }
-}
-
 /*
  * only lower either OFDM or CCK errors per turn
  * we lower the other one next time
@@ -446,11 +267,6 @@ static void ath9k_hw_ani_lower_immunity(struct ath_hw *ah)
 
         aniState = &ah->curchan->ani;
 
-        if (!use_new_ani(ah)) {
-                ath9k_hw_ani_lower_immunity_old(ah);
-                return;
-        }
-
         /* lower OFDM noise immunity */
         if (aniState->ofdmNoiseImmunityLevel > 0 &&
             (aniState->ofdmsTurn || aniState->cckNoiseImmunityLevel == 0)) {
@@ -463,72 +279,6 @@ static void ath9k_hw_ani_lower_immunity(struct ath_hw *ah)
                 ath9k_hw_set_cck_nil(ah, aniState->cckNoiseImmunityLevel - 1);
 }
 
-static void ath9k_ani_reset_old(struct ath_hw *ah, bool is_scanning)
-{
-        struct ar5416AniState *aniState;
-        struct ath9k_channel *chan = ah->curchan;
-        struct ath_common *common = ath9k_hw_common(ah);
-
-        if (!DO_ANI(ah))
-                return;
-
-        aniState = &ah->curchan->ani;
-
-        if (ah->opmode != NL80211_IFTYPE_STATION
-            && ah->opmode != NL80211_IFTYPE_ADHOC) {
-                ath_dbg(common, ANI, "Reset ANI state opmode %u\n", ah->opmode);
-                ah->stats.ast_ani_reset++;
-
-                if (ah->opmode == NL80211_IFTYPE_AP) {
-                        /*
-                         * ath9k_hw_ani_control() will only process items set on
-                         * ah->ani_function
-                         */
-                        if (IS_CHAN_2GHZ(chan))
-                                ah->ani_function = (ATH9K_ANI_SPUR_IMMUNITY_LEVEL |
-                                                    ATH9K_ANI_FIRSTEP_LEVEL);
-                        else
-                                ah->ani_function = 0;
-                }
-
-                ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL, 0);
-                ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL, 0);
-                ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL, 0);
-                ath9k_hw_ani_control(ah, ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                     !ATH9K_ANI_USE_OFDM_WEAK_SIG);
-                ath9k_hw_ani_control(ah, ATH9K_ANI_CCK_WEAK_SIGNAL_THR,
-                                     ATH9K_ANI_CCK_WEAK_SIG_THR);
-
-                ath9k_ani_restart(ah);
-                return;
-        }
-
-        if (aniState->noiseImmunityLevel != 0)
-                ath9k_hw_ani_control(ah, ATH9K_ANI_NOISE_IMMUNITY_LEVEL,
-                                     aniState->noiseImmunityLevel);
-        if (aniState->spurImmunityLevel != 0)
-                ath9k_hw_ani_control(ah, ATH9K_ANI_SPUR_IMMUNITY_LEVEL,
-                                     aniState->spurImmunityLevel);
-        if (aniState->ofdmWeakSigDetectOff)
-                ath9k_hw_ani_control(ah, ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION,
-                                     !aniState->ofdmWeakSigDetectOff);
-        if (aniState->cckWeakSigThreshold)
-                ath9k_hw_ani_control(ah, ATH9K_ANI_CCK_WEAK_SIGNAL_THR,
-                                     aniState->cckWeakSigThreshold);
-        if (aniState->firstepLevel != 0)
-                ath9k_hw_ani_control(ah, ATH9K_ANI_FIRSTEP_LEVEL,
-                                     aniState->firstepLevel);
-
-        ath9k_ani_restart(ah);
-
-        ENABLE_REGWRITE_BUFFER(ah);
-
-        REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
-        REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
-
-        REGWRITE_BUFFER_FLUSH(ah);
-}
-
 /*
  * Restore the ANI parameters in the HAL and reset the statistics.
  * This routine should be called for every hardware reset and for
@@ -539,13 +289,11 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
         struct ar5416AniState *aniState = &ah->curchan->ani;
         struct ath9k_channel *chan = ah->curchan;
         struct ath_common *common = ath9k_hw_common(ah);
+        int ofdm_nil, cck_nil;
 
         if (!DO_ANI(ah))
                 return;
 
-        if (!use_new_ani(ah))
-                return ath9k_ani_reset_old(ah, is_scanning);
-
         BUG_ON(aniState == NULL);
         ah->stats.ast_ani_reset++;
 
@@ -563,6 +311,11 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
         /* always allow mode (on/off) to be controlled */
         ah->ani_function |= ATH9K_ANI_MODE;
 
+        ofdm_nil = max_t(int, ATH9K_ANI_OFDM_DEF_LEVEL,
+                         aniState->ofdmNoiseImmunityLevel);
+        cck_nil = max_t(int, ATH9K_ANI_CCK_DEF_LEVEL,
+                         aniState->cckNoiseImmunityLevel);
+
         if (is_scanning ||
             (ah->opmode != NL80211_IFTYPE_STATION &&
              ah->opmode != NL80211_IFTYPE_ADHOC)) {
@@ -586,8 +339,8 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
                                 aniState->cckNoiseImmunityLevel);
 
                         aniState->update_ani = false;
-                        ath9k_hw_set_ofdm_nil(ah, ATH9K_ANI_OFDM_DEF_LEVEL);
-                        ath9k_hw_set_cck_nil(ah, ATH9K_ANI_CCK_DEF_LEVEL);
+                        ofdm_nil = ATH9K_ANI_OFDM_DEF_LEVEL;
+                        cck_nil = ATH9K_ANI_CCK_DEF_LEVEL;
                 }
         } else {
                 /*
@@ -603,11 +356,9 @@ void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning)
                         aniState->cckNoiseImmunityLevel);
 
                         aniState->update_ani = true;
-                        ath9k_hw_set_ofdm_nil(ah,
-                                              aniState->ofdmNoiseImmunityLevel);
-                        ath9k_hw_set_cck_nil(ah,
-                                             aniState->cckNoiseImmunityLevel);
         }
+        ath9k_hw_set_ofdm_nil(ah, ofdm_nil);
+        ath9k_hw_set_cck_nil(ah, cck_nil);
 
         /*
          * enable phy counters if hw supports or if not, enable phy
@@ -627,9 +378,6 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
 {
         struct ath_common *common = ath9k_hw_common(ah);
         struct ar5416AniState *aniState = &ah->curchan->ani;
-        u32 ofdm_base = 0;
-        u32 cck_base = 0;
-        u32 ofdmPhyErrCnt, cckPhyErrCnt;
         u32 phyCnt1, phyCnt2;
         int32_t listenTime;
 
@@ -642,11 +390,6 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
                 return false;
         }
 
-        if (!use_new_ani(ah)) {
-                ofdm_base = AR_PHY_COUNTMAX - ah->config.ofdm_trig_high;
-                cck_base = AR_PHY_COUNTMAX - ah->config.cck_trig_high;
-        }
-
         aniState->listenTime += listenTime;
 
         ath9k_hw_update_mibstats(ah, &ah->ah_mibStats);
@@ -654,35 +397,12 @@ static bool ath9k_hw_ani_read_counters(struct ath_hw *ah)
         phyCnt1 = REG_READ(ah, AR_PHY_ERR_1);
         phyCnt2 = REG_READ(ah, AR_PHY_ERR_2);
 
-        if (!use_new_ani(ah) && (phyCnt1 < ofdm_base || phyCnt2 < cck_base)) {
-                if (phyCnt1 < ofdm_base) {
-                        ath_dbg(common, ANI,
-                                "phyCnt1 0x%x, resetting counter value to 0x%x\n",
-                                phyCnt1, ofdm_base);
-                        REG_WRITE(ah, AR_PHY_ERR_1, ofdm_base);
-                        REG_WRITE(ah, AR_PHY_ERR_MASK_1,
-                                  AR_PHY_ERR_OFDM_TIMING);
-                }
-                if (phyCnt2 < cck_base) {
-                        ath_dbg(common, ANI,
-                                "phyCnt2 0x%x, resetting counter value to 0x%x\n",
-                                phyCnt2, cck_base);
-                        REG_WRITE(ah, AR_PHY_ERR_2, cck_base);
-                        REG_WRITE(ah, AR_PHY_ERR_MASK_2,
-                                  AR_PHY_ERR_CCK_TIMING);
-                }
-                return false;
-        }
+        ah->stats.ast_ani_ofdmerrs += phyCnt1 - aniState->ofdmPhyErrCount;
+        aniState->ofdmPhyErrCount = phyCnt1;
 
-        ofdmPhyErrCnt = phyCnt1 - ofdm_base;
-        ah->stats.ast_ani_ofdmerrs +=
-                ofdmPhyErrCnt - aniState->ofdmPhyErrCount;
-        aniState->ofdmPhyErrCount = ofdmPhyErrCnt;
+        ah->stats.ast_ani_cckerrs += phyCnt2 - aniState->cckPhyErrCount;
+        aniState->cckPhyErrCount = phyCnt2;
 
-        cckPhyErrCnt = phyCnt2 - cck_base;
-        ah->stats.ast_ani_cckerrs +=
-                cckPhyErrCnt - aniState->cckPhyErrCount;
-        aniState->cckPhyErrCount = cckPhyErrCnt;
         return true;
 }
 
@@ -716,21 +436,10 @@ void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan)
 
         if (aniState->listenTime > ah->aniperiod) {
                 if (cckPhyErrRate < ah->config.cck_trig_low &&
-                    ((ofdmPhyErrRate < ah->config.ofdm_trig_low &&
-                      aniState->ofdmNoiseImmunityLevel <
-                      ATH9K_ANI_OFDM_DEF_LEVEL) ||
-                     (ofdmPhyErrRate < ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI &&
-                      aniState->ofdmNoiseImmunityLevel >=
-                      ATH9K_ANI_OFDM_DEF_LEVEL))) {
+                    ofdmPhyErrRate < ah->config.ofdm_trig_low) {
                         ath9k_hw_ani_lower_immunity(ah);
                         aniState->ofdmsTurn = !aniState->ofdmsTurn;
-                } else if ((ofdmPhyErrRate > ah->config.ofdm_trig_high &&
-                            aniState->ofdmNoiseImmunityLevel >=
-                            ATH9K_ANI_OFDM_DEF_LEVEL) ||
-                           (ofdmPhyErrRate >
-                            ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI &&
-                            aniState->ofdmNoiseImmunityLevel <
-                            ATH9K_ANI_OFDM_DEF_LEVEL)) {
+                } else if (ofdmPhyErrRate > ah->config.ofdm_trig_high) {
                         ath9k_hw_ani_ofdm_err_trigger(ah);
                         aniState->ofdmsTurn = false;
                 } else if (cckPhyErrRate > ah->config.cck_trig_high) {
@@ -778,49 +487,6 @@ void ath9k_hw_disable_mib_counters(struct ath_hw *ah)
 }
 EXPORT_SYMBOL(ath9k_hw_disable_mib_counters);
 
-/*
- * Process a MIB interrupt.  We may potentially be invoked because
- * any of the MIB counters overflow/trigger so don't assume we're
- * here because a PHY error counter triggered.
- */
-void ath9k_hw_proc_mib_event(struct ath_hw *ah)
-{
-        u32 phyCnt1, phyCnt2;
-
-        /* Reset these counters regardless */
-        REG_WRITE(ah, AR_FILT_OFDM, 0);
-        REG_WRITE(ah, AR_FILT_CCK, 0);
-        if (!(REG_READ(ah, AR_SLP_MIB_CTRL) & AR_SLP_MIB_PENDING))
-                REG_WRITE(ah, AR_SLP_MIB_CTRL, AR_SLP_MIB_CLEAR);
-
-        /* Clear the mib counters and save them in the stats */
-        ath9k_hw_update_mibstats(ah, &ah->ah_mibStats);
-
-        if (!DO_ANI(ah)) {
-                /*
-                 * We must always clear the interrupt cause by
-                 * resetting the phy error regs.
-                 */
-                REG_WRITE(ah, AR_PHY_ERR_1, 0);
-                REG_WRITE(ah, AR_PHY_ERR_2, 0);
-                return;
-        }
-
-        /* NB: these are not reset-on-read */
-        phyCnt1 = REG_READ(ah, AR_PHY_ERR_1);
-        phyCnt2 = REG_READ(ah, AR_PHY_ERR_2);
-        if (((phyCnt1 & AR_MIBCNT_INTRMASK) == AR_MIBCNT_INTRMASK) ||
-            ((phyCnt2 & AR_MIBCNT_INTRMASK) == AR_MIBCNT_INTRMASK)) {
-
-                if (!use_new_ani(ah))
-                        ath9k_hw_ani_read_counters(ah);
-
-                /* NB: always restart to insure the h/w counters are reset */
-                ath9k_ani_restart(ah);
-        }
-}
-EXPORT_SYMBOL(ath9k_hw_proc_mib_event);
-
 void ath9k_hw_ani_setup(struct ath_hw *ah)
 {
         int i;
@@ -845,50 +511,27 @@ void ath9k_hw_ani_init(struct ath_hw *ah)
 
         ath_dbg(common, ANI, "Initialize ANI\n");
 
-        if (use_new_ani(ah)) {
-                ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_NEW;
-                ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_NEW;
+        ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH;
+        ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW;
 
-                ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH_NEW;
-                ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW_NEW;
-        } else {
-                ah->config.ofdm_trig_high = ATH9K_ANI_OFDM_TRIG_HIGH_OLD;
-                ah->config.ofdm_trig_low = ATH9K_ANI_OFDM_TRIG_LOW_OLD;
-
-                ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH_OLD;
-                ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW_OLD;
-        }
+        ah->config.cck_trig_high = ATH9K_ANI_CCK_TRIG_HIGH;
+        ah->config.cck_trig_low = ATH9K_ANI_CCK_TRIG_LOW;
 
         for (i = 0; i < ARRAY_SIZE(ah->channels); i++) {
                 struct ath9k_channel *chan = &ah->channels[i];
                 struct ar5416AniState *ani = &chan->ani;
 
-                if (use_new_ani(ah)) {
-                        ani->spurImmunityLevel =
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
+                ani->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
 
-                        ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
+                ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
 
-                        if (AR_SREV_9300_20_OR_LATER(ah))
-                                ani->mrcCCKOff =
-                                        !ATH9K_ANI_ENABLE_MRC_CCK;
-                        else
-                                ani->mrcCCKOff = true;
-
-                        ani->ofdmsTurn = true;
-                } else {
-                        ani->spurImmunityLevel =
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_OLD;
-                        ani->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_OLD;
-
-                        ani->cckWeakSigThreshold =
-                                ATH9K_ANI_CCK_WEAK_SIG_THR;
-                }
+                ani->mrcCCK = AR_SREV_9300_20_OR_LATER(ah) ? true : false;
+
+                ani->ofdmsTurn = true;
 
                 ani->rssiThrHigh = ATH9K_ANI_RSSI_THR_HIGH;
                 ani->rssiThrLow = ATH9K_ANI_RSSI_THR_LOW;
-                ani->ofdmWeakSigDetectOff =
-                        !ATH9K_ANI_USE_OFDM_WEAK_SIG;
+                ani->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
                 ani->cckNoiseImmunityLevel = ATH9K_ANI_CCK_DEF_LEVEL;
                 ani->ofdmNoiseImmunityLevel = ATH9K_ANI_OFDM_DEF_LEVEL;
                 ani->update_ani = false;
@@ -898,13 +541,8 @@ void ath9k_hw_ani_init(struct ath_hw *ah)
          * since we expect some ongoing maintenance on the tables, let's sanity
          * check here default level should not modify INI setting.
          */
-        if (use_new_ani(ah)) {
-                ah->aniperiod = ATH9K_ANI_PERIOD_NEW;
-                ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL_NEW;
-        } else {
-                ah->aniperiod = ATH9K_ANI_PERIOD_OLD;
-                ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL_OLD;
-        }
+        ah->aniperiod = ATH9K_ANI_PERIOD;
+        ah->config.ani_poll_interval = ATH9K_ANI_POLLINTERVAL;
 
         if (ah->config.enable_ani)
                 ah->proc_phyerr |= HAL_PROCESS_ANI;
diff --git a/drivers/net/wireless/ath/ath9k/ani.h b/drivers/net/wireless/ath/ath9k/ani.h
index 72e2b874e17..e9d841bbe86 100644
--- a/drivers/net/wireless/ath/ath9k/ani.h
+++ b/drivers/net/wireless/ath/ath9k/ani.h
@@ -24,42 +24,34 @@
 #define BEACON_RSSI(ahp) (ahp->stats.avgbrssi)
 
 /* units are errors per second */
-#define ATH9K_ANI_OFDM_TRIG_HIGH_OLD      500
-#define ATH9K_ANI_OFDM_TRIG_HIGH_NEW      3500
+#define ATH9K_ANI_OFDM_TRIG_HIGH          3500
 #define ATH9K_ANI_OFDM_TRIG_HIGH_BELOW_INI 1000
 
 /* units are errors per second */
-#define ATH9K_ANI_OFDM_TRIG_LOW_OLD       200
-#define ATH9K_ANI_OFDM_TRIG_LOW_NEW       400
+#define ATH9K_ANI_OFDM_TRIG_LOW           400
 #define ATH9K_ANI_OFDM_TRIG_LOW_ABOVE_INI 900
 
 /* units are errors per second */
-#define ATH9K_ANI_CCK_TRIG_HIGH_OLD       200
-#define ATH9K_ANI_CCK_TRIG_HIGH_NEW       600
+#define ATH9K_ANI_CCK_TRIG_HIGH           600
 
 /* units are errors per second */
-#define ATH9K_ANI_CCK_TRIG_LOW_OLD        100
-#define ATH9K_ANI_CCK_TRIG_LOW_NEW        300
+#define ATH9K_ANI_CCK_TRIG_LOW            300
 
 #define ATH9K_ANI_NOISE_IMMUNE_LVL        4
 #define ATH9K_ANI_USE_OFDM_WEAK_SIG       true
 #define ATH9K_ANI_CCK_WEAK_SIG_THR        false
 
-#define ATH9K_ANI_SPUR_IMMUNE_LVL_OLD     7
-#define ATH9K_ANI_SPUR_IMMUNE_LVL_NEW     3
+#define ATH9K_ANI_SPUR_IMMUNE_LVL         3
 
-#define ATH9K_ANI_FIRSTEP_LVL_OLD         0
-#define ATH9K_ANI_FIRSTEP_LVL_NEW         2
+#define ATH9K_ANI_FIRSTEP_LVL             2
 
 #define ATH9K_ANI_RSSI_THR_HIGH           40
 #define ATH9K_ANI_RSSI_THR_LOW            7
 
-#define ATH9K_ANI_PERIOD_OLD              100
-#define ATH9K_ANI_PERIOD_NEW              300
+#define ATH9K_ANI_PERIOD                  300
 
 /* in ms */
-#define ATH9K_ANI_POLLINTERVAL_OLD        100
-#define ATH9K_ANI_POLLINTERVAL_NEW        1000
+#define ATH9K_ANI_POLLINTERVAL            1000
 
 #define HAL_NOISE_IMMUNE_MAX              4
 #define HAL_SPUR_IMMUNE_MAX               7
@@ -70,8 +62,6 @@
 #define ATH9K_SIG_SPUR_IMM_SETTING_MIN    0
 #define ATH9K_SIG_SPUR_IMM_SETTING_MAX    22
 
-#define ATH9K_ANI_ENABLE_MRC_CCK          true
-
 /* values here are relative to the INI */
 
 enum ath9k_ani_cmd {
@@ -119,16 +109,15 @@ struct ar5416AniState {
         u8 ofdmNoiseImmunityLevel;
         u8 cckNoiseImmunityLevel;
         bool ofdmsTurn;
-        u8 mrcCCKOff;
+        u8 mrcCCK;
         u8 spurImmunityLevel;
         u8 firstepLevel;
-        u8 ofdmWeakSigDetectOff;
+        u8 ofdmWeakSigDetect;
         u8 cckWeakSigThreshold;
         bool update_ani;
         u32 listenTime;
         int32_t rssiThrLow;
         int32_t rssiThrHigh;
-        u32 noiseFloor;
         u32 ofdmPhyErrCount;
         u32 cckPhyErrCount;
         int16_t pktRssi[2];
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index c7492c6a251..874186bfda4 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -995,141 +995,6 @@ static u32 ar5008_hw_compute_pll_control(struct ath_hw *ah,
         return pll;
 }
 
-static bool ar5008_hw_ani_control_old(struct ath_hw *ah,
-                                      enum ath9k_ani_cmd cmd,
-                                      int param)
-{
-        struct ar5416AniState *aniState = &ah->curchan->ani;
-        struct ath_common *common = ath9k_hw_common(ah);
-
-        switch (cmd & ah->ani_function) {
-        case ATH9K_ANI_NOISE_IMMUNITY_LEVEL:{
-                u32 level = param;
-
-                if (level >= ARRAY_SIZE(ah->totalSizeDesired)) {
-                        ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-                                level, ARRAY_SIZE(ah->totalSizeDesired));
-                        return false;
-                }
-
-                REG_RMW_FIELD(ah, AR_PHY_DESIRED_SZ,
-                              AR_PHY_DESIRED_SZ_TOT_DES,
-                              ah->totalSizeDesired[level]);
-                REG_RMW_FIELD(ah, AR_PHY_AGC_CTL1,
-                              AR_PHY_AGC_CTL1_COARSE_LOW,
-                              ah->coarse_low[level]);
-                REG_RMW_FIELD(ah, AR_PHY_AGC_CTL1,
-                              AR_PHY_AGC_CTL1_COARSE_HIGH,
-                              ah->coarse_high[level]);
-                REG_RMW_FIELD(ah, AR_PHY_FIND_SIG,
-                              AR_PHY_FIND_SIG_FIRPWR,
-                              ah->firpwr[level]);
-
-                if (level > aniState->noiseImmunityLevel)
-                        ah->stats.ast_ani_niup++;
-                else if (level < aniState->noiseImmunityLevel)
-                        ah->stats.ast_ani_nidown++;
-                aniState->noiseImmunityLevel = level;
-                break;
-        }
-        case ATH9K_ANI_OFDM_WEAK_SIGNAL_DETECTION:{
-                u32 on = param ? 1 : 0;
-
-                if (on)
-                        REG_SET_BIT(ah, AR_PHY_SFCORR_LOW,
-                                    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
-                else
-                        REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
-                                    AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
-
-                if (!on != aniState->ofdmWeakSigDetectOff) {
-                        if (on)
-                                ah->stats.ast_ani_ofdmon++;
-                        else
-                                ah->stats.ast_ani_ofdmoff++;
-                        aniState->ofdmWeakSigDetectOff = !on;
-                }
-                break;
-        }
-        case ATH9K_ANI_CCK_WEAK_SIGNAL_THR:{
-                static const int weakSigThrCck[] = { 8, 6 };
-                u32 high = param ? 1 : 0;
-
-                REG_RMW_FIELD(ah, AR_PHY_CCK_DETECT,
-                              AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK,
-                              weakSigThrCck[high]);
-                if (high != aniState->cckWeakSigThreshold) {
-                        if (high)
-                                ah->stats.ast_ani_cckhigh++;
-                        else
-                                ah->stats.ast_ani_ccklow++;
-                        aniState->cckWeakSigThreshold = high;
-                }
-                break;
-        }
-        case ATH9K_ANI_FIRSTEP_LEVEL:{
-                static const int firstep[] = { 0, 4, 8 };
-                u32 level = param;
-
-                if (level >= ARRAY_SIZE(firstep)) {
-                        ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-                                level, ARRAY_SIZE(firstep));
-                        return false;
-                }
-                REG_RMW_FIELD(ah, AR_PHY_FIND_SIG,
-                              AR_PHY_FIND_SIG_FIRSTEP,
-                              firstep[level]);
-                if (level > aniState->firstepLevel)
-                        ah->stats.ast_ani_stepup++;
-                else if (level < aniState->firstepLevel)
-                        ah->stats.ast_ani_stepdown++;
-                aniState->firstepLevel = level;
-                break;
-        }
-        case ATH9K_ANI_SPUR_IMMUNITY_LEVEL:{
-                static const int cycpwrThr1[] = { 2, 4, 6, 8, 10, 12, 14, 16 };
-                u32 level = param;
-
-                if (level >= ARRAY_SIZE(cycpwrThr1)) {
-                        ath_dbg(common, ANI, "level out of range (%u > %zu)\n",
-                                level, ARRAY_SIZE(cycpwrThr1));
-                        return false;
-                }
-                REG_RMW_FIELD(ah, AR_PHY_TIMING5,
-                              AR_PHY_TIMING5_CYCPWR_THR1,
-                              cycpwrThr1[level]);
-                if (level > aniState->spurImmunityLevel)
-                        ah->stats.ast_ani_spurup++;
-                else if (level < aniState->spurImmunityLevel)
-                        ah->stats.ast_ani_spurdown++;
-                aniState->spurImmunityLevel = level;
-                break;
-        }
-        case ATH9K_ANI_PRESENT:
-                break;
-        default:
-                ath_dbg(common, ANI, "invalid cmd %u\n", cmd);
-                return false;
-        }
-
-        ath_dbg(common, ANI, "ANI parameters:\n");
-        ath_dbg(common, ANI,
-                "noiseImmunityLevel=%d, spurImmunityLevel=%d, ofdmWeakSigDetectOff=%d\n",
-                aniState->noiseImmunityLevel,
-                aniState->spurImmunityLevel,
-                !aniState->ofdmWeakSigDetectOff);
-        ath_dbg(common, ANI,
-                "cckWeakSigThreshold=%d, firstepLevel=%d, listenTime=%d\n",
-                aniState->cckWeakSigThreshold,
-                aniState->firstepLevel,
-                aniState->listenTime);
-        ath_dbg(common, ANI, "ofdmPhyErrCount=%d, cckPhyErrCount=%d\n\n",
-                aniState->ofdmPhyErrCount,
-                aniState->cckPhyErrCount);
-
-        return true;
-}
-
 static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                                       enum ath9k_ani_cmd cmd,
                                       int param)
@@ -1206,18 +1071,18 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                         REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
                                     AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
 
-                if (!on != aniState->ofdmWeakSigDetectOff) {
+                if (on != aniState->ofdmWeakSigDetect) {
                         ath_dbg(common, ANI,
                                 "** ch %d: ofdm weak signal: %s=>%s\n",
                                 chan->channel,
-                                !aniState->ofdmWeakSigDetectOff ?
+                                aniState->ofdmWeakSigDetect ?
                                 "on" : "off",
                                 on ? "on" : "off");
                         if (on)
                                 ah->stats.ast_ani_ofdmon++;
                         else
                                 ah->stats.ast_ani_ofdmoff++;
-                        aniState->ofdmWeakSigDetectOff = !on;
+                        aniState->ofdmWeakSigDetect = on;
                 }
                 break;
         }
@@ -1236,7 +1101,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value = firstep_table[level] -
-                        firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+                        firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
                         aniState->iniDef.firstep;
                 if (value < ATH9K_SIG_FIRSTEP_SETTING_MIN)
                         value = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -1251,7 +1116,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value2 = firstep_table[level] -
-                         firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+                         firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
                          aniState->iniDef.firstepLow;
                 if (value2 < ATH9K_SIG_FIRSTEP_SETTING_MIN)
                         value2 = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -1267,7 +1132,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->firstepLevel,
                                 level,
-                                ATH9K_ANI_FIRSTEP_LVL_NEW,
+                                ATH9K_ANI_FIRSTEP_LVL,
                                 value,
                                 aniState->iniDef.firstep);
                         ath_dbg(common, ANI,
@@ -1275,7 +1140,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->firstepLevel,
                                 level,
-                                ATH9K_ANI_FIRSTEP_LVL_NEW,
+                                ATH9K_ANI_FIRSTEP_LVL,
                                 value2,
                                 aniState->iniDef.firstepLow);
                         if (level > aniState->firstepLevel)
@@ -1300,7 +1165,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value = cycpwrThr1_table[level] -
-                        cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+                        cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
                         aniState->iniDef.cycpwrThr1;
                 if (value < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
                         value = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -1316,7 +1181,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value2 = cycpwrThr1_table[level] -
-                         cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+                         cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
                          aniState->iniDef.cycpwrThr1Ext;
                 if (value2 < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
                         value2 = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -1331,7 +1196,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->spurImmunityLevel,
                                 level,
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+                                ATH9K_ANI_SPUR_IMMUNE_LVL,
                                 value,
                                 aniState->iniDef.cycpwrThr1);
                         ath_dbg(common, ANI,
@@ -1339,7 +1204,7 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->spurImmunityLevel,
                                 level,
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+                                ATH9K_ANI_SPUR_IMMUNE_LVL,
                                 value2,
                                 aniState->iniDef.cycpwrThr1Ext);
                         if (level > aniState->spurImmunityLevel)
@@ -1367,9 +1232,9 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
         ath_dbg(common, ANI,
                 "ANI parameters: SI=%d, ofdmWS=%s FS=%d MRCcck=%s listenTime=%d ofdmErrs=%d cckErrs=%d\n",
                 aniState->spurImmunityLevel,
-                !aniState->ofdmWeakSigDetectOff ? "on" : "off",
+                aniState->ofdmWeakSigDetect ? "on" : "off",
                 aniState->firstepLevel,
-                !aniState->mrcCCKOff ? "on" : "off",
+                aniState->mrcCCK ? "on" : "off",
                 aniState->listenTime,
                 aniState->ofdmPhyErrCount,
                 aniState->cckPhyErrCount);
@@ -1454,10 +1319,10 @@ static void ar5008_hw_ani_cache_ini_regs(struct ath_hw *ah)
                                                AR_PHY_EXT_TIMING5_CYCPWR_THR1);
 
         /* these levels just got reset to defaults by the INI */
-        aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
-        aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
-        aniState->ofdmWeakSigDetectOff = !ATH9K_ANI_USE_OFDM_WEAK_SIG;
-        aniState->mrcCCKOff = true; /* not available on pre AR9003 */
+        aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
+        aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
+        aniState->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
+        aniState->mrcCCK = false; /* not available on pre AR9003 */
 }
 
 static void ar5008_hw_set_nf_limits(struct ath_hw *ah)
@@ -1545,11 +1410,8 @@ void ar5008_hw_attach_phy_ops(struct ath_hw *ah)
         priv_ops->do_getnf = ar5008_hw_do_getnf;
         priv_ops->set_radar_params = ar5008_hw_set_radar_params;
 
-        if (modparam_force_new_ani) {
-                priv_ops->ani_control = ar5008_hw_ani_control_new;
-                priv_ops->ani_cache_ini_regs = ar5008_hw_ani_cache_ini_regs;
-        } else
-                priv_ops->ani_control = ar5008_hw_ani_control_old;
+        priv_ops->ani_control = ar5008_hw_ani_control_new;
+        priv_ops->ani_cache_ini_regs = ar5008_hw_ani_cache_ini_regs;
 
         if (AR_SREV_9100(ah) || AR_SREV_9160_10_OR_LATER(ah))
                 priv_ops->compute_pll_control = ar9160_hw_compute_pll_control;
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_hw.c b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
index d9a69fc470c..edf21ea4fe9 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_hw.c
@@ -21,10 +21,6 @@
 #include "ar9002_initvals.h"
 #include "ar9002_phy.h"
 
-int modparam_force_new_ani;
-module_param_named(force_new_ani, modparam_force_new_ani, int, 0444);
-MODULE_PARM_DESC(force_new_ani, "Force new ANI for AR5008, AR9001, AR9002");
-
 /* General hardware code for the A5008/AR9001/AR9002 hadware families */
 
 static void ar9002_hw_init_mode_regs(struct ath_hw *ah)
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index d9e0824af09..78816b8b217 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 b1ced2a76da..61558375bfb 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;
 
@@ -769,10 +767,6 @@ static void ar9003_mci_mute_bt(struct ath_hw *ah)
 {
         /* disable all MCI messages */
         REG_WRITE(ah, AR_MCI_MSG_ATTRIBUTES_TABLE, 0xffff0000);
-        REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS0, 0xffffffff);
-        REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS1, 0xffffffff);
-        REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS2, 0xffffffff);
-        REG_WRITE(ah, AR_BTCOEX_WL_WEIGHTS3, 0xffffffff);
         REG_SET_BIT(ah, AR_MCI_TX_CTRL, AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
 
         /* wait pending HW messages to flush out */
@@ -893,13 +887,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,17 +1007,20 @@ 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);
+                if (!force) {
+                        ar9003_mci_send_2g5g_status(ah, true);
+
+                        ar9003_mci_send_lna_transfer(ah, true);
+                        udelay(5);
+                }
 
                 REG_CLR_BIT(ah, AR_MCI_TX_CTRL,
                             AR_MCI_TX_CTRL_DISABLE_LNA_UPDATE);
@@ -1028,20 +1028,21 @@ void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool wait_done)
                             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);
+                if (!force) {
+                        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 +1170,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 +1186,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 +1197,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 +1205,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 +1229,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 +1236,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);
@@ -1396,6 +1261,9 @@ void ar9003_mci_bt_gain_ctrl(struct ath_hw *ah)
 
         ath_dbg(common, MCI, "Give LNA and SPDT control to BT\n");
 
+        ar9003_mci_send_lna_take(ah, true);
+        udelay(50);
+
         REG_SET_BIT(ah, AR_PHY_GLB_CONTROL, AR_BTCOEX_CTRL_BT_OWN_SPDT_CTRL);
         mci->is_2g = false;
         mci->update_2g5g = true;
@@ -1404,3 +1272,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 10282e2bcdc..d33b8e12885 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/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index d6baf69cdc1..6b91ebb158f 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -173,7 +173,7 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
         int cur_bb_spur, negative = 0, cck_spur_freq;
         int i;
         int range, max_spur_cnts, synth_freq;
-        u8 *spur_fbin_ptr = NULL;
+        u8 *spur_fbin_ptr = ar9003_get_spur_chan_ptr(ah, IS_CHAN_2GHZ(chan));
 
         /*
          * Need to verify range +/- 10 MHz in control channel, otherwise spur
@@ -181,8 +181,6 @@ static void ar9003_hw_spur_mitigate_mrc_cck(struct ath_hw *ah,
          */
 
         if (AR_SREV_9485(ah) || AR_SREV_9340(ah) || AR_SREV_9330(ah)) {
-                spur_fbin_ptr = ar9003_get_spur_chan_ptr(ah,
-                                                         IS_CHAN_2GHZ(chan));
                 if (spur_fbin_ptr[0] == 0) /* No spur */
                         return;
                 max_spur_cnts = 5;
@@ -825,18 +823,18 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                         REG_CLR_BIT(ah, AR_PHY_SFCORR_LOW,
                                     AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
 
-                if (!on != aniState->ofdmWeakSigDetectOff) {
+                if (on != aniState->ofdmWeakSigDetect) {
                         ath_dbg(common, ANI,
                                 "** ch %d: ofdm weak signal: %s=>%s\n",
                                 chan->channel,
-                                !aniState->ofdmWeakSigDetectOff ?
+                                aniState->ofdmWeakSigDetect ?
                                 "on" : "off",
                                 on ? "on" : "off");
                         if (on)
                                 ah->stats.ast_ani_ofdmon++;
                         else
                                 ah->stats.ast_ani_ofdmoff++;
-                        aniState->ofdmWeakSigDetectOff = !on;
+                        aniState->ofdmWeakSigDetect = on;
                 }
                 break;
         }
@@ -855,7 +853,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value = firstep_table[level] -
-                        firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+                        firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
                         aniState->iniDef.firstep;
                 if (value < ATH9K_SIG_FIRSTEP_SETTING_MIN)
                         value = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -870,7 +868,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value2 = firstep_table[level] -
-                         firstep_table[ATH9K_ANI_FIRSTEP_LVL_NEW] +
+                         firstep_table[ATH9K_ANI_FIRSTEP_LVL] +
                          aniState->iniDef.firstepLow;
                 if (value2 < ATH9K_SIG_FIRSTEP_SETTING_MIN)
                         value2 = ATH9K_SIG_FIRSTEP_SETTING_MIN;
@@ -886,7 +884,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->firstepLevel,
                                 level,
-                                ATH9K_ANI_FIRSTEP_LVL_NEW,
+                                ATH9K_ANI_FIRSTEP_LVL,
                                 value,
                                 aniState->iniDef.firstep);
                         ath_dbg(common, ANI,
@@ -894,7 +892,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->firstepLevel,
                                 level,
-                                ATH9K_ANI_FIRSTEP_LVL_NEW,
+                                ATH9K_ANI_FIRSTEP_LVL,
                                 value2,
                                 aniState->iniDef.firstepLow);
                         if (level > aniState->firstepLevel)
@@ -919,7 +917,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value = cycpwrThr1_table[level] -
-                        cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+                        cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
                         aniState->iniDef.cycpwrThr1;
                 if (value < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
                         value = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -935,7 +933,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                  * from INI file & cap value
                  */
                 value2 = cycpwrThr1_table[level] -
-                         cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL_NEW] +
+                         cycpwrThr1_table[ATH9K_ANI_SPUR_IMMUNE_LVL] +
                          aniState->iniDef.cycpwrThr1Ext;
                 if (value2 < ATH9K_SIG_SPUR_IMM_SETTING_MIN)
                         value2 = ATH9K_SIG_SPUR_IMM_SETTING_MIN;
@@ -950,7 +948,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->spurImmunityLevel,
                                 level,
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+                                ATH9K_ANI_SPUR_IMMUNE_LVL,
                                 value,
                                 aniState->iniDef.cycpwrThr1);
                         ath_dbg(common, ANI,
@@ -958,7 +956,7 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                                 chan->channel,
                                 aniState->spurImmunityLevel,
                                 level,
-                                ATH9K_ANI_SPUR_IMMUNE_LVL_NEW,
+                                ATH9K_ANI_SPUR_IMMUNE_LVL,
                                 value2,
                                 aniState->iniDef.cycpwrThr1Ext);
                         if (level > aniState->spurImmunityLevel)
@@ -979,16 +977,16 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
                               AR_PHY_MRC_CCK_ENABLE, is_on);
                 REG_RMW_FIELD(ah, AR_PHY_MRC_CCK_CTRL,
                               AR_PHY_MRC_CCK_MUX_REG, is_on);
-                if (!is_on != aniState->mrcCCKOff) {
+                if (is_on != aniState->mrcCCK) {
                         ath_dbg(common, ANI, "** ch %d: MRC CCK: %s=>%s\n",
                                 chan->channel,
-                                !aniState->mrcCCKOff ? "on" : "off",
+                                aniState->mrcCCK ? "on" : "off",
                                 is_on ? "on" : "off");
                 if (is_on)
                         ah->stats.ast_ani_ccklow++;
                 else
                         ah->stats.ast_ani_cckhigh++;
-                aniState->mrcCCKOff = !is_on;
+                aniState->mrcCCK = is_on;
                 }
         break;
         }
@@ -1002,9 +1000,9 @@ static bool ar9003_hw_ani_control(struct ath_hw *ah,
         ath_dbg(common, ANI,
                 "ANI parameters: SI=%d, ofdmWS=%s FS=%d MRCcck=%s listenTime=%d ofdmErrs=%d cckErrs=%d\n",
                 aniState->spurImmunityLevel,
-                !aniState->ofdmWeakSigDetectOff ? "on" : "off",
+                aniState->ofdmWeakSigDetect ? "on" : "off",
                 aniState->firstepLevel,
-                !aniState->mrcCCKOff ? "on" : "off",
+                aniState->mrcCCK ? "on" : "off",
                 aniState->listenTime,
                 aniState->ofdmPhyErrCount,
                 aniState->cckPhyErrCount);
@@ -1111,10 +1109,10 @@ static void ar9003_hw_ani_cache_ini_regs(struct ath_hw *ah)
                                                AR_PHY_EXT_CYCPWR_THR1);
 
         /* these levels just got reset to defaults by the INI */
-        aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL_NEW;
-        aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL_NEW;
-        aniState->ofdmWeakSigDetectOff = !ATH9K_ANI_USE_OFDM_WEAK_SIG;
-        aniState->mrcCCKOff = !ATH9K_ANI_ENABLE_MRC_CCK;
+        aniState->spurImmunityLevel = ATH9K_ANI_SPUR_IMMUNE_LVL;
+        aniState->firstepLevel = ATH9K_ANI_FIRSTEP_LVL;
+        aniState->ofdmWeakSigDetect = ATH9K_ANI_USE_OFDM_WEAK_SIG;
+        aniState->mrcCCK = true;
 }
 
 static void ar9003_hw_set_radar_params(struct ath_hw *ah,
diff --git a/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9462_2p0_initvals.h
index a10ece0cb6c..bbf48918a56 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},
@@ -61,7 +61,7 @@ static const u32 ar9462_2p0_baseband_postamble[][5] = {
         {0x00009e44, 0x62321e27, 0x62321e27, 0xfe291e27, 0xfe291e27},
         {0x00009e48, 0x5030201a, 0x5030201a, 0x50302012, 0x50302012},
         {0x00009fc8, 0x0003f000, 0x0003f000, 0x0001a000, 0x0001a000},
-        {0x0000a204, 0x013187c0, 0x013187c4, 0x013187c4, 0x013187c0},
+        {0x0000a204, 0x01318fc0, 0x01318fc4, 0x01318fc4, 0x01318fc0},
         {0x0000a208, 0x00000104, 0x00000104, 0x00000004, 0x00000004},
         {0x0000a22c, 0x01026a2f, 0x01026a27, 0x01026a2f, 0x01026a2f},
         {0x0000a230, 0x0000400a, 0x00004014, 0x00004016, 0x0000400b},
@@ -1007,6 +1007,7 @@ static const u32 ar9462_2p0_radio_core[][2] = {
 
 static const u32 ar9462_2p0_soc_preamble[][2] = {
         /* Addr      allmodes  */
+        {0x000040a4 ,0x00a0c1c9},
         {0x00007020, 0x00000000},
         {0x00007034, 0x00000002},
         {0x00007038, 0x000004c2},
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 02fc1c1e5ee..4473278efc5 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;
@@ -720,6 +721,7 @@ extern int ath9k_modparam_nohwcrypt;
 extern int led_blink;
 extern bool is_ath9k_unloaded;
 
+u8 ath9k_parse_mpdudensity(u8 mpdudensity);
 irqreturn_t ath_isr(int irq, void *dev);
 int ath9k_init_device(u16 devid, struct ath_softc *sc,
                     const struct ath_bus_ops *bus_ops);
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 2831258d950..5c3192ffc19 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -348,8 +348,6 @@ void ath_debug_stat_interrupt(struct ath_softc *sc, enum ath9k_int status)
                 sc->debug.stats.istats.txok++;
         if (status & ATH9K_INT_TXURN)
                 sc->debug.stats.istats.txurn++;
-        if (status & ATH9K_INT_MIB)
-                sc->debug.stats.istats.mib++;
         if (status & ATH9K_INT_RXPHY)
                 sc->debug.stats.istats.rxphyerr++;
         if (status & ATH9K_INT_RXKCM)
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index af6d2735029..26032cb59b8 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/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 135795257d9..936e920fb88 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -453,7 +453,6 @@ struct ath9k_htc_priv {
         u8 num_sta_assoc_vif;
         u8 num_ap_vif;
 
-        u16 op_flags;
         u16 curtxpow;
         u16 txpowlimit;
         u16 nvifs;
@@ -461,6 +460,7 @@ struct ath9k_htc_priv {
         bool rearm_ani;
         bool reconfig_beacon;
         unsigned int rxfilter;
+        unsigned long op_flags;
 
         struct ath9k_hw_cal_data caldata;
         struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
@@ -572,8 +572,6 @@ bool ath9k_htc_setpower(struct ath9k_htc_priv *priv,
 
 void ath9k_start_rfkill_poll(struct ath9k_htc_priv *priv);
 void ath9k_htc_rfkill_poll_state(struct ieee80211_hw *hw);
-void ath9k_htc_radio_enable(struct ieee80211_hw *hw);
-void ath9k_htc_radio_disable(struct ieee80211_hw *hw);
 
 #ifdef CONFIG_MAC80211_LEDS
 void ath9k_init_leds(struct ath9k_htc_priv *priv);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
index 2eadffb7971..77d541feb91 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_beacon.c
@@ -207,9 +207,9 @@ static void ath9k_htc_beacon_config_ap(struct ath9k_htc_priv *priv,
         else
                 priv->ah->config.sw_beacon_response_time = MIN_SWBA_RESPONSE;
 
-        if (priv->op_flags & OP_TSF_RESET) {
+        if (test_bit(OP_TSF_RESET, &priv->op_flags)) {
                 ath9k_hw_reset_tsf(priv->ah);
-                priv->op_flags &= ~OP_TSF_RESET;
+                clear_bit(OP_TSF_RESET, &priv->op_flags);
         } else {
                 /*
                  * Pull nexttbtt forward to reflect the current TSF.
@@ -221,7 +221,7 @@ static void ath9k_htc_beacon_config_ap(struct ath9k_htc_priv *priv,
                 } while (nexttbtt < tsftu);
         }
 
-        if (priv->op_flags & OP_ENABLE_BEACON)
+        if (test_bit(OP_ENABLE_BEACON, &priv->op_flags))
                 imask |= ATH9K_INT_SWBA;
 
         ath_dbg(common, CONFIG,
@@ -269,7 +269,7 @@ static void ath9k_htc_beacon_config_adhoc(struct ath9k_htc_priv *priv,
         else
                 priv->ah->config.sw_beacon_response_time = MIN_SWBA_RESPONSE;
 
-        if (priv->op_flags & OP_ENABLE_BEACON)
+        if (test_bit(OP_ENABLE_BEACON, &priv->op_flags))
                 imask |= ATH9K_INT_SWBA;
 
         ath_dbg(common, CONFIG,
@@ -365,7 +365,7 @@ static void ath9k_htc_send_beacon(struct ath9k_htc_priv *priv,
         vif = priv->cur_beacon_conf.bslot[slot];
         avp = (struct ath9k_htc_vif *)vif->drv_priv;
 
-        if (unlikely(priv->op_flags & OP_SCANNING)) {
+        if (unlikely(test_bit(OP_SCANNING, &priv->op_flags))) {
                 spin_unlock_bh(&priv->beacon_lock);
                 return;
         }
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
index 1c10e2e5c23..07df279c8d4 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
@@ -37,17 +37,18 @@ static void ath_detect_bt_priority(struct ath9k_htc_priv *priv)
 
         if (time_after(jiffies, btcoex->bt_priority_time +
                         msecs_to_jiffies(ATH_BT_PRIORITY_TIME_THRESHOLD))) {
-                priv->op_flags &= ~(OP_BT_PRIORITY_DETECTED | OP_BT_SCAN);
+                clear_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+                clear_bit(OP_BT_SCAN, &priv->op_flags);
                 /* Detect if colocated bt started scanning */
                 if (btcoex->bt_priority_cnt >= ATH_BT_CNT_SCAN_THRESHOLD) {
                         ath_dbg(ath9k_hw_common(ah), BTCOEX,
                                 "BT scan detected\n");
-                        priv->op_flags |= (OP_BT_SCAN |
-                                         OP_BT_PRIORITY_DETECTED);
+                        set_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+                        set_bit(OP_BT_SCAN, &priv->op_flags);
                 } else if (btcoex->bt_priority_cnt >= ATH_BT_CNT_THRESHOLD) {
                         ath_dbg(ath9k_hw_common(ah), BTCOEX,
                                 "BT priority traffic detected\n");
-                        priv->op_flags |= OP_BT_PRIORITY_DETECTED;
+                        set_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
                 }
 
                 btcoex->bt_priority_cnt = 0;
@@ -67,26 +68,23 @@ static void ath_btcoex_period_work(struct work_struct *work)
         struct ath_btcoex *btcoex = &priv->btcoex;
         struct ath_common *common = ath9k_hw_common(priv->ah);
         u32 timer_period;
-        bool is_btscan;
         int ret;
 
         ath_detect_bt_priority(priv);
 
-        is_btscan = !!(priv->op_flags & OP_BT_SCAN);
-
         ret = ath9k_htc_update_cap_target(priv,
-                                  !!(priv->op_flags & OP_BT_PRIORITY_DETECTED));
+                          test_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags));
         if (ret) {
                 ath_err(common, "Unable to set BTCOEX parameters\n");
                 return;
         }
 
-        ath9k_hw_btcoex_bt_stomp(priv->ah, is_btscan ? ATH_BTCOEX_STOMP_ALL :
-                        btcoex->bt_stomp_type);
+        ath9k_hw_btcoex_bt_stomp(priv->ah, test_bit(OP_BT_SCAN, &priv->op_flags) ?
+                                 ATH_BTCOEX_STOMP_ALL : btcoex->bt_stomp_type);
 
         ath9k_hw_btcoex_enable(priv->ah);
-        timer_period = is_btscan ? btcoex->btscan_no_stomp :
-                btcoex->btcoex_no_stomp;
+        timer_period = test_bit(OP_BT_SCAN, &priv->op_flags) ?
+                btcoex->btscan_no_stomp : btcoex->btcoex_no_stomp;
         ieee80211_queue_delayed_work(priv->hw, &priv->duty_cycle_work,
                                      msecs_to_jiffies(timer_period));
         ieee80211_queue_delayed_work(priv->hw, &priv->coex_period_work,
@@ -104,14 +102,15 @@ static void ath_btcoex_duty_cycle_work(struct work_struct *work)
         struct ath_hw *ah = priv->ah;
         struct ath_btcoex *btcoex = &priv->btcoex;
         struct ath_common *common = ath9k_hw_common(ah);
-        bool is_btscan = priv->op_flags & OP_BT_SCAN;
 
         ath_dbg(common, BTCOEX, "time slice work for bt and wlan\n");
 
-        if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW || is_btscan)
+        if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW ||
+            test_bit(OP_BT_SCAN, &priv->op_flags))
                 ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_NONE);
         else if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_ALL)
                 ath9k_hw_btcoex_bt_stomp(ah, ATH_BTCOEX_STOMP_LOW);
+
         ath9k_hw_btcoex_enable(priv->ah);
 }
 
@@ -141,7 +140,8 @@ static void ath_htc_resume_btcoex_work(struct ath9k_htc_priv *priv)
 
         btcoex->bt_priority_cnt = 0;
         btcoex->bt_priority_time = jiffies;
-        priv->op_flags &= ~(OP_BT_PRIORITY_DETECTED | OP_BT_SCAN);
+        clear_bit(OP_BT_PRIORITY_DETECTED, &priv->op_flags);
+        clear_bit(OP_BT_SCAN, &priv->op_flags);
         ieee80211_queue_delayed_work(priv->hw, &priv->coex_period_work, 0);
 }
 
@@ -310,95 +310,3 @@ void ath9k_start_rfkill_poll(struct ath9k_htc_priv *priv)
         if (priv->ah->caps.hw_caps & ATH9K_HW_CAP_RFSILENT)
                 wiphy_rfkill_start_polling(priv->hw->wiphy);
 }
-
-void ath9k_htc_radio_enable(struct ieee80211_hw *hw)
-{
-        struct ath9k_htc_priv *priv = hw->priv;
-        struct ath_hw *ah = priv->ah;
-        struct ath_common *common = ath9k_hw_common(ah);
-        int ret;
-        u8 cmd_rsp;
-
-        if (!ah->curchan)
-                ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
-
-        /* Reset the HW */
-        ret = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
-        if (ret) {
-                ath_err(common,
-                        "Unable to reset hardware; reset status %d (freq %u MHz)\n",
-                        ret, ah->curchan->channel);
-        }
-
-        ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
-                               &priv->curtxpow);
-
-        /* Start RX */
-        WMI_CMD(WMI_START_RECV_CMDID);
-        ath9k_host_rx_init(priv);
-
-        /* Start TX */
-        htc_start(priv->htc);
-        spin_lock_bh(&priv->tx.tx_lock);
-        priv->tx.flags &= ~ATH9K_HTC_OP_TX_QUEUES_STOP;
-        spin_unlock_bh(&priv->tx.tx_lock);
-        ieee80211_wake_queues(hw);
-
-        WMI_CMD(WMI_ENABLE_INTR_CMDID);
-
-        /* Enable LED */
-        ath9k_hw_cfg_output(ah, ah->led_pin,
-                            AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
-        ath9k_hw_set_gpio(ah, ah->led_pin, 0);
-}
-
-void ath9k_htc_radio_disable(struct ieee80211_hw *hw)
-{
-        struct ath9k_htc_priv *priv = hw->priv;
-        struct ath_hw *ah = priv->ah;
-        struct ath_common *common = ath9k_hw_common(ah);
-        int ret;
-        u8 cmd_rsp;
-
-        ath9k_htc_ps_wakeup(priv);
-
-        /* Disable LED */
-        ath9k_hw_set_gpio(ah, ah->led_pin, 1);
-        ath9k_hw_cfg_gpio_input(ah, ah->led_pin);
-
-        WMI_CMD(WMI_DISABLE_INTR_CMDID);
-
-        /* Stop TX */
-        ieee80211_stop_queues(hw);
-        ath9k_htc_tx_drain(priv);
-        WMI_CMD(WMI_DRAIN_TXQ_ALL_CMDID);
-
-        /* Stop RX */
-        WMI_CMD(WMI_STOP_RECV_CMDID);
-
-        /* Clear the WMI event queue */
-        ath9k_wmi_event_drain(priv);
-
-        /*
-         * The MIB counters have to be disabled here,
-         * since the target doesn't do it.
-         */
-        ath9k_hw_disable_mib_counters(ah);
-
-        if (!ah->curchan)
-                ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
-
-        /* Reset the HW */
-        ret = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
-        if (ret) {
-                ath_err(common,
-                        "Unable to reset hardware; reset status %d (freq %u MHz)\n",
-                        ret, ah->curchan->channel);
-        }
-
-        /* Disable the PHY */
-        ath9k_hw_phy_disable(ah);
-
-        ath9k_htc_ps_restore(priv);
-        ath9k_htc_setpower(priv, ATH9K_PM_FULL_SLEEP);
-}
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index 25213d521bc..a035a380d66 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -611,7 +611,7 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
         struct ath_common *common;
         int i, ret = 0, csz = 0;
 
-        priv->op_flags |= OP_INVALID;
+        set_bit(OP_INVALID, &priv->op_flags);
 
         ah = kzalloc(sizeof(struct ath_hw), GFP_KERNEL);
         if (!ah)
@@ -718,7 +718,7 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 
         hw->queues = 4;
         hw->channel_change_time = 5000;
-        hw->max_listen_interval = 10;
+        hw->max_listen_interval = 1;
 
         hw->vif_data_size = sizeof(struct ath9k_htc_vif);
         hw->sta_data_size = sizeof(struct ath9k_htc_sta);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 2b8f61c210e..fc8cd828410 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -75,14 +75,19 @@ unlock:
 
 void ath9k_htc_ps_restore(struct ath9k_htc_priv *priv)
 {
+        bool reset;
+
         mutex_lock(&priv->htc_pm_lock);
         if (--priv->ps_usecount != 0)
                 goto unlock;
 
-        if (priv->ps_idle)
+        if (priv->ps_idle) {
+                ath9k_hw_setrxabort(priv->ah, true);
+                ath9k_hw_stopdmarecv(priv->ah, &reset);
                 ath9k_hw_setpower(priv->ah, ATH9K_PM_FULL_SLEEP);
-        else if (priv->ps_enabled)
+        } else if (priv->ps_enabled) {
                 ath9k_hw_setpower(priv->ah, ATH9K_PM_NETWORK_SLEEP);
+        }
 
 unlock:
         mutex_unlock(&priv->htc_pm_lock);
@@ -250,7 +255,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
         u8 cmd_rsp;
         int ret;
 
-        if (priv->op_flags & OP_INVALID)
+        if (test_bit(OP_INVALID, &priv->op_flags))
                 return -EIO;
 
         fastcc = !!(hw->conf.flags & IEEE80211_CONF_OFFCHANNEL);
@@ -304,7 +309,7 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
 
         htc_start(priv->htc);
 
-        if (!(priv->op_flags & OP_SCANNING) &&
+        if (!test_bit(OP_SCANNING, &priv->op_flags) &&
             !(hw->conf.flags & IEEE80211_CONF_OFFCHANNEL))
                 ath9k_htc_vif_reconfig(priv);
 
@@ -750,7 +755,7 @@ void ath9k_htc_start_ani(struct ath9k_htc_priv *priv)
         common->ani.shortcal_timer = timestamp;
         common->ani.checkani_timer = timestamp;
 
-        priv->op_flags |= OP_ANI_RUNNING;
+        set_bit(OP_ANI_RUNNING, &priv->op_flags);
 
         ieee80211_queue_delayed_work(common->hw, &priv->ani_work,
                                      msecs_to_jiffies(ATH_ANI_POLLINTERVAL));
@@ -759,7 +764,7 @@ void ath9k_htc_start_ani(struct ath9k_htc_priv *priv)
 void ath9k_htc_stop_ani(struct ath9k_htc_priv *priv)
 {
         cancel_delayed_work_sync(&priv->ani_work);
-        priv->op_flags &= ~OP_ANI_RUNNING;
+        clear_bit(OP_ANI_RUNNING, &priv->op_flags);
 }
 
 void ath9k_htc_ani_work(struct work_struct *work)
@@ -944,7 +949,7 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
                 ath_dbg(common, CONFIG,
                         "Failed to update capability in target\n");
 
-        priv->op_flags &= ~OP_INVALID;
+        clear_bit(OP_INVALID, &priv->op_flags);
         htc_start(priv->htc);
 
         spin_lock_bh(&priv->tx.tx_lock);
@@ -973,7 +978,7 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
 
         mutex_lock(&priv->mutex);
 
-        if (priv->op_flags & OP_INVALID) {
+        if (test_bit(OP_INVALID, &priv->op_flags)) {
                 ath_dbg(common, ANY, "Device not present\n");
                 mutex_unlock(&priv->mutex);
                 return;
@@ -1015,7 +1020,7 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
         ath9k_htc_ps_restore(priv);
         ath9k_htc_setpower(priv, ATH9K_PM_FULL_SLEEP);
 
-        priv->op_flags |= OP_INVALID;
+        set_bit(OP_INVALID, &priv->op_flags);
 
         ath_dbg(common, CONFIG, "Driver halt\n");
         mutex_unlock(&priv->mutex);
@@ -1105,7 +1110,7 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
         ath9k_htc_set_opmode(priv);
 
         if ((priv->ah->opmode == NL80211_IFTYPE_AP) &&
-            !(priv->op_flags & OP_ANI_RUNNING)) {
+            !test_bit(OP_ANI_RUNNING, &priv->op_flags)) {
                 ath9k_hw_set_tsfadjust(priv->ah, 1);
                 ath9k_htc_start_ani(priv);
         }
@@ -1178,24 +1183,20 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
         struct ath9k_htc_priv *priv = hw->priv;
         struct ath_common *common = ath9k_hw_common(priv->ah);
         struct ieee80211_conf *conf = &hw->conf;
+        bool chip_reset = false;
+        int ret = 0;
 
         mutex_lock(&priv->mutex);
+        ath9k_htc_ps_wakeup(priv);
 
         if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-                bool enable_radio = false;
-                bool idle = !!(conf->flags & IEEE80211_CONF_IDLE);
-
                 mutex_lock(&priv->htc_pm_lock);
-                if (!idle && priv->ps_idle)
-                        enable_radio = true;
-                priv->ps_idle = idle;
-                mutex_unlock(&priv->htc_pm_lock);
 
-                if (enable_radio) {
-                        ath_dbg(common, CONFIG, "not-idle: enabling radio\n");
-                        ath9k_htc_setpower(priv, ATH9K_PM_AWAKE);
-                        ath9k_htc_radio_enable(hw);
-                }
+                priv->ps_idle = !!(conf->flags & IEEE80211_CONF_IDLE);
+                if (priv->ps_idle)
+                        chip_reset = true;
+
+                mutex_unlock(&priv->htc_pm_lock);
         }
 
         /*
@@ -1210,7 +1211,7 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
                         ath9k_htc_remove_monitor_interface(priv);
         }
 
-        if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+        if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) || chip_reset) {
                 struct ieee80211_channel *curchan = hw->conf.channel;
                 int pos = curchan->hw_value;
 
@@ -1223,8 +1224,8 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
 
                 if (ath9k_htc_set_channel(priv, hw, &priv->ah->channels[pos]) < 0) {
                         ath_err(common, "Unable to set channel\n");
-                        mutex_unlock(&priv->mutex);
-                        return -EINVAL;
+                        ret = -EINVAL;
+                        goto out;
                 }
 
         }
@@ -1246,21 +1247,10 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
                                        priv->txpowlimit, &priv->curtxpow);
         }
 
-        if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-                mutex_lock(&priv->htc_pm_lock);
-                if (!priv->ps_idle) {
-                        mutex_unlock(&priv->htc_pm_lock);
-                        goto out;
-                }
-                mutex_unlock(&priv->htc_pm_lock);
-
-                ath_dbg(common, CONFIG, "idle: disabling radio\n");
-                ath9k_htc_radio_disable(hw);
-        }
-
 out:
+        ath9k_htc_ps_restore(priv);
         mutex_unlock(&priv->mutex);
-        return 0;
+        return ret;
 }
 
 #define SUPPORTED_FILTERS                       \
@@ -1285,7 +1275,7 @@ static void ath9k_htc_configure_filter(struct ieee80211_hw *hw,
         changed_flags &= SUPPORTED_FILTERS;
         *total_flags &= SUPPORTED_FILTERS;
 
-        if (priv->op_flags & OP_INVALID) {
+        if (test_bit(OP_INVALID, &priv->op_flags)) {
                 ath_dbg(ath9k_hw_common(priv->ah), ANY,
                         "Unable to configure filter on invalid state\n");
                 mutex_unlock(&priv->mutex);
@@ -1517,7 +1507,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
                 ath_dbg(common, CONFIG, "Beacon enabled for BSS: %pM\n",
                         bss_conf->bssid);
                 ath9k_htc_set_tsfadjust(priv, vif);
-                priv->op_flags |= OP_ENABLE_BEACON;
+                set_bit(OP_ENABLE_BEACON, &priv->op_flags);
                 ath9k_htc_beacon_config(priv, vif);
         }
 
@@ -1530,7 +1520,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
                         ath_dbg(common, CONFIG,
                                 "Beacon disabled for BSS: %pM\n",
                                 bss_conf->bssid);
-                        priv->op_flags &= ~OP_ENABLE_BEACON;
+                        clear_bit(OP_ENABLE_BEACON, &priv->op_flags);
                         ath9k_htc_beacon_config(priv, vif);
                 }
         }
@@ -1543,7 +1533,7 @@ static void ath9k_htc_bss_info_changed(struct ieee80211_hw *hw,
                     (priv->nvifs == 1) &&
                     (priv->num_ap_vif == 1) &&
                     (vif->type == NL80211_IFTYPE_AP)) {
-                        priv->op_flags |= OP_TSF_RESET;
+                        set_bit(OP_TSF_RESET, &priv->op_flags);
                 }
                 ath_dbg(common, CONFIG,
                         "Beacon interval changed for BSS: %pM\n",
@@ -1655,7 +1645,7 @@ static void ath9k_htc_sw_scan_start(struct ieee80211_hw *hw)
 
         mutex_lock(&priv->mutex);
         spin_lock_bh(&priv->beacon_lock);
-        priv->op_flags |= OP_SCANNING;
+        set_bit(OP_SCANNING, &priv->op_flags);
         spin_unlock_bh(&priv->beacon_lock);
         cancel_work_sync(&priv->ps_work);
         ath9k_htc_stop_ani(priv);
@@ -1668,7 +1658,7 @@ static void ath9k_htc_sw_scan_complete(struct ieee80211_hw *hw)
 
         mutex_lock(&priv->mutex);
         spin_lock_bh(&priv->beacon_lock);
-        priv->op_flags &= ~OP_SCANNING;
+        clear_bit(OP_SCANNING, &priv->op_flags);
         spin_unlock_bh(&priv->beacon_lock);
         ath9k_htc_ps_wakeup(priv);
         ath9k_htc_vif_reconfig(priv);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index 3e40a646151..47e61d0da33 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -916,7 +916,7 @@ void ath9k_host_rx_init(struct ath9k_htc_priv *priv)
 {
         ath9k_hw_rxena(priv->ah);
         ath9k_htc_opmode_init(priv);
-        ath9k_hw_startpcureceive(priv->ah, (priv->op_flags & OP_SCANNING));
+        ath9k_hw_startpcureceive(priv->ah, test_bit(OP_SCANNING, &priv->op_flags));
         priv->rx.last_rssi = ATH_RSSI_DUMMY_MARKER;
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 45e670087e1..784baee5db8 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/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 03d590924c6..94096607cbd 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -1019,16 +1019,8 @@ void ar9002_hw_attach_ops(struct ath_hw *ah);
 void ar9003_hw_attach_ops(struct ath_hw *ah);
 
 void ar9002_hw_load_ani_reg(struct ath_hw *ah, struct ath9k_channel *chan);
-/*
- * ANI work can be shared between all families but a next
- * generation implementation of ANI will be used only for AR9003 only
- * for now as the other families still need to be tested with the same
- * next generation ANI. Feel free to start testing it though for the
- * older families (AR5008, AR9001, AR9002) by using modparam_force_new_ani.
- */
-extern int modparam_force_new_ani;
+
 void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning);
-void ath9k_hw_proc_mib_event(struct ath_hw *ah);
 void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan);
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
@@ -1038,7 +1030,8 @@ static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 }
 static inline bool ath9k_hw_mci_is_enabled(struct ath_hw *ah)
 {
-        return ah->btcoex_hw.enabled && (ah->caps.hw_caps & ATH9K_HW_CAP_MCI);
+        return ah->common.btcoex_enabled &&
+               (ah->caps.hw_caps & ATH9K_HW_CAP_MCI);
 
 }
 void ath9k_hw_btcoex_enable(struct ath_hw *ah);
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 0cc4c70f7f0..91650fe5046 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);
@@ -399,6 +407,7 @@ void ath_ani_calibrate(unsigned long data)
                 longcal ? "long" : "", shortcal ? "short" : "",
                 aniflag ? "ani" : "", common->ani.caldone ? "true" : "false");
 
+        ath9k_debug_samp_bb_mac(sc);
         ath9k_ps_restore(sc);
 
 set_timer:
@@ -407,7 +416,6 @@ set_timer:
         * The interval must be the shortest necessary to satisfy ANI,
         * short calibration and long calibration.
         */
-        ath9k_debug_samp_bb_mac(sc);
         cal_interval = ATH_LONG_CALINTERVAL;
         if (sc->sc_ah->config.enable_ani)
                 cal_interval = min(cal_interval,
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index c0f478b0a9a..e4e73f061a2 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -19,7 +19,7 @@
 #include "ath9k.h"
 #include "btcoex.h"
 
-static u8 parse_mpdudensity(u8 mpdudensity)
+u8 ath9k_parse_mpdudensity(u8 mpdudensity)
 {
         /*
          * 802.11n D2.0 defined values for "Minimum MPDU Start Spacing":
@@ -150,6 +150,11 @@ 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
+        if (ath9k_hw_mci_is_enabled(sc->sc_ah))
+                cancel_work_sync(&sc->mci_work);
+#endif
 }
 
 static void ath_cancel_work(struct ath_softc *sc)
@@ -317,6 +322,7 @@ static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta,
                             struct ieee80211_vif *vif)
 {
         struct ath_node *an;
+        u8 density;
         an = (struct ath_node *)sta->drv_priv;
 
 #ifdef CONFIG_ATH9K_DEBUGFS
@@ -331,7 +337,8 @@ static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta,
                 ath_tx_node_init(sc, an);
                 an->maxampdu = 1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
                                      sta->ht_cap.ampdu_factor);
-                an->mpdudensity = parse_mpdudensity(sta->ht_cap.ampdu_density);
+                density = ath9k_parse_mpdudensity(sta->ht_cap.ampdu_density);
+                an->mpdudensity = density;
         }
 }
 
@@ -513,24 +520,6 @@ irqreturn_t ath_isr(int irq, void *dev)
                 ath9k_hw_set_interrupts(ah);
         }
 
-        if (status & ATH9K_INT_MIB) {
-                /*
-                 * Disable interrupts until we service the MIB
-                 * interrupt; otherwise it will continue to
-                 * fire.
-                 */
-                ath9k_hw_disable_interrupts(ah);
-                /*
-                 * Let the hal handle the event. We assume
-                 * it will clear whatever condition caused
-                 * the interrupt.
-                 */
-                spin_lock(&common->cc_lock);
-                ath9k_hw_proc_mib_event(ah);
-                spin_unlock(&common->cc_lock);
-                ath9k_hw_enable_interrupts(ah);
-        }
-
         if (!(ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP))
                 if (status & ATH9K_INT_TIM_TIMER) {
                         if (ATH_DBG_WARN_ON_ONCE(sc->ps_idle))
@@ -956,14 +945,10 @@ static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
         /*
          * Enable MIB interrupts when there are hardware phy counters.
          */
-        if ((iter_data.nstations + iter_data.nadhocs + iter_data.nmeshes) > 0) {
-                if (ah->config.enable_ani)
-                        ah->imask |= ATH9K_INT_MIB;
+        if ((iter_data.nstations + iter_data.nadhocs + iter_data.nmeshes) > 0)
                 ah->imask |= ATH9K_INT_TSFOOR;
-        } else {
-                ah->imask &= ~ATH9K_INT_MIB;
+        else
                 ah->imask &= ~ATH9K_INT_TSFOOR;
-        }
 
         ath9k_hw_set_interrupts(ah);
 
@@ -1033,15 +1018,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 +1042,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 +1225,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 49137f477b0..c40e568b5c2 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,55 @@ 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) {
+                /*
+                 * Two MCI interrupts are generated while connecting to
+                 * headset and A2DP profile, but only one MCI interrupt
+                 * is generated with last added profile type while disconnecting
+                 * both profiles.
+                 * So while adding second profile type decrement
+                 * the first one.
+                 */
+                if (entry->type != info->type) {
+                        DEC_PROF(mci, entry);
+                        INC_PROF(mci, info);
+                }
+                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 +280,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 +299,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 +308,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 +388,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 +416,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 +425,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 +445,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 +488,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 +530,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 92a6c0a87f8..e034add9cd5 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 560d6effac7..5046b282a93 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
 
@@ -2162,10 +2162,6 @@ enum {
 #define AR_BTCOEX_CTRL_SPDT_POLARITY                    0x80000000
 #define AR_BTCOEX_CTRL_SPDT_POLARITY_S                  31
 
-#define AR_BTCOEX_WL_WEIGHTS0                           0x18b0
-#define AR_BTCOEX_WL_WEIGHTS1                           0x18b4
-#define AR_BTCOEX_WL_WEIGHTS2                           0x18b8
-#define AR_BTCOEX_WL_WEIGHTS3                           0x18bc
 #define AR_BTCOEX_MAX_TXPWR(_x)                         (0x18c0 + ((_x) << 2))
 #define AR_BTCOEX_WL_LNA                                0x1940
 #define AR_BTCOEX_RFGAIN_CTRL                           0x1944
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index f777ddcd117..2afc5e289df 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1175,6 +1175,7 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 {
         struct ath_atx_tid *txtid;
         struct ath_node *an;
+        u8 density;
 
         an = (struct ath_node *)sta->drv_priv;
         txtid = ATH_AN_2_TID(an, tid);
@@ -1182,6 +1183,17 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
         if (txtid->state & (AGGR_CLEANUP | AGGR_ADDBA_COMPLETE))
                 return -EAGAIN;
 
+        /* update ampdu factor/density, they may have changed. This may happen
+         * in HT IBSS when a beacon with HT-info is received after the station
+         * has already been added.
+         */
+        if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_HT) {
+                an->maxampdu = 1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
+                                     sta->ht_cap.ampdu_factor);
+                density = ath9k_parse_mpdudensity(sta->ht_cap.ampdu_density);
+                an->mpdudensity = density;
+        }
+
         txtid->state |= AGGR_ADDBA_PROGRESS;
         txtid->paused = true;
         *ssn = txtid->seq_start = txtid->seq_next;
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 67c13af6f20..c06b6cb5c91 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -877,6 +877,10 @@ struct b43_wl {
          * from the mac80211 subsystem. */
         u16 mac80211_initially_registered_queues;
 
+        /* Set this if we call ieee80211_register_hw() and check if we call
+         * ieee80211_unregister_hw(). */
+        bool hw_registred;
+
         /* We can only have one operating interface (802.11 core)
          * at a time. General information about this interface follows.
          */
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 5a39b226b2e..acd03a4f973 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -2437,6 +2437,7 @@ start_ieee80211:
         err = ieee80211_register_hw(wl->hw);
         if (err)
                 goto err_one_core_detach;
+        wl->hw_registred = true;
         b43_leds_register(wl->current_dev);
         goto out;
 
@@ -5299,6 +5300,7 @@ static struct b43_wl *b43_wireless_init(struct b43_bus_dev *dev)
 
         hw->queues = modparam_qos ? B43_QOS_QUEUE_NUM : 1;
         wl->mac80211_initially_registered_queues = hw->queues;
+        wl->hw_registred = false;
         hw->max_rates = 2;
         SET_IEEE80211_DEV(hw, dev->dev);
         if (is_valid_ether_addr(sprom->et1mac))
@@ -5370,12 +5372,15 @@ static void b43_bcma_remove(struct bcma_device *core)
          * as the ieee80211 unreg will destroy the workqueue. */
         cancel_work_sync(&wldev->restart_work);
 
-        /* Restore the queues count before unregistering, because firmware detect
-         * might have modified it. Restoring is important, so the networking
-         * stack can properly free resources. */
-        wl->hw->queues = wl->mac80211_initially_registered_queues;
-        b43_leds_stop(wldev);
-        ieee80211_unregister_hw(wl->hw);
+        B43_WARN_ON(!wl);
+        if (wl->current_dev == wldev && wl->hw_registred) {
+                /* Restore the queues count before unregistering, because firmware detect
+                 * might have modified it. Restoring is important, so the networking
+                 * stack can properly free resources. */
+                wl->hw->queues = wl->mac80211_initially_registered_queues;
+                b43_leds_stop(wldev);
+                ieee80211_unregister_hw(wl->hw);
+        }
 
         b43_one_core_detach(wldev->dev);
 
@@ -5446,7 +5451,7 @@ static void b43_ssb_remove(struct ssb_device *sdev)
         cancel_work_sync(&wldev->restart_work);
 
         B43_WARN_ON(!wl);
-        if (wl->current_dev == wldev) {
+        if (wl->current_dev == wldev && wl->hw_registred) {
                 /* Restore the queues count before unregistering, because firmware detect
                  * might have modified it. Restoring is important, so the networking
                  * stack can properly free resources. */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index abb48032753..9d5170b6df5 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/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 82f51dbd0d6..49765d34b4e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -44,6 +44,7 @@
 
 #define SDIO_DEVICE_ID_BROADCOM_4329    0x4329
 #define SDIO_DEVICE_ID_BROADCOM_4330    0x4330
+#define SDIO_DEVICE_ID_BROADCOM_4334    0x4334
 
 #define SDIO_FUNC1_BLOCKSIZE            64
 #define SDIO_FUNC2_BLOCKSIZE            512
@@ -52,6 +53,7 @@
 static const struct sdio_device_id brcmf_sdmmc_ids[] = {
         {SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4329)},
         {SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4330)},
+        {SDIO_DEVICE(SDIO_VENDOR_ID_BROADCOM, SDIO_DEVICE_ID_BROADCOM_4334)},
         { /* end: all zeroes */ },
 };
 MODULE_DEVICE_TABLE(sdio, brcmf_sdmmc_ids);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index 9f637014486..a11fe54f595 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_bus.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
index 366916494be..537f499cc5d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
@@ -36,6 +36,13 @@ struct dngl_stats {
         unsigned long multicast;        /* multicast packets received */
 };
 
+struct brcmf_bus_dcmd {
+        char *name;
+        char *param;
+        int param_len;
+        struct list_head list;
+};
+
 /* interface structure between common and bus layer */
 struct brcmf_bus {
         u8 type;                /* bus type */
@@ -50,6 +57,7 @@ struct brcmf_bus {
         unsigned long tx_realloc;       /* Tx packets realloced for headroom */
         struct dngl_stats dstats;       /* Stats for dongle-based data */
         u8 align;               /* bus alignment requirement */
+        struct list_head dcmd_list;
 
         /* interface functions pointers */
         /* Stop bus module: clear pending frames, disable data flow */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 236cb9fa460..2621dd3d7dc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -800,13 +800,13 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
         char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];      /*  Room for
                                  "event_msgs" + '\0' + bitvec  */
         char buf[128], *ptr;
-        u32 dongle_align = drvr->bus_if->align;
-        u32 glom = 0;
         u32 roaming = 1;
         uint bcn_timeout = 3;
         int scan_assoc_time = 40;
         int scan_unassoc_time = 40;
         int i;
+        struct brcmf_bus_dcmd *cmdlst;
+        struct list_head *cur, *q;
 
         mutex_lock(&drvr->proto_block);
 
@@ -827,17 +827,6 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
         /* Print fw version info */
         brcmf_dbg(ERROR, "Firmware version = %s\n", buf);
 
-        /* Match Host and Dongle rx alignment */
-        brcmf_c_mkiovar("bus:txglomalign", (char *)&dongle_align, 4, iovbuf,
-                    sizeof(iovbuf));
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-                                  sizeof(iovbuf));
-
-        /* disable glom option per default */
-        brcmf_c_mkiovar("bus:txglom", (char *)&glom, 4, iovbuf, sizeof(iovbuf));
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-                                  sizeof(iovbuf));
-
         /* Setup timeout if Beacons are lost and roam is off to report
                  link down */
         brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout, 4, iovbuf,
@@ -874,6 +863,20 @@ int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr)
                                                  0, true);
         }
 
+        /* set bus specific command if there is any */
+        list_for_each_safe(cur, q, &drvr->bus_if->dcmd_list) {
+                cmdlst = list_entry(cur, struct brcmf_bus_dcmd, list);
+                if (cmdlst->name && cmdlst->param && cmdlst->param_len) {
+                        brcmf_c_mkiovar(cmdlst->name, cmdlst->param,
+                                        cmdlst->param_len, iovbuf,
+                                        sizeof(iovbuf));
+                        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
+                                                 iovbuf, sizeof(iovbuf));
+                }
+                list_del(cur);
+                kfree(cmdlst);
+        }
+
         mutex_unlock(&drvr->proto_block);
 
         return 0;
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 00000000000..7f89540b56d
--- /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 a2c4576cf9f..b784920532d 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 8933f9b31a9..57bf1d7ee80 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) {
@@ -1017,6 +1020,8 @@ int brcmf_attach(uint bus_hdrlen, struct device *dev)
         INIT_WORK(&drvr->setmacaddr_work, _brcmf_set_mac_address);
         INIT_WORK(&drvr->multicast_work, _brcmf_set_multicast_list);
 
+        INIT_LIST_HEAD(&drvr->bus_if->dcmd_list);
+
         return ret;
 
 fail:
@@ -1123,6 +1128,7 @@ void brcmf_detach(struct device *dev)
                 brcmf_proto_detach(drvr);
         }
 
+        brcmf_debugfs_detach(drvr);
         bus_if->drvr = NULL;
         kfree(drvr);
 }
@@ -1192,6 +1198,8 @@ exit:
 
 static void brcmf_driver_init(struct work_struct *work)
 {
+        brcmf_debugfs_init();
+
 #ifdef CONFIG_BRCMFMAC_SDIO
         brcmf_sdio_init();
 #endif
@@ -1219,6 +1227,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 1dbf2be478c..5c868bac1c1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -31,6 +31,8 @@
 #include <linux/firmware.h>
 #include <linux/module.h>
 #include <linux/bcma/bcma.h>
+#include <linux/debugfs.h>
+#include <linux/vmalloc.h>
 #include <asm/unaligned.h>
 #include <defs.h>
 #include <brcmu_wifi.h>
@@ -48,6 +50,9 @@
 
 #define CBUF_LEN        (128)
 
+/* Device console log buffer state */
+#define CONSOLE_BUFFER_MAX      2024
+
 struct rte_log_le {
         __le32 buf;             /* Can't be pointer on (64-bit) hosts */
         __le32 buf_size;
@@ -281,7 +286,7 @@ struct rte_console {
  * Shared structure between dongle and the host.
  * The structure contains pointers to trap or assert information.
  */
-#define SDPCM_SHARED_VERSION       0x0002
+#define SDPCM_SHARED_VERSION       0x0003
 #define SDPCM_SHARED_VERSION_MASK  0x00FF
 #define SDPCM_SHARED_ASSERT_BUILT  0x0100
 #define SDPCM_SHARED_ASSERT        0x0200
@@ -428,6 +433,29 @@ struct brcmf_console {
         u8 *buf;                /* Log buffer (host copy) */
         uint last;              /* Last buffer read index */
 };
+
+struct brcmf_trap_info {
+        __le32          type;
+        __le32          epc;
+        __le32          cpsr;
+        __le32          spsr;
+        __le32          r0;     /* a1 */
+        __le32          r1;     /* a2 */
+        __le32          r2;     /* a3 */
+        __le32          r3;     /* a4 */
+        __le32          r4;     /* v1 */
+        __le32          r5;     /* v2 */
+        __le32          r6;     /* v3 */
+        __le32          r7;     /* v4 */
+        __le32          r8;     /* v5 */
+        __le32          r9;     /* sb/v6 */
+        __le32          r10;    /* sl/v7 */
+        __le32          r11;    /* fp/v8 */
+        __le32          r12;    /* ip */
+        __le32          r13;    /* sp */
+        __le32          r14;    /* lr */
+        __le32          pc;     /* r15 */
+};
 #endif                          /* DEBUG */
 
 struct sdpcm_shared {
@@ -439,6 +467,7 @@ struct sdpcm_shared {
         u32 console_addr;       /* Address of struct rte_console */
         u32 msgtrace_addr;
         u8 tag[32];
+        u32 brpt_addr;
 };
 
 struct sdpcm_shared_le {
@@ -450,6 +479,7 @@ struct sdpcm_shared_le {
         __le32 console_addr;    /* Address of struct rte_console */
         __le32 msgtrace_addr;
         u8 tag[32];
+        __le32 brpt_addr;
 };
 
 
@@ -502,12 +532,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 +542,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 +556,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 +581,7 @@ struct brcmf_sdio {
         u32 fw_ptr;
 
         bool txoff;             /* Transmit flow-controlled */
+        struct brcmf_sdio_count sdcnt;
 };
 
 /* clkstate */
@@ -945,7 +944,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 +983,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 +1020,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 +1028,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 +1046,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 +1242,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 +1255,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 +1311,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 +1375,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 +1401,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 +1440,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 +1525,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 +1535,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 +1588,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 +1629,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 +1745,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 +1753,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 +1766,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 +1813,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 +1839,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 +1860,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 +1879,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 +1936,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 +1959,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 +2146,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 +2165,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 +2223,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 +2416,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 +2425,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 +2444,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 +2509,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 +2525,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 +2656,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 +2844,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 +2858,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 +2975,324 @@ 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 inline bool brcmf_sdio_valid_shared_address(u32 addr)
+{
+        return !(addr == 0 || ((~addr >> 16) & 0xffff) == (addr & 0xffff));
+}
+
+static int brcmf_sdio_readshared(struct brcmf_sdio *bus,
+                                 struct sdpcm_shared *sh)
+{
+        u32 addr;
+        int rv;
+        u32 shaddr = 0;
+        struct sdpcm_shared_le sh_le;
+        __le32 addr_le;
+
+        shaddr = bus->ramsize - 4;
+
+        /*
+         * Read last word in socram to determine
+         * address of sdpcm_shared structure
+         */
+        rv = brcmf_sdbrcm_membytes(bus, false, shaddr,
+                                   (u8 *)&addr_le, 4);
+        if (rv < 0)
+                return rv;
+
+        addr = le32_to_cpu(addr_le);
+
+        brcmf_dbg(INFO, "sdpcm_shared address 0x%08X\n", addr);
+
+        /*
+         * Check if addr is valid.
+         * NVRAM length at the end of memory should have been overwritten.
+         */
+        if (!brcmf_sdio_valid_shared_address(addr)) {
+                        brcmf_dbg(ERROR, "invalid sdpcm_shared address 0x%08X\n",
+                                  addr);
+                        return -EINVAL;
+        }
+
+        /* Read hndrte_shared structure */
+        rv = brcmf_sdbrcm_membytes(bus, false, addr, (u8 *)&sh_le,
+                                   sizeof(struct sdpcm_shared_le));
+        if (rv < 0)
+                return rv;
+
+        /* Endianness */
+        sh->flags = le32_to_cpu(sh_le.flags);
+        sh->trap_addr = le32_to_cpu(sh_le.trap_addr);
+        sh->assert_exp_addr = le32_to_cpu(sh_le.assert_exp_addr);
+        sh->assert_file_addr = le32_to_cpu(sh_le.assert_file_addr);
+        sh->assert_line = le32_to_cpu(sh_le.assert_line);
+        sh->console_addr = le32_to_cpu(sh_le.console_addr);
+        sh->msgtrace_addr = le32_to_cpu(sh_le.msgtrace_addr);
+
+        if ((sh->flags & SDPCM_SHARED_VERSION_MASK) != SDPCM_SHARED_VERSION) {
+                brcmf_dbg(ERROR,
+                          "sdpcm_shared version mismatch: dhd %d dongle %d\n",
+                          SDPCM_SHARED_VERSION,
+                          sh->flags & SDPCM_SHARED_VERSION_MASK);
+                return -EPROTO;
+        }
+
+        return 0;
+}
+
+static int brcmf_sdio_dump_console(struct brcmf_sdio *bus,
+                                   struct sdpcm_shared *sh, char __user *data,
+                                   size_t count)
+{
+        u32 addr, console_ptr, console_size, console_index;
+        char *conbuf = NULL;
+        __le32 sh_val;
+        int rv;
+        loff_t pos = 0;
+        int nbytes = 0;
+
+        /* obtain console information from device memory */
+        addr = sh->console_addr + offsetof(struct rte_console, log_le);
+        rv = brcmf_sdbrcm_membytes(bus, false, addr,
+                        (u8 *)&sh_val, sizeof(u32));
+        if (rv < 0)
+                return rv;
+        console_ptr = le32_to_cpu(sh_val);
+
+        addr = sh->console_addr + offsetof(struct rte_console, log_le.buf_size);
+        rv = brcmf_sdbrcm_membytes(bus, false, addr,
+                        (u8 *)&sh_val, sizeof(u32));
+        if (rv < 0)
+                return rv;
+        console_size = le32_to_cpu(sh_val);
+
+        addr = sh->console_addr + offsetof(struct rte_console, log_le.idx);
+        rv = brcmf_sdbrcm_membytes(bus, false, addr,
+                        (u8 *)&sh_val, sizeof(u32));
+        if (rv < 0)
+                return rv;
+        console_index = le32_to_cpu(sh_val);
+
+        /* allocate buffer for console data */
+        if (console_size <= CONSOLE_BUFFER_MAX)
+                conbuf = vzalloc(console_size+1);
+
+        if (!conbuf)
+                return -ENOMEM;
+
+        /* obtain the console data from device */
+        conbuf[console_size] = '\0';
+        rv = brcmf_sdbrcm_membytes(bus, false, console_ptr, (u8 *)conbuf,
+                                   console_size);
+        if (rv < 0)
+                goto done;
+
+        rv = simple_read_from_buffer(data, count, &pos,
+                                     conbuf + console_index,
+                                     console_size - console_index);
+        if (rv < 0)
+                goto done;
+
+        nbytes = rv;
+        if (console_index > 0) {
+                pos = 0;
+                rv = simple_read_from_buffer(data+nbytes, count, &pos,
+                                             conbuf, console_index - 1);
+                if (rv < 0)
+                        goto done;
+                rv += nbytes;
+        }
+done:
+        vfree(conbuf);
+        return rv;
+}
+
+static int brcmf_sdio_trap_info(struct brcmf_sdio *bus, struct sdpcm_shared *sh,
+                                char __user *data, size_t count)
+{
+        int error, res;
+        char buf[350];
+        struct brcmf_trap_info tr;
+        int nbytes;
+        loff_t pos = 0;
+
+        if ((sh->flags & SDPCM_SHARED_TRAP) == 0)
+                return 0;
+
+        error = brcmf_sdbrcm_membytes(bus, false, sh->trap_addr, (u8 *)&tr,
+                                      sizeof(struct brcmf_trap_info));
+        if (error < 0)
+                return error;
+
+        nbytes = brcmf_sdio_dump_console(bus, sh, data, count);
+        if (nbytes < 0)
+                return nbytes;
+
+        res = scnprintf(buf, sizeof(buf),
+                        "dongle trap info: type 0x%x @ epc 0x%08x\n"
+                        "  cpsr 0x%08x spsr 0x%08x sp 0x%08x\n"
+                        "  lr   0x%08x pc   0x%08x offset 0x%x\n"
+                        "  r0   0x%08x r1   0x%08x r2 0x%08x r3 0x%08x\n"
+                        "  r4   0x%08x r5   0x%08x r6 0x%08x r7 0x%08x\n",
+                        le32_to_cpu(tr.type), le32_to_cpu(tr.epc),
+                        le32_to_cpu(tr.cpsr), le32_to_cpu(tr.spsr),
+                        le32_to_cpu(tr.r13), le32_to_cpu(tr.r14),
+                        le32_to_cpu(tr.pc), sh->trap_addr,
+                        le32_to_cpu(tr.r0), le32_to_cpu(tr.r1),
+                        le32_to_cpu(tr.r2), le32_to_cpu(tr.r3),
+                        le32_to_cpu(tr.r4), le32_to_cpu(tr.r5),
+                        le32_to_cpu(tr.r6), le32_to_cpu(tr.r7));
+
+        error = simple_read_from_buffer(data+nbytes, count, &pos, buf, res);
+        if (error < 0)
+                return error;
+
+        nbytes += error;
+        return nbytes;
+}
+
+static int brcmf_sdio_assert_info(struct brcmf_sdio *bus,
+                                  struct sdpcm_shared *sh, char __user *data,
+                                  size_t count)
+{
+        int error = 0;
+        char buf[200];
+        char file[80] = "?";
+        char expr[80] = "<???>";
+        int res;
+        loff_t pos = 0;
+
+        if ((sh->flags & SDPCM_SHARED_ASSERT_BUILT) == 0) {
+                brcmf_dbg(INFO, "firmware not built with -assert\n");
+                return 0;
+        } else if ((sh->flags & SDPCM_SHARED_ASSERT) == 0) {
+                brcmf_dbg(INFO, "no assert in dongle\n");
+                return 0;
+        }
+
+        if (sh->assert_file_addr != 0) {
+                error = brcmf_sdbrcm_membytes(bus, false, sh->assert_file_addr,
+                                              (u8 *)file, 80);
+                if (error < 0)
+                        return error;
+        }
+        if (sh->assert_exp_addr != 0) {
+                error = brcmf_sdbrcm_membytes(bus, false, sh->assert_exp_addr,
+                                              (u8 *)expr, 80);
+                if (error < 0)
+                        return error;
+        }
+
+        res = scnprintf(buf, sizeof(buf),
+                        "dongle assert: %s:%d: assert(%s)\n",
+                        file, sh->assert_line, expr);
+        return simple_read_from_buffer(data, count, &pos, buf, res);
+}
+
+static int brcmf_sdbrcm_checkdied(struct brcmf_sdio *bus)
+{
+        int error;
+        struct sdpcm_shared sh;
+
+        down(&bus->sdsem);
+        error = brcmf_sdio_readshared(bus, &sh);
+        up(&bus->sdsem);
+
+        if (error < 0)
+                return error;
+
+        if ((sh.flags & SDPCM_SHARED_ASSERT_BUILT) == 0)
+                brcmf_dbg(INFO, "firmware not built with -assert\n");
+        else if (sh.flags & SDPCM_SHARED_ASSERT)
+                brcmf_dbg(ERROR, "assertion in dongle\n");
+
+        if (sh.flags & SDPCM_SHARED_TRAP)
+                brcmf_dbg(ERROR, "firmware trap in dongle\n");
+
+        return 0;
+}
+
+static int brcmf_sdbrcm_died_dump(struct brcmf_sdio *bus, char __user *data,
+                                  size_t count, loff_t *ppos)
+{
+        int error = 0;
+        struct sdpcm_shared sh;
+        int nbytes = 0;
+        loff_t pos = *ppos;
+
+        if (pos != 0)
+                return 0;
+
+        down(&bus->sdsem);
+        error = brcmf_sdio_readshared(bus, &sh);
+        if (error < 0)
+                goto done;
+
+        error = brcmf_sdio_assert_info(bus, &sh, data, count);
+        if (error < 0)
+                goto done;
+
+        nbytes = error;
+        error = brcmf_sdio_trap_info(bus, &sh, data, count);
+        if (error < 0)
+                goto done;
+
+        error += nbytes;
+        *ppos += error;
+done:
+        up(&bus->sdsem);
+        return error;
+}
+
+static ssize_t brcmf_sdio_forensic_read(struct file *f, char __user *data,
+                                        size_t count, loff_t *ppos)
+{
+        struct brcmf_sdio *bus = f->private_data;
+        int res;
+
+        res = brcmf_sdbrcm_died_dump(bus, data, count, ppos);
+        if (res > 0)
+                *ppos += res;
+        return (ssize_t)res;
+}
+
+static const struct file_operations brcmf_sdio_forensic_ops = {
+        .owner = THIS_MODULE,
+        .open = simple_open,
+        .read = brcmf_sdio_forensic_read
+};
+
+static void brcmf_sdio_debugfs_create(struct brcmf_sdio *bus)
+{
+        struct brcmf_pub *drvr = bus->sdiodev->bus_if->drvr;
+        struct dentry *dentry = brcmf_debugfs_get_devdir(drvr);
+
+        if (IS_ERR_OR_NULL(dentry))
+                return;
+
+        debugfs_create_file("forensics", S_IRUGO, dentry, bus,
+                            &brcmf_sdio_forensic_ops);
+        brcmf_debugfs_create_sdio_count(drvr, &bus->sdcnt);
+}
+#else
+static int brcmf_sdbrcm_checkdied(struct brcmf_sdio *bus)
+{
+        return 0;
+}
+
+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)
 {
@@ -3009,60 +3319,27 @@ brcmf_sdbrcm_bus_rxctl(struct device *dev, unsigned char *msg, uint msglen)
                           rxlen, msglen);
         } else if (timeleft == 0) {
                 brcmf_dbg(ERROR, "resumed on timeout\n");
+                brcmf_sdbrcm_checkdied(bus);
         } else if (pending) {
                 brcmf_dbg(CTL, "cancelled\n");
                 return -ERESTARTSYS;
         } else {
                 brcmf_dbg(CTL, "resumed for unknown reason?\n");
+                brcmf_sdbrcm_checkdied(bus);
         }
 
         if (rxlen)
-                bus->rx_ctlpkts++;
+                bus->sdcnt.rx_ctlpkts++;
         else
-                bus->rx_ctlerrs++;
+                bus->sdcnt.rx_ctlerrs++;
 
         return rxlen ? (int)rxlen : -ETIMEDOUT;
 }
 
-static int brcmf_sdbrcm_downloadvars(struct brcmf_sdio *bus, void *arg, int len)
-{
-        int bcmerror = 0;
-
-        brcmf_dbg(TRACE, "Enter\n");
-
-        /* Basic sanity checks */
-        if (bus->sdiodev->bus_if->drvr_up) {
-                bcmerror = -EISCONN;
-                goto err;
-        }
-        if (!len) {
-                bcmerror = -EOVERFLOW;
-                goto err;
-        }
-
-        /* Free the old ones and replace with passed variables */
-        kfree(bus->vars);
-
-        bus->vars = kmalloc(len, GFP_ATOMIC);
-        bus->varsz = bus->vars ? len : 0;
-        if (bus->vars == NULL) {
-                bcmerror = -ENOMEM;
-                goto err;
-        }
-
-        /* Copy the passed variables, which should include the
-                 terminating double-null */
-        memcpy(bus->vars, arg, bus->varsz);
-err:
-        return bcmerror;
-}
-
 static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 {
         int bcmerror = 0;
-        u32 varsize;
         u32 varaddr;
-        u8 *vbuffer;
         u32 varsizew;
         __le32 varsizew_le;
 #ifdef DEBUG
@@ -3071,56 +3348,44 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
 
         /* Even if there are no vars are to be written, we still
                  need to set the ramsize. */
-        varsize = bus->varsz ? roundup(bus->varsz, 4) : 0;
-        varaddr = (bus->ramsize - 4) - varsize;
+        varaddr = (bus->ramsize - 4) - bus->varsz;
 
         if (bus->vars) {
-                vbuffer = kzalloc(varsize, GFP_ATOMIC);
-                if (!vbuffer)
-                        return -ENOMEM;
-
-                memcpy(vbuffer, bus->vars, bus->varsz);
-
                 /* Write the vars list */
-                bcmerror =
-                    brcmf_sdbrcm_membytes(bus, true, varaddr, vbuffer, varsize);
+                bcmerror = brcmf_sdbrcm_membytes(bus, true, varaddr,
+                                                 bus->vars, bus->varsz);
 #ifdef DEBUG
                 /* Verify NVRAM bytes */
-                brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n", varsize);
-                nvram_ularray = kmalloc(varsize, GFP_ATOMIC);
-                if (!nvram_ularray) {
-                        kfree(vbuffer);
+                brcmf_dbg(INFO, "Compare NVRAM dl & ul; varsize=%d\n",
+                          bus->varsz);
+                nvram_ularray = kmalloc(bus->varsz, GFP_ATOMIC);
+                if (!nvram_ularray)
                         return -ENOMEM;
-                }
 
                 /* Upload image to verify downloaded contents. */
-                memset(nvram_ularray, 0xaa, varsize);
+                memset(nvram_ularray, 0xaa, bus->varsz);
 
                 /* Read the vars list to temp buffer for comparison */
-                bcmerror =
-                    brcmf_sdbrcm_membytes(bus, false, varaddr, nvram_ularray,
-                                     varsize);
+                bcmerror = brcmf_sdbrcm_membytes(bus, false, varaddr,
+                                                 nvram_ularray, bus->varsz);
                 if (bcmerror) {
                         brcmf_dbg(ERROR, "error %d on reading %d nvram bytes at 0x%08x\n",
-                                  bcmerror, varsize, varaddr);
+                                  bcmerror, bus->varsz, varaddr);
                 }
                 /* Compare the org NVRAM with the one read from RAM */
-                if (memcmp(vbuffer, nvram_ularray, varsize))
+                if (memcmp(bus->vars, nvram_ularray, bus->varsz))
                         brcmf_dbg(ERROR, "Downloaded NVRAM image is corrupted\n");
                 else
                         brcmf_dbg(ERROR, "Download/Upload/Compare of NVRAM ok\n");
 
                 kfree(nvram_ularray);
 #endif                          /* DEBUG */
-
-                kfree(vbuffer);
         }
 
         /* adjust to the user specified RAM */
         brcmf_dbg(INFO, "Physical memory size: %d\n", bus->ramsize);
         brcmf_dbg(INFO, "Vars are at %d, orig varsize is %d\n",
-                  varaddr, varsize);
-        varsize = ((bus->ramsize - 4) - varaddr);
+                  varaddr, bus->varsz);
 
         /*
          * Determine the length token:
@@ -3131,13 +3396,13 @@ static int brcmf_sdbrcm_write_vars(struct brcmf_sdio *bus)
                 varsizew = 0;
                 varsizew_le = cpu_to_le32(0);
         } else {
-                varsizew = varsize / 4;
+                varsizew = bus->varsz / 4;
                 varsizew = (~varsizew << 16) | (varsizew & 0x0000FFFF);
                 varsizew_le = cpu_to_le32(varsizew);
         }
 
         brcmf_dbg(INFO, "New varsize is %d, length token=0x%08x\n",
-                  varsize, varsizew);
+                  bus->varsz, varsizew);
 
         /* Write the length token to the last word */
         bcmerror = brcmf_sdbrcm_membytes(bus, true, (bus->ramsize - 4),
@@ -3261,13 +3526,21 @@ err:
  * by two NULs.
 */
 
-static uint brcmf_process_nvram_vars(char *varbuf, uint len)
+static int brcmf_process_nvram_vars(struct brcmf_sdio *bus)
 {
+        char *varbuf;
         char *dp;
         bool findNewline;
         int column;
-        uint buf_len, n;
+        int ret = 0;
+        uint buf_len, n, len;
 
+        len = bus->firmware->size;
+        varbuf = vmalloc(len);
+        if (!varbuf)
+                return -ENOMEM;
+
+        memcpy(varbuf, bus->firmware->data, len);
         dp = varbuf;
 
         findNewline = false;
@@ -3296,56 +3569,44 @@ static uint brcmf_process_nvram_vars(char *varbuf, uint len)
                 column++;
         }
         buf_len = dp - varbuf;
-
         while (dp < varbuf + n)
                 *dp++ = 0;
 
-        return buf_len;
+        kfree(bus->vars);
+        /* roundup needed for download to device */
+        bus->varsz = roundup(buf_len + 1, 4);
+        bus->vars = kmalloc(bus->varsz, GFP_KERNEL);
+        if (bus->vars == NULL) {
+                bus->varsz = 0;
+                ret = -ENOMEM;
+                goto err;
+        }
+
+        /* copy the processed variables and add null termination */
+        memcpy(bus->vars, varbuf, buf_len);
+        bus->vars[buf_len] = 0;
+err:
+        vfree(varbuf);
+        return ret;
 }
 
 static int brcmf_sdbrcm_download_nvram(struct brcmf_sdio *bus)
 {
-        uint len;
-        char *memblock = NULL;
-        char *bufp;
         int ret;
 
+        if (bus->sdiodev->bus_if->drvr_up)
+                return -EISCONN;
+
         ret = request_firmware(&bus->firmware, BRCMF_SDIO_NV_NAME,
                                &bus->sdiodev->func[2]->dev);
         if (ret) {
                 brcmf_dbg(ERROR, "Fail to request nvram %d\n", ret);
                 return ret;
         }
-        bus->fw_ptr = 0;
-
-        memblock = kmalloc(MEMBLOCK, GFP_ATOMIC);
-        if (memblock == NULL) {
-                ret = -ENOMEM;
-                goto err;
-        }
-
-        len = brcmf_sdbrcm_get_image(memblock, MEMBLOCK, bus);
-
-        if (len > 0 && len < MEMBLOCK) {
-                bufp = (char *)memblock;
-                bufp[len] = 0;
-                len = brcmf_process_nvram_vars(bufp, len);
-                bufp += len;
-                *bufp++ = 0;
-                if (len)
-                        ret = brcmf_sdbrcm_downloadvars(bus, memblock, len + 1);
-                if (ret)
-                        brcmf_dbg(ERROR, "error downloading vars: %d\n", ret);
-        } else {
-                brcmf_dbg(ERROR, "error reading nvram file: %d\n", len);
-                ret = -EIO;
-        }
 
-err:
-        kfree(memblock);
+        ret = brcmf_process_nvram_vars(bus);
 
         release_firmware(bus->firmware);
-        bus->fw_ptr = 0;
 
         return ret;
 }
@@ -3419,7 +3680,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 +3773,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 +3815,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 +3831,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 +3843,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 */
@@ -3623,6 +3885,8 @@ static bool brcmf_sdbrcm_chipmatch(u16 chipid)
                 return true;
         if (chipid == BCM4330_CHIP_ID)
                 return true;
+        if (chipid == BCM4334_CHIP_ID)
+                return true;
         return false;
 }
 
@@ -3793,7 +4057,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;
         }
@@ -3856,6 +4120,10 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
 {
         int ret;
         struct brcmf_sdio *bus;
+        struct brcmf_bus_dcmd *dlst;
+        u32 dngl_txglom;
+        u32 dngl_txglomalign;
+        u8 idx;
 
         brcmf_dbg(TRACE, "Enter\n");
 
@@ -3938,8 +4206,29 @@ void *brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev *sdiodev)
                 goto fail;
         }
 
+        brcmf_sdio_debugfs_create(bus);
         brcmf_dbg(INFO, "completed!!\n");
 
+        /* sdio bus core specific dcmd */
+        idx = brcmf_sdio_chip_getinfidx(bus->ci, BCMA_CORE_SDIO_DEV);
+        dlst = kzalloc(sizeof(struct brcmf_bus_dcmd), GFP_KERNEL);
+        if (dlst) {
+                if (bus->ci->c_inf[idx].rev < 12) {
+                        /* for sdio core rev < 12, disable txgloming */
+                        dngl_txglom = 0;
+                        dlst->name = "bus:txglom";
+                        dlst->param = (char *)&dngl_txglom;
+                        dlst->param_len = sizeof(u32);
+                } else {
+                        /* otherwise, set txglomalign */
+                        dngl_txglomalign = bus->sdiodev->bus_if->align;
+                        dlst->name = "bus:txglomalign";
+                        dlst->param = (char *)&dngl_txglomalign;
+                        dlst->param_len = sizeof(u32);
+                }
+                list_add(&dlst->list, &bus->sdiodev->bus_if->dcmd_list);
+        }
+
         /* if firmware path present try to download and bring up bus */
         ret = brcmf_bus_start(bus->sdiodev->dev);
         if (ret != 0) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
index f8e1f1c84d0..58155e23d22 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_chip.c
@@ -403,6 +403,23 @@ static int brcmf_sdio_chip_recognition(struct brcmf_sdio_dev *sdiodev,
                 ci->c_inf[3].cib = 0x03004211;
                 ci->ramsize = 0x48000;
                 break;
+        case BCM4334_CHIP_ID:
+                ci->c_inf[0].wrapbase = 0x18100000;
+                ci->c_inf[0].cib = 0x29004211;
+                ci->c_inf[1].id = BCMA_CORE_SDIO_DEV;
+                ci->c_inf[1].base = 0x18002000;
+                ci->c_inf[1].wrapbase = 0x18102000;
+                ci->c_inf[1].cib = 0x0d004211;
+                ci->c_inf[2].id = BCMA_CORE_INTERNAL_MEM;
+                ci->c_inf[2].base = 0x18004000;
+                ci->c_inf[2].wrapbase = 0x18104000;
+                ci->c_inf[2].cib = 0x13080401;
+                ci->c_inf[3].id = BCMA_CORE_ARM_CM3;
+                ci->c_inf[3].base = 0x18003000;
+                ci->c_inf[3].wrapbase = 0x18103000;
+                ci->c_inf[3].cib = 0x07004211;
+                ci->ramsize = 0x80000;
+                break;
         default:
                 brcmf_dbg(ERROR, "chipid 0x%x is not supported\n", ci->chip);
                 return -ENODEV;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c b/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
index 95b5902bc4b..01b190a25d9 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/channel.c b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
index eb77ac3cfb6..2d365d3486d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.c
@@ -15,7 +15,9 @@
  */
 
 #include <linux/types.h>
+#include <net/cfg80211.h>
 #include <net/mac80211.h>
+#include <net/regulatory.h>
 
 #include <defs.h>
 #include "pub.h"
@@ -23,73 +25,17 @@
 #include "main.h"
 #include "stf.h"
 #include "channel.h"
+#include "mac80211_if.h"
 
 /* QDB() macro takes a dB value and converts to a quarter dB value */
 #define QDB(n) ((n) * BRCMS_TXPWR_DB_FACTOR)
 
-#define  LOCALE_CHAN_01_11       (1<<0)
-#define  LOCALE_CHAN_12_13       (1<<1)
-#define  LOCALE_CHAN_14          (1<<2)
-#define  LOCALE_SET_5G_LOW_JP1   (1<<3) /* 34-48, step 2 */
-#define  LOCALE_SET_5G_LOW_JP2   (1<<4) /* 34-46, step 4 */
-#define  LOCALE_SET_5G_LOW1      (1<<5) /* 36-48, step 4 */
-#define  LOCALE_SET_5G_LOW2      (1<<6) /* 52 */
-#define  LOCALE_SET_5G_LOW3      (1<<7) /* 56-64, step 4 */
-#define  LOCALE_SET_5G_MID1      (1<<8) /* 100-116, step 4 */
-#define  LOCALE_SET_5G_MID2      (1<<9) /* 120-124, step 4 */
-#define  LOCALE_SET_5G_MID3      (1<<10)        /* 128 */
-#define  LOCALE_SET_5G_HIGH1     (1<<11)        /* 132-140, step 4 */
-#define  LOCALE_SET_5G_HIGH2     (1<<12)        /* 149-161, step 4 */
-#define  LOCALE_SET_5G_HIGH3     (1<<13)        /* 165 */
-#define  LOCALE_CHAN_52_140_ALL  (1<<14)
-#define  LOCALE_SET_5G_HIGH4     (1<<15)        /* 184-216 */
-
-#define  LOCALE_CHAN_36_64      (LOCALE_SET_5G_LOW1 | \
-                                 LOCALE_SET_5G_LOW2 | \
-                                 LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_52_64      (LOCALE_SET_5G_LOW2 | LOCALE_SET_5G_LOW3)
-#define  LOCALE_CHAN_100_124    (LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2)
-#define  LOCALE_CHAN_100_140    (LOCALE_SET_5G_MID1 | LOCALE_SET_5G_MID2 | \
-                                  LOCALE_SET_5G_MID3 | LOCALE_SET_5G_HIGH1)
-#define  LOCALE_CHAN_149_165    (LOCALE_SET_5G_HIGH2 | LOCALE_SET_5G_HIGH3)
-#define  LOCALE_CHAN_184_216    LOCALE_SET_5G_HIGH4
-
-#define  LOCALE_CHAN_01_14      (LOCALE_CHAN_01_11 | \
-                                 LOCALE_CHAN_12_13 | \
-                                 LOCALE_CHAN_14)
-
-#define  LOCALE_RADAR_SET_NONE            0
-#define  LOCALE_RADAR_SET_1               1
-
-#define  LOCALE_RESTRICTED_NONE           0
-#define  LOCALE_RESTRICTED_SET_2G_SHORT   1
-#define  LOCALE_RESTRICTED_CHAN_165       2
-#define  LOCALE_CHAN_ALL_5G               3
-#define  LOCALE_RESTRICTED_JAPAN_LEGACY   4
-#define  LOCALE_RESTRICTED_11D_2G         5
-#define  LOCALE_RESTRICTED_11D_5G         6
-#define  LOCALE_RESTRICTED_LOW_HI         7
-#define  LOCALE_RESTRICTED_12_13_14       8
-
-#define LOCALE_2G_IDX_i                 0
-#define LOCALE_5G_IDX_11                0
 #define LOCALE_MIMO_IDX_bn              0
 #define LOCALE_MIMO_IDX_11n             0
 
-/* max of BAND_5G_PWR_LVLS and 6 for 2.4 GHz */
-#define BRCMS_MAXPWR_TBL_SIZE           6
 /* max of BAND_5G_PWR_LVLS and 14 for 2.4 GHz */
 #define BRCMS_MAXPWR_MIMO_TBL_SIZE      14
 
-/* power level in group of 2.4GHz band channels:
- * maxpwr[0] - CCK  channels [1]
- * maxpwr[1] - CCK  channels [2-10]
- * maxpwr[2] - CCK  channels [11-14]
- * maxpwr[3] - OFDM channels [1]
- * maxpwr[4] - OFDM channels [2-10]
- * maxpwr[5] - OFDM channels [11-14]
- */
-
 /* maxpwr mapping to 5GHz band channels:
  * maxpwr[0] - channels [34-48]
  * maxpwr[1] - channels [52-60]
@@ -101,16 +47,8 @@
 
 #define LC(id)  LOCALE_MIMO_IDX_ ## id
 
-#define LC_2G(id)       LOCALE_2G_IDX_ ## id
-
-#define LC_5G(id)       LOCALE_5G_IDX_ ## id
-
-#define LOCALES(band2, band5, mimo2, mimo5) \
-                {LC_2G(band2), LC_5G(band5), LC(mimo2), LC(mimo5)}
-
-/* macro to get 2.4 GHz channel group index for tx power */
-#define CHANNEL_POWER_IDX_2G_CCK(c) (((c) < 2) ? 0 : (((c) < 11) ? 1 : 2))
-#define CHANNEL_POWER_IDX_2G_OFDM(c) (((c) < 2) ? 3 : (((c) < 11) ? 4 : 5))
+#define LOCALES(mimo2, mimo5) \
+                {LC(mimo2), LC(mimo5)}
 
 /* macro to get 5 GHz channel group index for tx power */
 #define CHANNEL_POWER_IDX_5G(c) (((c) < 52) ? 0 : \
@@ -118,18 +56,37 @@
                                  (((c) < 100) ? 2 : \
                                  (((c) < 149) ? 3 : 4))))
 
-#define ISDFS_EU(fl)            (((fl) & BRCMS_DFS_EU) == BRCMS_DFS_EU)
-
-struct brcms_cm_band {
-        /* struct locale_info flags */
-        u8 locale_flags;
-        /* List of valid channels in the country */
-        struct brcms_chanvec valid_channels;
-        /* List of restricted use channels */
-        const struct brcms_chanvec *restricted_channels;
-        /* List of radar sensitive channels */
-        const struct brcms_chanvec *radar_channels;
-        u8 PAD[8];
+#define BRCM_2GHZ_2412_2462     REG_RULE(2412-10, 2462+10, 40, 0, 19, 0)
+#define BRCM_2GHZ_2467_2472     REG_RULE(2467-10, 2472+10, 20, 0, 19, \
+                                         NL80211_RRF_PASSIVE_SCAN | \
+                                         NL80211_RRF_NO_IBSS)
+
+#define BRCM_5GHZ_5180_5240     REG_RULE(5180-10, 5240+10, 40, 0, 21, \
+                                         NL80211_RRF_PASSIVE_SCAN | \
+                                         NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5260_5320     REG_RULE(5260-10, 5320+10, 40, 0, 21, \
+                                         NL80211_RRF_PASSIVE_SCAN | \
+                                         NL80211_RRF_DFS | \
+                                         NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5500_5700     REG_RULE(5500-10, 5700+10, 40, 0, 21, \
+                                         NL80211_RRF_PASSIVE_SCAN | \
+                                         NL80211_RRF_DFS | \
+                                         NL80211_RRF_NO_IBSS)
+#define BRCM_5GHZ_5745_5825     REG_RULE(5745-10, 5825+10, 40, 0, 21, \
+                                         NL80211_RRF_PASSIVE_SCAN | \
+                                         NL80211_RRF_NO_IBSS)
+
+static const struct ieee80211_regdomain brcms_regdom_x2 = {
+        .n_reg_rules = 7,
+        .alpha2 = "X2",
+        .reg_rules = {
+                BRCM_2GHZ_2412_2462,
+                BRCM_2GHZ_2467_2472,
+                BRCM_5GHZ_5180_5240,
+                BRCM_5GHZ_5260_5320,
+                BRCM_5GHZ_5500_5700,
+                BRCM_5GHZ_5745_5825,
+        }
 };
 
  /* locale per-channel tx power limits for MIMO frames
@@ -141,337 +98,23 @@ struct locale_mimo_info {
         s8 maxpwr20[BRCMS_MAXPWR_MIMO_TBL_SIZE];
         /* tx 40 MHz power limits, qdBm units */
         s8 maxpwr40[BRCMS_MAXPWR_MIMO_TBL_SIZE];
-        u8 flags;
 };
 
 /* Country names and abbreviations with locale defined from ISO 3166 */
 struct country_info {
-        const u8 locale_2G;     /* 2.4G band locale */
-        const u8 locale_5G;     /* 5G band locale */
         const u8 locale_mimo_2G;        /* 2.4G mimo info */
         const u8 locale_mimo_5G;        /* 5G mimo info */
 };
 
+struct brcms_regd {
+        struct country_info country;
+        const struct ieee80211_regdomain *regdomain;
+};
+
 struct brcms_cm_info {
         struct brcms_pub *pub;
         struct brcms_c_info *wlc;
-        char srom_ccode[BRCM_CNTRY_BUF_SZ];     /* Country Code in SROM */
-        uint srom_regrev;       /* Regulatory Rev for the SROM ccode */
-        const struct country_info *country;     /* current country def */
-        char ccode[BRCM_CNTRY_BUF_SZ];  /* current internal Country Code */
-        uint regrev;            /* current Regulatory Revision */
-        char country_abbrev[BRCM_CNTRY_BUF_SZ]; /* current advertised ccode */
-        /* per-band state (one per phy/radio) */
-        struct brcms_cm_band bandstate[MAXBANDS];
-        /* quiet channels currently for radar sensitivity or 11h support */
-        /* channels on which we cannot transmit */
-        struct brcms_chanvec quiet_channels;
-};
-
-/* locale channel and power info. */
-struct locale_info {
-        u32 valid_channels;
-        /* List of radar sensitive channels */
-        u8 radar_channels;
-        /* List of channels used only if APs are detected */
-        u8 restricted_channels;
-        /* Max tx pwr in qdBm for each sub-band */
-        s8 maxpwr[BRCMS_MAXPWR_TBL_SIZE];
-        /* Country IE advertised max tx pwr in dBm per sub-band */
-        s8 pub_maxpwr[BAND_5G_PWR_LVLS];
-        u8 flags;
-};
-
-/* Regulatory Matrix Spreadsheet (CLM) MIMO v3.7.9 */
-
-/*
- * Some common channel sets
- */
-
-/* No channels */
-static const struct brcms_chanvec chanvec_none = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* All 2.4 GHz HW channels */
-static const struct brcms_chanvec chanvec_all_2G = {
-        {0xfe, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* All 5 GHz HW channels */
-static const struct brcms_chanvec chanvec_all_5G = {
-        {0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x11, 0x11,
-         0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,
-         0x11, 0x11, 0x20, 0x22, 0x22, 0x00, 0x00, 0x11,
-         0x11, 0x11, 0x11, 0x01}
-};
-
-/*
- * Radar channel sets
- */
-
-/* Channels 52 - 64, 100 - 140 */
-static const struct brcms_chanvec radar_set1 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,  /* 52 - 60 */
-         0x01, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x11,  /* 64, 100 - 124 */
-         0x11, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  /* 128 - 140 */
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/*
- * Restricted channel sets
- */
-
-/* Channels 34, 38, 42, 46 */
-static const struct brcms_chanvec restricted_set_japan_legacy = {
-        {0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 12, 13 */
-static const struct brcms_chanvec restricted_set_2g_short = {
-        {0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channel 165 */
-static const struct brcms_chanvec restricted_chan_165 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 36 - 48 & 149 - 165 */
-static const struct brcms_chanvec restricted_low_hi = {
-        {0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x20, 0x22, 0x22, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* Channels 12 - 14 */
-static const struct brcms_chanvec restricted_set_12_13_14 = {
-        {0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-/* global memory to provide working buffer for expanded locale */
-
-static const struct brcms_chanvec *g_table_radar_set[] = {
-        &chanvec_none,
-        &radar_set1
-};
-
-static const struct brcms_chanvec *g_table_restricted_chan[] = {
-        &chanvec_none,          /* restricted_set_none */
-        &restricted_set_2g_short,
-        &restricted_chan_165,
-        &chanvec_all_5G,
-        &restricted_set_japan_legacy,
-        &chanvec_all_2G,        /* restricted_set_11d_2G */
-        &chanvec_all_5G,        /* restricted_set_11d_5G */
-        &restricted_low_hi,
-        &restricted_set_12_13_14
-};
-
-static const struct brcms_chanvec locale_2g_01_11 = {
-        {0xfe, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_2g_12_13 = {
-        {0x00, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_2g_14 = {
-        {0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW_JP1 = {
-        {0x00, 0x00, 0x00, 0x00, 0x54, 0x55, 0x01, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW_JP2 = {
-        {0x00, 0x00, 0x00, 0x00, 0x44, 0x44, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW1 = {
-        {0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x01, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW2 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_LOW3 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-         0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID1 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x10, 0x11, 0x11, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID2 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_MID3 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH1 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x10, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH2 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x20, 0x22, 0x02, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH3 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_52_140_ALL = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x11,
-         0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11,
-         0x11, 0x11, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00}
-};
-
-static const struct brcms_chanvec locale_5g_HIGH4 = {
-        {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11,
-         0x11, 0x11, 0x11, 0x11}
-};
-
-static const struct brcms_chanvec *g_table_locale_base[] = {
-        &locale_2g_01_11,
-        &locale_2g_12_13,
-        &locale_2g_14,
-        &locale_5g_LOW_JP1,
-        &locale_5g_LOW_JP2,
-        &locale_5g_LOW1,
-        &locale_5g_LOW2,
-        &locale_5g_LOW3,
-        &locale_5g_MID1,
-        &locale_5g_MID2,
-        &locale_5g_MID3,
-        &locale_5g_HIGH1,
-        &locale_5g_HIGH2,
-        &locale_5g_HIGH3,
-        &locale_5g_52_140_ALL,
-        &locale_5g_HIGH4
-};
-
-static void brcms_c_locale_add_channels(struct brcms_chanvec *target,
-                                    const struct brcms_chanvec *channels)
-{
-        u8 i;
-        for (i = 0; i < sizeof(struct brcms_chanvec); i++)
-                target->vec[i] |= channels->vec[i];
-}
-
-static void brcms_c_locale_get_channels(const struct locale_info *locale,
-                                    struct brcms_chanvec *channels)
-{
-        u8 i;
-
-        memset(channels, 0, sizeof(struct brcms_chanvec));
-
-        for (i = 0; i < ARRAY_SIZE(g_table_locale_base); i++) {
-                if (locale->valid_channels & (1 << i))
-                        brcms_c_locale_add_channels(channels,
-                                                g_table_locale_base[i]);
-        }
-}
-
-/*
- * Locale Definitions - 2.4 GHz
- */
-static const struct locale_info locale_i = {    /* locale i. channel 1 - 13 */
-        LOCALE_CHAN_01_11 | LOCALE_CHAN_12_13,
-        LOCALE_RADAR_SET_NONE,
-        LOCALE_RESTRICTED_SET_2G_SHORT,
-        {QDB(19), QDB(19), QDB(19),
-         QDB(19), QDB(19), QDB(19)},
-        {20, 20, 20, 0},
-        BRCMS_EIRP
-};
-
-/*
- * Locale Definitions - 5 GHz
- */
-static const struct locale_info locale_11 = {
-        /* locale 11. channel 36 - 48, 52 - 64, 100 - 140, 149 - 165 */
-        LOCALE_CHAN_36_64 | LOCALE_CHAN_100_140 | LOCALE_CHAN_149_165,
-        LOCALE_RADAR_SET_1,
-        LOCALE_RESTRICTED_NONE,
-        {QDB(21), QDB(21), QDB(21), QDB(21), QDB(21)},
-        {23, 23, 23, 30, 30},
-        BRCMS_EIRP | BRCMS_DFS_EU
-};
-
-static const struct locale_info *g_locale_2g_table[] = {
-        &locale_i
-};
-
-static const struct locale_info *g_locale_5g_table[] = {
-        &locale_11
+        const struct brcms_regd *world_regd;
 };
 
 /*
@@ -484,7 +127,6 @@ static const struct locale_mimo_info locale_bn = {
         {0, 0, QDB(13), QDB(13), QDB(13),
          QDB(13), QDB(13), QDB(13), QDB(13), QDB(13),
          QDB(13), 0, 0},
-        0
 };
 
 static const struct locale_mimo_info *g_mimo_2g_table[] = {
@@ -497,114 +139,20 @@ static const struct locale_mimo_info *g_mimo_2g_table[] = {
 static const struct locale_mimo_info locale_11n = {
         { /* 12.5 dBm */ 50, 50, 50, QDB(15), QDB(15)},
         {QDB(14), QDB(15), QDB(15), QDB(15), QDB(15)},
-        0
 };
 
 static const struct locale_mimo_info *g_mimo_5g_table[] = {
         &locale_11n
 };
 
-static const struct {
-        char abbrev[BRCM_CNTRY_BUF_SZ]; /* country abbreviation */
-        struct country_info country;
-} cntry_locales[] = {
+static const struct brcms_regd cntry_locales[] = {
+        /* Worldwide RoW 2, must always be at index 0 */
         {
-        "X2", LOCALES(i, 11, bn, 11n)}, /* Worldwide RoW 2 */
-};
-
-#ifdef SUPPORT_40MHZ
-/* 20MHz channel info for 40MHz pairing support */
-struct chan20_info {
-        u8 sb;
-        u8 adj_sbs;
+                .country = LOCALES(bn, 11n),
+                .regdomain = &brcms_regdom_x2,
+        },
 };
 
-/* indicates adjacent channels that are allowed for a 40 Mhz channel and
- * those that permitted by the HT
- */
-struct chan20_info chan20_info[] = {
-        /* 11b/11g */
-/* 0 */ {1, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 1 */ {2, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 2 */ {3, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 3 */ {4, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 4 */ {5, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 5 */ {6, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 6 */ {7, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 7 */ {8, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 8 */ {9, (CH_UPPER_SB | CH_LOWER_SB | CH_EWA_VALID)},
-/* 9 */ {10, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 10 */ {11, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 11 */ {12, (CH_LOWER_SB)},
-/* 12 */ {13, (CH_LOWER_SB)},
-/* 13 */ {14, (CH_LOWER_SB)},
-
-/* 11a japan high */
-/* 14 */ {34, (CH_UPPER_SB)},
-/* 15 */ {38, (CH_LOWER_SB)},
-/* 16 */ {42, (CH_LOWER_SB)},
-/* 17 */ {46, (CH_LOWER_SB)},
-
-/* 11a usa low */
-/* 18 */ {36, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 19 */ {40, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 20 */ {44, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 21 */ {48, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 22 */ {52, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 23 */ {56, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 24 */ {60, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 25 */ {64, (CH_LOWER_SB | CH_EWA_VALID)},
-
-/* 11a Europe */
-/* 26 */ {100, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 27 */ {104, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 28 */ {108, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 29 */ {112, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 30 */ {116, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 31 */ {120, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 32 */ {124, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 33 */ {128, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 34 */ {132, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 35 */ {136, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 36 */ {140, (CH_LOWER_SB)},
-
-/* 11a usa high, ref5 only */
-/* The 0x80 bit in pdiv means these are REF5, other entries are REF20 */
-/* 37 */ {149, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 38 */ {153, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 39 */ {157, (CH_UPPER_SB | CH_EWA_VALID)},
-/* 40 */ {161, (CH_LOWER_SB | CH_EWA_VALID)},
-/* 41 */ {165, (CH_LOWER_SB)},
-
-/* 11a japan */
-/* 42 */ {184, (CH_UPPER_SB)},
-/* 43 */ {188, (CH_LOWER_SB)},
-/* 44 */ {192, (CH_UPPER_SB)},
-/* 45 */ {196, (CH_LOWER_SB)},
-/* 46 */ {200, (CH_UPPER_SB)},
-/* 47 */ {204, (CH_LOWER_SB)},
-/* 48 */ {208, (CH_UPPER_SB)},
-/* 49 */ {212, (CH_LOWER_SB)},
-/* 50 */ {216, (CH_LOWER_SB)}
-};
-#endif                          /* SUPPORT_40MHZ */
-
-static const struct locale_info *brcms_c_get_locale_2g(u8 locale_idx)
-{
-        if (locale_idx >= ARRAY_SIZE(g_locale_2g_table))
-                return NULL; /* error condition */
-
-        return g_locale_2g_table[locale_idx];
-}
-
-static const struct locale_info *brcms_c_get_locale_5g(u8 locale_idx)
-{
-        if (locale_idx >= ARRAY_SIZE(g_locale_5g_table))
-                return NULL; /* error condition */
-
-        return g_locale_5g_table[locale_idx];
-}
-
 static const struct locale_mimo_info *brcms_c_get_mimo_2g(u8 locale_idx)
 {
         if (locale_idx >= ARRAY_SIZE(g_mimo_2g_table))
@@ -621,13 +169,6 @@ static const struct locale_mimo_info *brcms_c_get_mimo_5g(u8 locale_idx)
         return g_mimo_5g_table[locale_idx];
 }
 
-static int
-brcms_c_country_aggregate_map(struct brcms_cm_info *wlc_cm, const char *ccode,
-                          char *mapped_ccode, uint *mapped_regrev)
-{
-        return false;
-}
-
 /*
  * Indicates whether the country provided is valid to pass
  * to cfg80211 or not.
@@ -662,155 +203,24 @@ static bool brcms_c_country_valid(const char *ccode)
         return true;
 }
 
-/* Lookup a country info structure from a null terminated country
- * abbreviation and regrev directly with no translation.
- */
-static const struct country_info *
-brcms_c_country_lookup_direct(const char *ccode, uint regrev)
+static const struct brcms_regd *brcms_world_regd(const char *regdom, int len)
 {
-        uint size, i;
-
-        /* Should just return 0 for single locale driver. */
-        /* Keep it this way in case we add more locales. (for now anyway) */
-
-        /*
-         * all other country def arrays are for regrev == 0, so if
-         * regrev is non-zero, fail
-         */
-        if (regrev > 0)
-                return NULL;
-
-        /* find matched table entry from country code */
-        size = ARRAY_SIZE(cntry_locales);
-        for (i = 0; i < size; i++) {
-                if (strcmp(ccode, cntry_locales[i].abbrev) == 0)
-                        return &cntry_locales[i].country;
-        }
-        return NULL;
-}
-
-static const struct country_info *
-brcms_c_countrycode_map(struct brcms_cm_info *wlc_cm, const char *ccode,
-                        char *mapped_ccode, uint *mapped_regrev)
-{
-        struct brcms_c_info *wlc = wlc_cm->wlc;
-        const struct country_info *country;
-        uint srom_regrev = wlc_cm->srom_regrev;
-        const char *srom_ccode = wlc_cm->srom_ccode;
-        int mapped;
-
-        /* check for currently supported ccode size */
-        if (strlen(ccode) > (BRCM_CNTRY_BUF_SZ - 1)) {
-                wiphy_err(wlc->wiphy, "wl%d: %s: ccode \"%s\" too long for "
-                          "match\n", wlc->pub->unit, __func__, ccode);
-                return NULL;
-        }
-
-        /* default mapping is the given ccode and regrev 0 */
-        strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
-        *mapped_regrev = 0;
-
-        /* If the desired country code matches the srom country code,
-         * then the mapped country is the srom regulatory rev.
-         * Otherwise look for an aggregate mapping.
-         */
-        if (!strcmp(srom_ccode, ccode)) {
-                *mapped_regrev = srom_regrev;
-                mapped = 0;
-                wiphy_err(wlc->wiphy, "srom_code == ccode %s\n", __func__);
-        } else {
-                mapped =
-                    brcms_c_country_aggregate_map(wlc_cm, ccode, mapped_ccode,
-                                              mapped_regrev);
-        }
-
-        /* find the matching built-in country definition */
-        country = brcms_c_country_lookup_direct(mapped_ccode, *mapped_regrev);
-
-        /* if there is not an exact rev match, default to rev zero */
-        if (country == NULL && *mapped_regrev != 0) {
-                *mapped_regrev = 0;
-                country =
-                    brcms_c_country_lookup_direct(mapped_ccode, *mapped_regrev);
-        }
-
-        return country;
-}
-
-/* Lookup a country info structure from a null terminated country code
- * The lookup is case sensitive.
- */
-static const struct country_info *
-brcms_c_country_lookup(struct brcms_c_info *wlc, const char *ccode)
-{
-        const struct country_info *country;
-        char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-        uint mapped_regrev;
-
-        /*
-         * map the country code to a built-in country code, regrev, and
-         * country_info struct
-         */
-        country = brcms_c_countrycode_map(wlc->cmi, ccode, mapped_ccode,
-                                          &mapped_regrev);
-
-        return country;
-}
-
-/*
- * reset the quiet channels vector to the union
- * of the restricted and radar channel sets
- */
-static void brcms_c_quiet_channels_reset(struct brcms_cm_info *wlc_cm)
-{
-        struct brcms_c_info *wlc = wlc_cm->wlc;
-        uint i, j;
-        struct brcms_band *band;
-        const struct brcms_chanvec *chanvec;
-
-        memset(&wlc_cm->quiet_channels, 0, sizeof(struct brcms_chanvec));
-
-        band = wlc->band;
-        for (i = 0; i < wlc->pub->_nbands;
-             i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
-
-                /* initialize quiet channels for restricted channels */
-                chanvec = wlc_cm->bandstate[band->bandunit].restricted_channels;
-                for (j = 0; j < sizeof(struct brcms_chanvec); j++)
-                        wlc_cm->quiet_channels.vec[j] |= chanvec->vec[j];
+        const struct brcms_regd *regd = NULL;
+        int i;
 
+        for (i = 0; i < ARRAY_SIZE(cntry_locales); i++) {
+                if (!strncmp(regdom, cntry_locales[i].regdomain->alpha2, len)) {
+                        regd = &cntry_locales[i];
+                        break;
+                }
         }
-}
-
-/* Is the channel valid for the current locale and current band? */
-static bool brcms_c_valid_channel20(struct brcms_cm_info *wlc_cm, uint val)
-{
-        struct brcms_c_info *wlc = wlc_cm->wlc;
 
-        return ((val < MAXCHANNEL) &&
-                isset(wlc_cm->bandstate[wlc->band->bandunit].valid_channels.vec,
-                      val));
+        return regd;
 }
 
-/* Is the channel valid for the current locale and specified band? */
-static bool brcms_c_valid_channel20_in_band(struct brcms_cm_info *wlc_cm,
-                                            uint bandunit, uint val)
-{
-        return ((val < MAXCHANNEL)
-                && isset(wlc_cm->bandstate[bandunit].valid_channels.vec, val));
-}
-
-/* Is the channel valid for the current locale? (but don't consider channels not
- *   available due to bandlocking)
- */
-static bool brcms_c_valid_channel20_db(struct brcms_cm_info *wlc_cm, uint val)
+static const struct brcms_regd *brcms_default_world_regd(void)
 {
-        struct brcms_c_info *wlc = wlc_cm->wlc;
-
-        return brcms_c_valid_channel20(wlc->cmi, val) ||
-                (!wlc->bandlocked
-                 && brcms_c_valid_channel20_in_band(wlc->cmi,
-                                                    OTHERBANDUNIT(wlc), val));
+        return &cntry_locales[0];
 }
 
 /* JP, J1 - J10 are Japan ccodes */
@@ -820,12 +230,6 @@ static bool brcms_c_japan_ccode(const char *ccode)
                 (ccode[1] == 'P' || (ccode[1] >= '1' && ccode[1] <= '9')));
 }
 
-/* Returns true if currently set country is Japan or variant */
-static bool brcms_c_japan(struct brcms_c_info *wlc)
-{
-        return brcms_c_japan_ccode(wlc->cmi->country_abbrev);
-}
-
 static void
 brcms_c_channel_min_txpower_limits_with_local_constraint(
                 struct brcms_cm_info *wlc_cm, struct txpwr_limits *txpwr,
@@ -901,140 +305,16 @@ brcms_c_channel_min_txpower_limits_with_local_constraint(
 
 }
 
-/* Update the radio state (enable/disable) and tx power targets
- * based on a new set of channel/regulatory information
- */
-static void brcms_c_channels_commit(struct brcms_cm_info *wlc_cm)
-{
-        struct brcms_c_info *wlc = wlc_cm->wlc;
-        uint chan;
-        struct txpwr_limits txpwr;
-
-        /* search for the existence of any valid channel */
-        for (chan = 0; chan < MAXCHANNEL; chan++) {
-                if (brcms_c_valid_channel20_db(wlc->cmi, chan))
-                        break;
-        }
-        if (chan == MAXCHANNEL)
-                chan = INVCHANNEL;
-
-        /*
-         * based on the channel search above, set or
-         * clear WL_RADIO_COUNTRY_DISABLE.
-         */
-        if (chan == INVCHANNEL) {
-                /*
-                 * country/locale with no valid channels, set
-                 * the radio disable bit
-                 */
-                mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-                wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\" "
-                          "nbands %d bandlocked %d\n", wlc->pub->unit,
-                          __func__, wlc_cm->country_abbrev, wlc->pub->_nbands,
-                          wlc->bandlocked);
-        } else if (mboolisset(wlc->pub->radio_disabled,
-                              WL_RADIO_COUNTRY_DISABLE)) {
-                /*
-                 * country/locale with valid channel, clear
-                 * the radio disable bit
-                 */
-                mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
-        }
-
-        /*
-         * Now that the country abbreviation is set, if the radio supports 2G,
-         * then set channel 14 restrictions based on the new locale.
-         */
-        if (wlc->pub->_nbands > 1 || wlc->band->bandtype == BRCM_BAND_2G)
-                wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
-                                                     brcms_c_japan(wlc) ? true :
-                                                     false);
-
-        if (wlc->pub->up && chan != INVCHANNEL) {
-                brcms_c_channel_reg_limits(wlc_cm, wlc->chanspec, &txpwr);
-                brcms_c_channel_min_txpower_limits_with_local_constraint(wlc_cm,
-                        &txpwr, BRCMS_TXPWR_MAX);
-                wlc_phy_txpower_limit_set(wlc->band->pi, &txpwr, wlc->chanspec);
-        }
-}
-
-static int
-brcms_c_channels_init(struct brcms_cm_info *wlc_cm,
-                      const struct country_info *country)
-{
-        struct brcms_c_info *wlc = wlc_cm->wlc;
-        uint i, j;
-        struct brcms_band *band;
-        const struct locale_info *li;
-        struct brcms_chanvec sup_chan;
-        const struct locale_mimo_info *li_mimo;
-
-        band = wlc->band;
-        for (i = 0; i < wlc->pub->_nbands;
-             i++, band = wlc->bandstate[OTHERBANDUNIT(wlc)]) {
-
-                li = (band->bandtype == BRCM_BAND_5G) ?
-                    brcms_c_get_locale_5g(country->locale_5G) :
-                    brcms_c_get_locale_2g(country->locale_2G);
-                wlc_cm->bandstate[band->bandunit].locale_flags = li->flags;
-                li_mimo = (band->bandtype == BRCM_BAND_5G) ?
-                    brcms_c_get_mimo_5g(country->locale_mimo_5G) :
-                    brcms_c_get_mimo_2g(country->locale_mimo_2G);
-
-                /* merge the mimo non-mimo locale flags */
-                wlc_cm->bandstate[band->bandunit].locale_flags |=
-                    li_mimo->flags;
-
-                wlc_cm->bandstate[band->bandunit].restricted_channels =
-                    g_table_restricted_chan[li->restricted_channels];
-                wlc_cm->bandstate[band->bandunit].radar_channels =
-                    g_table_radar_set[li->radar_channels];
-
-                /*
-                 * set the channel availability, masking out the channels
-                 * that may not be supported on this phy.
-                 */
-                wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
-                                              &sup_chan);
-                brcms_c_locale_get_channels(li,
-                                        &wlc_cm->bandstate[band->bandunit].
-                                        valid_channels);
-                for (j = 0; j < sizeof(struct brcms_chanvec); j++)
-                        wlc_cm->bandstate[band->bandunit].valid_channels.
-                            vec[j] &= sup_chan.vec[j];
-        }
-
-        brcms_c_quiet_channels_reset(wlc_cm);
-        brcms_c_channels_commit(wlc_cm);
-
-        return 0;
-}
-
 /*
  * set the driver's current country and regulatory information
  * using a country code as the source. Look up built in country
  * information found with the country code.
  */
 static void
-brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
-                       const char *country_abbrev,
-                       const char *ccode, uint regrev,
-                       const struct country_info *country)
+brcms_c_set_country(struct brcms_cm_info *wlc_cm,
+                    const struct brcms_regd *regd)
 {
-        const struct locale_info *locale;
         struct brcms_c_info *wlc = wlc_cm->wlc;
-        char prev_country_abbrev[BRCM_CNTRY_BUF_SZ];
-
-        /* save current country state */
-        wlc_cm->country = country;
-
-        memset(&prev_country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-        strncpy(prev_country_abbrev, wlc_cm->country_abbrev,
-                BRCM_CNTRY_BUF_SZ - 1);
-
-        strncpy(wlc_cm->country_abbrev, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
-        strncpy(wlc_cm->ccode, ccode, BRCM_CNTRY_BUF_SZ - 1);
-        wlc_cm->regrev = regrev;
 
         if ((wlc->pub->_n_enab & SUPPORT_11N) !=
             wlc->protection->nmode_user)
@@ -1042,75 +322,19 @@ brcms_c_set_country_common(struct brcms_cm_info *wlc_cm,
 
         brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_2G_INDEX]);
         brcms_c_stf_ss_update(wlc, wlc->bandstate[BAND_5G_INDEX]);
-        /* set or restore gmode as required by regulatory */
-        locale = brcms_c_get_locale_2g(country->locale_2G);
-        if (locale && (locale->flags & BRCMS_NO_OFDM))
-                brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
-        else
-                brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
 
-        brcms_c_channels_init(wlc_cm, country);
+        brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
 
         return;
 }
 
-static int
-brcms_c_set_countrycode_rev(struct brcms_cm_info *wlc_cm,
-                        const char *country_abbrev,
-                        const char *ccode, int regrev)
-{
-        const struct country_info *country;
-        char mapped_ccode[BRCM_CNTRY_BUF_SZ];
-        uint mapped_regrev;
-
-        /* if regrev is -1, lookup the mapped country code,
-         * otherwise use the ccode and regrev directly
-         */
-        if (regrev == -1) {
-                /*
-                 * map the country code to a built-in country
-                 * code, regrev, and country_info
-                 */
-                country =
-                    brcms_c_countrycode_map(wlc_cm, ccode, mapped_ccode,
-                                        &mapped_regrev);
-        } else {
-                /* find the matching built-in country definition */
-                country = brcms_c_country_lookup_direct(ccode, regrev);
-                strncpy(mapped_ccode, ccode, BRCM_CNTRY_BUF_SZ);
-                mapped_regrev = regrev;
-        }
-
-        if (country == NULL)
-                return -EINVAL;
-
-        /* set the driver state for the country */
-        brcms_c_set_country_common(wlc_cm, country_abbrev, mapped_ccode,
-                               mapped_regrev, country);
-
-        return 0;
-}
-
-/*
- * set the driver's current country and regulatory information using
- * a country code as the source. Lookup built in country information
- * found with the country code.
- */
-static int
-brcms_c_set_countrycode(struct brcms_cm_info *wlc_cm, const char *ccode)
-{
-        char country_abbrev[BRCM_CNTRY_BUF_SZ];
-        strncpy(country_abbrev, ccode, BRCM_CNTRY_BUF_SZ);
-        return brcms_c_set_countrycode_rev(wlc_cm, country_abbrev, ccode, -1);
-}
-
 struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
 {
         struct brcms_cm_info *wlc_cm;
-        char country_abbrev[BRCM_CNTRY_BUF_SZ];
-        const struct country_info *country;
         struct brcms_pub *pub = wlc->pub;
         struct ssb_sprom *sprom = &wlc->hw->d11core->bus->sprom;
+        const char *ccode = sprom->alpha2;
+        int ccode_len = sizeof(sprom->alpha2);
 
         BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
@@ -1122,24 +346,27 @@ struct brcms_cm_info *brcms_c_channel_mgr_attach(struct brcms_c_info *wlc)
         wlc->cmi = wlc_cm;
 
         /* store the country code for passing up as a regulatory hint */
-        if (sprom->alpha2 && brcms_c_country_valid(sprom->alpha2))
-                strncpy(wlc->pub->srom_ccode, sprom->alpha2, sizeof(sprom->alpha2));
+        wlc_cm->world_regd = brcms_world_regd(ccode, ccode_len);
+        if (brcms_c_country_valid(ccode))
+                strncpy(wlc->pub->srom_ccode, ccode, ccode_len);
 
         /*
-         * internal country information which must match
-         * regulatory constraints in firmware
+         * If no custom world domain is found in the SROM, use the
+         * default "X2" domain.
          */
-        memset(country_abbrev, 0, BRCM_CNTRY_BUF_SZ);
-        strncpy(country_abbrev, "X2", sizeof(country_abbrev) - 1);
-        country = brcms_c_country_lookup(wlc, country_abbrev);
+        if (!wlc_cm->world_regd) {
+                wlc_cm->world_regd = brcms_default_world_regd();
+                ccode = wlc_cm->world_regd->regdomain->alpha2;
+                ccode_len = BRCM_CNTRY_BUF_SZ - 1;
+        }
 
         /* save default country for exiting 11d regulatory mode */
-        strncpy(wlc->country_default, country_abbrev, BRCM_CNTRY_BUF_SZ - 1);
+        strncpy(wlc->country_default, ccode, ccode_len);
 
         /* initialize autocountry_default to driver default */
-        strncpy(wlc->autocountry_default, "X2", BRCM_CNTRY_BUF_SZ - 1);
+        strncpy(wlc->autocountry_default, ccode, ccode_len);
 
-        brcms_c_set_countrycode(wlc_cm, country_abbrev);
+        brcms_c_set_country(wlc_cm, wlc_cm->world_regd);
 
         return wlc_cm;
 }
@@ -1149,31 +376,15 @@ void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm)
         kfree(wlc_cm);
 }
 
-u8
-brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
-                                     uint bandunit)
-{
-        return wlc_cm->bandstate[bandunit].locale_flags;
-}
-
-static bool
-brcms_c_quiet_chanspec(struct brcms_cm_info *wlc_cm, u16 chspec)
-{
-        return (wlc_cm->wlc->pub->_n_enab & SUPPORT_11N) &&
-                CHSPEC_IS40(chspec) ?
-                (isset(wlc_cm->quiet_channels.vec,
-                       lower_20_sb(CHSPEC_CHANNEL(chspec))) ||
-                 isset(wlc_cm->quiet_channels.vec,
-                       upper_20_sb(CHSPEC_CHANNEL(chspec)))) :
-                isset(wlc_cm->quiet_channels.vec, CHSPEC_CHANNEL(chspec));
-}
-
 void
 brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
                          u8 local_constraint_qdbm)
 {
         struct brcms_c_info *wlc = wlc_cm->wlc;
+        struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
+        const struct ieee80211_reg_rule *reg_rule;
         struct txpwr_limits txpwr;
+        int ret;
 
         brcms_c_channel_reg_limits(wlc_cm, chanspec, &txpwr);
 
@@ -1181,8 +392,15 @@ brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm, u16 chanspec,
                 wlc_cm, &txpwr, local_constraint_qdbm
         );
 
+        /* set or restore gmode as required by regulatory */
+        ret = freq_reg_info(wlc->wiphy, ch->center_freq, 0, &reg_rule);
+        if (!ret && (reg_rule->flags & NL80211_RRF_NO_OFDM))
+                brcms_c_set_gmode(wlc, GMODE_LEGACY_B, false);
+        else
+                brcms_c_set_gmode(wlc, wlc->protection->gmode_user, false);
+
         brcms_b_set_chanspec(wlc->hw, chanspec,
-                              (brcms_c_quiet_chanspec(wlc_cm, chanspec) != 0),
+                              !!(ch->flags & IEEE80211_CHAN_PASSIVE_SCAN),
                               &txpwr);
 }
 
@@ -1191,15 +409,14 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
                        struct txpwr_limits *txpwr)
 {
         struct brcms_c_info *wlc = wlc_cm->wlc;
+        struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
         uint i;
         uint chan;
         int maxpwr;
         int delta;
         const struct country_info *country;
         struct brcms_band *band;
-        const struct locale_info *li;
         int conducted_max = BRCMS_TXPWR_MAX;
-        int conducted_ofdm_max = BRCMS_TXPWR_MAX;
         const struct locale_mimo_info *li_mimo;
         int maxpwr20, maxpwr40;
         int maxpwr_idx;
@@ -1207,67 +424,35 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
 
         memset(txpwr, 0, sizeof(struct txpwr_limits));
 
-        if (!brcms_c_valid_chanspec_db(wlc_cm, chanspec)) {
-                country = brcms_c_country_lookup(wlc, wlc->autocountry_default);
-                if (country == NULL)
-                        return;
-        } else {
-                country = wlc_cm->country;
-        }
+        if (WARN_ON(!ch))
+                return;
+
+        country = &wlc_cm->world_regd->country;
 
         chan = CHSPEC_CHANNEL(chanspec);
         band = wlc->bandstate[chspec_bandunit(chanspec)];
-        li = (band->bandtype == BRCM_BAND_5G) ?
-            brcms_c_get_locale_5g(country->locale_5G) :
-            brcms_c_get_locale_2g(country->locale_2G);
-
         li_mimo = (band->bandtype == BRCM_BAND_5G) ?
             brcms_c_get_mimo_5g(country->locale_mimo_5G) :
             brcms_c_get_mimo_2g(country->locale_mimo_2G);
 
-        if (li->flags & BRCMS_EIRP) {
-                delta = band->antgain;
-        } else {
-                delta = 0;
-                if (band->antgain > QDB(6))
-                        delta = band->antgain - QDB(6); /* Excess over 6 dB */
-        }
+        delta = band->antgain;
 
-        if (li == &locale_i) {
+        if (band->bandtype == BRCM_BAND_2G)
                 conducted_max = QDB(22);
-                conducted_ofdm_max = QDB(22);
-        }
+
+        maxpwr = QDB(ch->max_power) - delta;
+        maxpwr = max(maxpwr, 0);
+        maxpwr = min(maxpwr, conducted_max);
 
         /* CCK txpwr limits for 2.4G band */
         if (band->bandtype == BRCM_BAND_2G) {
-                maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_CCK(chan)];
-
-                maxpwr = maxpwr - delta;
-                maxpwr = max(maxpwr, 0);
-                maxpwr = min(maxpwr, conducted_max);
-
                 for (i = 0; i < BRCMS_NUM_RATES_CCK; i++)
                         txpwr->cck[i] = (u8) maxpwr;
         }
 
-        /* OFDM txpwr limits for 2.4G or 5G bands */
-        if (band->bandtype == BRCM_BAND_2G)
-                maxpwr = li->maxpwr[CHANNEL_POWER_IDX_2G_OFDM(chan)];
-        else
-                maxpwr = li->maxpwr[CHANNEL_POWER_IDX_5G(chan)];
-
-        maxpwr = maxpwr - delta;
-        maxpwr = max(maxpwr, 0);
-        maxpwr = min(maxpwr, conducted_ofdm_max);
-
-        /* Keep OFDM lmit below CCK limit */
-        if (band->bandtype == BRCM_BAND_2G)
-                maxpwr = min_t(int, maxpwr, txpwr->cck[0]);
-
-        for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++)
+        for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
                 txpwr->ofdm[i] = (u8) maxpwr;
 
-        for (i = 0; i < BRCMS_NUM_RATES_OFDM; i++) {
                 /*
                  * OFDM 40 MHz SISO has the same power as the corresponding
                  * MCS0-7 rate unless overriden by the locale specific code.
@@ -1282,14 +467,9 @@ brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm, u16 chanspec,
                 txpwr->ofdm_40_cdd[i] = 0;
         }
 
-        /* MIMO/HT specific limits */
-        if (li_mimo->flags & BRCMS_EIRP) {
-                delta = band->antgain;
-        } else {
-                delta = 0;
-                if (band->antgain > QDB(6))
-                        delta = band->antgain - QDB(6); /* Excess over 6 dB */
-        }
+        delta = 0;
+        if (band->antgain > QDB(6))
+                delta = band->antgain - QDB(6); /* Excess over 6 dB */
 
         if (band->bandtype == BRCM_BAND_2G)
                 maxpwr_idx = (chan - 1);
@@ -1431,8 +611,7 @@ static bool brcms_c_chspec_malformed(u16 chanspec)
  * and they are also a legal HT combination
  */
 static bool
-brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec,
-                           bool dualband)
+brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec)
 {
         struct brcms_c_info *wlc = wlc_cm->wlc;
         u8 channel = CHSPEC_CHANNEL(chspec);
@@ -1448,59 +627,166 @@ brcms_c_valid_chanspec_ext(struct brcms_cm_info *wlc_cm, u16 chspec,
             chspec_bandunit(chspec))
                 return false;
 
-        /* Check a 20Mhz channel */
-        if (CHSPEC_IS20(chspec)) {
-                if (dualband)
-                        return brcms_c_valid_channel20_db(wlc_cm->wlc->cmi,
-                                                          channel);
-                else
-                        return brcms_c_valid_channel20(wlc_cm->wlc->cmi,
-                                                       channel);
+        return true;
+}
+
+bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, u16 chspec)
+{
+        return brcms_c_valid_chanspec_ext(wlc_cm, chspec);
+}
+
+static bool brcms_is_radar_freq(u16 center_freq)
+{
+        return center_freq >= 5260 && center_freq <= 5700;
+}
+
+static void brcms_reg_apply_radar_flags(struct wiphy *wiphy)
+{
+        struct ieee80211_supported_band *sband;
+        struct ieee80211_channel *ch;
+        int i;
+
+        sband = wiphy->bands[IEEE80211_BAND_5GHZ];
+        if (!sband)
+                return;
+
+        for (i = 0; i < sband->n_channels; i++) {
+                ch = &sband->channels[i];
+
+                if (!brcms_is_radar_freq(ch->center_freq))
+                        continue;
+
+                /*
+                 * All channels in this range should be passive and have
+                 * DFS enabled.
+                 */
+                if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+                        ch->flags |= IEEE80211_CHAN_RADAR |
+                                     IEEE80211_CHAN_NO_IBSS |
+                                     IEEE80211_CHAN_PASSIVE_SCAN;
         }
-#ifdef SUPPORT_40MHZ
-        /*
-         * We know we are now checking a 40MHZ channel, so we should
-         * only be here for NPHYS
-         */
-        if (BRCMS_ISNPHY(wlc->band) || BRCMS_ISSSLPNPHY(wlc->band)) {
-                u8 upper_sideband = 0, idx;
-                u8 num_ch20_entries =
-                    sizeof(chan20_info) / sizeof(struct chan20_info);
-
-                if (!VALID_40CHANSPEC_IN_BAND(wlc, chspec_bandunit(chspec)))
-                        return false;
-
-                if (dualband) {
-                        if (!brcms_c_valid_channel20_db(wlc->cmi,
-                                                        lower_20_sb(channel)) ||
-                            !brcms_c_valid_channel20_db(wlc->cmi,
-                                                        upper_20_sb(channel)))
-                                return false;
-                } else {
-                        if (!brcms_c_valid_channel20(wlc->cmi,
-                                                     lower_20_sb(channel)) ||
-                            !brcms_c_valid_channel20(wlc->cmi,
-                                                     upper_20_sb(channel)))
-                                return false;
+}
+
+static void
+brcms_reg_apply_beaconing_flags(struct wiphy *wiphy,
+                                enum nl80211_reg_initiator initiator)
+{
+        struct ieee80211_supported_band *sband;
+        struct ieee80211_channel *ch;
+        const struct ieee80211_reg_rule *rule;
+        int band, i, ret;
+
+        for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+                sband = wiphy->bands[band];
+                if (!sband)
+                        continue;
+
+                for (i = 0; i < sband->n_channels; i++) {
+                        ch = &sband->channels[i];
+
+                        if (ch->flags &
+                            (IEEE80211_CHAN_DISABLED | IEEE80211_CHAN_RADAR))
+                                continue;
+
+                        if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
+                                ret = freq_reg_info(wiphy, ch->center_freq,
+                                                    0, &rule);
+                                if (ret)
+                                        continue;
+
+                                if (!(rule->flags & NL80211_RRF_NO_IBSS))
+                                        ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
+                                if (!(rule->flags & NL80211_RRF_PASSIVE_SCAN))
+                                        ch->flags &=
+                                                ~IEEE80211_CHAN_PASSIVE_SCAN;
+                        } else if (ch->beacon_found) {
+                                ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
+                                               IEEE80211_CHAN_PASSIVE_SCAN);
+                        }
                 }
+        }
+}
 
-                /* find the lower sideband info in the sideband array */
-                for (idx = 0; idx < num_ch20_entries; idx++) {
-                        if (chan20_info[idx].sb == lower_20_sb(channel))
-                                upper_sideband = chan20_info[idx].adj_sbs;
+static int brcms_reg_notifier(struct wiphy *wiphy,
+                              struct regulatory_request *request)
+{
+        struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
+        struct brcms_info *wl = hw->priv;
+        struct brcms_c_info *wlc = wl->wlc;
+        struct ieee80211_supported_band *sband;
+        struct ieee80211_channel *ch;
+        int band, i;
+        bool ch_found = false;
+
+        brcms_reg_apply_radar_flags(wiphy);
+
+        if (request->initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE)
+                brcms_reg_apply_beaconing_flags(wiphy, request->initiator);
+
+        /* Disable radio if all channels disallowed by regulatory */
+        for (band = 0; !ch_found && band < IEEE80211_NUM_BANDS; band++) {
+                sband = wiphy->bands[band];
+                if (!sband)
+                        continue;
+
+                for (i = 0; !ch_found && i < sband->n_channels; i++) {
+                        ch = &sband->channels[i];
+
+                        if (!(ch->flags & IEEE80211_CHAN_DISABLED))
+                                ch_found = true;
                 }
-                /* check that the lower sideband allows an upper sideband */
-                if ((upper_sideband & (CH_UPPER_SB | CH_EWA_VALID)) ==
-                    (CH_UPPER_SB | CH_EWA_VALID))
-                        return true;
-                return false;
         }
-#endif                          /* 40 MHZ */
 
-        return false;
+        if (ch_found) {
+                mboolclr(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+        } else {
+                mboolset(wlc->pub->radio_disabled, WL_RADIO_COUNTRY_DISABLE);
+                wiphy_err(wlc->wiphy, "wl%d: %s: no valid channel for \"%s\"\n",
+                          wlc->pub->unit, __func__, request->alpha2);
+        }
+
+        if (wlc->pub->_nbands > 1 || wlc->band->bandtype == BRCM_BAND_2G)
+                wlc_phy_chanspec_ch14_widefilter_set(wlc->band->pi,
+                                        brcms_c_japan_ccode(request->alpha2));
+
+        return 0;
 }
 
-bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm, u16 chspec)
+void brcms_c_regd_init(struct brcms_c_info *wlc)
 {
-        return brcms_c_valid_chanspec_ext(wlc_cm, chspec, true);
+        struct wiphy *wiphy = wlc->wiphy;
+        const struct brcms_regd *regd = wlc->cmi->world_regd;
+        struct ieee80211_supported_band *sband;
+        struct ieee80211_channel *ch;
+        struct brcms_chanvec sup_chan;
+        struct brcms_band *band;
+        int band_idx, i;
+
+        /* Disable any channels not supported by the phy */
+        for (band_idx = 0; band_idx < IEEE80211_NUM_BANDS; band_idx++) {
+                if (band_idx == IEEE80211_BAND_2GHZ)
+                        band = wlc->bandstate[BAND_2G_INDEX];
+                else
+                        band = wlc->bandstate[BAND_5G_INDEX];
+
+                /* skip if band not initialized */
+                if (band->pi == NULL)
+                        continue;
+
+                wlc_phy_chanspec_band_validch(band->pi, band->bandtype,
+                                              &sup_chan);
+
+                sband = wiphy->bands[band_idx];
+                for (i = 0; i < sband->n_channels; i++) {
+                        ch = &sband->channels[i];
+                        if (!isset(sup_chan.vec, ch->hw_value))
+                                ch->flags |= IEEE80211_CHAN_DISABLED;
+                }
+        }
+
+        wlc->wiphy->reg_notifier = brcms_reg_notifier;
+        wlc->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
+                             WIPHY_FLAG_STRICT_REGULATORY;
+        wiphy_apply_custom_regulatory(wlc->wiphy, regd->regdomain);
+        brcms_reg_apply_beaconing_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER);
 }
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.h b/drivers/net/wireless/brcm80211/brcmsmac/channel.h
index 808cb4fbfbe..006483a0abe 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/channel.h
@@ -37,9 +37,6 @@ brcms_c_channel_mgr_attach(struct brcms_c_info *wlc);
 
 extern void brcms_c_channel_mgr_detach(struct brcms_cm_info *wlc_cm);
 
-extern u8 brcms_c_channel_locale_flags_in_band(struct brcms_cm_info *wlc_cm,
-                                           uint bandunit);
-
 extern bool brcms_c_valid_chanspec_db(struct brcms_cm_info *wlc_cm,
                                       u16 chspec);
 
@@ -49,5 +46,6 @@ extern void brcms_c_channel_reg_limits(struct brcms_cm_info *wlc_cm,
 extern void brcms_c_channel_set_chanspec(struct brcms_cm_info *wlc_cm,
                                      u16 chanspec,
                                      u8 local_constraint_qdbm);
+extern void brcms_c_regd_init(struct brcms_c_info *wlc);
 
 #endif                          /* _WLC_CHANNEL_H */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 50f92a0b7c4..2d5a4041269 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;
@@ -1058,6 +1050,8 @@ static struct brcms_info *brcms_attach(struct bcma_device *pdev)
                 goto fail;
         }
 
+        brcms_c_regd_init(wl->wlc);
+
         memcpy(perm, &wl->pub->cur_etheraddr, ETH_ALEN);
         if (WARN_ON(!is_valid_ether_addr(perm)))
                 goto fail;
@@ -1068,9 +1062,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/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 19db4052c44..8776fbc8dcf 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -18,6 +18,7 @@
 
 #include <linux/pci_ids.h>
 #include <linux/if_ether.h>
+#include <net/cfg80211.h>
 #include <net/mac80211.h>
 #include <brcm_hw_ids.h>
 #include <aiutils.h>
@@ -3139,20 +3140,6 @@ void brcms_c_reset(struct brcms_c_info *wlc)
         brcms_b_reset(wlc->hw);
 }
 
-/* Return the channel the driver should initialize during brcms_c_init.
- * the channel may have to be changed from the currently configured channel
- * if other configurations are in conflict (bandlocked, 11n mode disabled,
- * invalid channel for current country, etc.)
- */
-static u16 brcms_c_init_chanspec(struct brcms_c_info *wlc)
-{
-        u16 chanspec =
-            1 | WL_CHANSPEC_BW_20 | WL_CHANSPEC_CTL_SB_NONE |
-            WL_CHANSPEC_BAND_2G;
-
-        return chanspec;
-}
-
 void brcms_c_init_scb(struct scb *scb)
 {
         int i;
@@ -5129,6 +5116,8 @@ static void brcms_c_wme_retries_write(struct brcms_c_info *wlc)
 /* make interface operational */
 int brcms_c_up(struct brcms_c_info *wlc)
 {
+        struct ieee80211_channel *ch;
+
         BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
         /* HW is turned off so don't try to access it */
@@ -5195,8 +5184,9 @@ int brcms_c_up(struct brcms_c_info *wlc)
         wlc->pub->up = true;
 
         if (wlc->bandinit_pending) {
+                ch = wlc->pub->ieee_hw->conf.channel;
                 brcms_c_suspend_mac_and_wait(wlc);
-                brcms_c_set_chanspec(wlc, wlc->default_bss->chanspec);
+                brcms_c_set_chanspec(wlc, ch20mhz_chspec(ch->hw_value));
                 wlc->bandinit_pending = false;
                 brcms_c_enable_mac(wlc);
         }
@@ -5397,11 +5387,6 @@ int brcms_c_set_gmode(struct brcms_c_info *wlc, u8 gmode, bool config)
         else
                 return -EINVAL;
 
-        /* Legacy or bust when no OFDM is supported by regulatory */
-        if ((brcms_c_channel_locale_flags_in_band(wlc->cmi, band->bandunit) &
-             BRCMS_NO_OFDM) && (gmode != GMODE_LEGACY_B))
-                return -EINVAL;
-
         /* update configuration value */
         if (config)
                 brcms_c_protection_upd(wlc, BRCMS_PROT_G_USER, gmode);
@@ -8201,19 +8186,12 @@ bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
 void brcms_c_init(struct brcms_c_info *wlc, bool mute_tx)
 {
         struct bcma_device *core = wlc->hw->d11core;
+        struct ieee80211_channel *ch = wlc->pub->ieee_hw->conf.channel;
         u16 chanspec;
 
         BCMMSG(wlc->wiphy, "wl%d\n", wlc->pub->unit);
 
-        /*
-         * This will happen if a big-hammer was executed. In
-         * that case, we want to go back to the channel that
-         * we were on and not new channel
-         */
-        if (wlc->pub->associated)
-                chanspec = wlc->home_chanspec;
-        else
-                chanspec = brcms_c_init_chanspec(wlc);
+        chanspec = ch20mhz_chspec(ch->hw_value);
 
         brcms_b_init(wlc->hw, chanspec);
 
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
index 333193f20e1..bcc79b4e326 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
@@ -37,5 +37,6 @@
 #define BCM4329_CHIP_ID         0x4329
 #define BCM4330_CHIP_ID         0x4330
 #define BCM4331_CHIP_ID         0x4331
+#define BCM4334_CHIP_ID         0x4334
 
 #endif                          /* _BRCM_HW_IDS_H_ */
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 9cfae0c0870..95aa8e1683e 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -1903,14 +1903,6 @@ static void ipw2100_down(struct ipw2100_priv *priv)
         netif_stop_queue(priv->net_dev);
 }
 
-/* Called by register_netdev() */
-static int ipw2100_net_init(struct net_device *dev)
-{
-        struct ipw2100_priv *priv = libipw_priv(dev);
-
-        return ipw2100_up(priv, 1);
-}
-
 static int ipw2100_wdev_init(struct net_device *dev)
 {
         struct ipw2100_priv *priv = libipw_priv(dev);
@@ -6087,7 +6079,6 @@ static const struct net_device_ops ipw2100_netdev_ops = {
         .ndo_stop               = ipw2100_close,
         .ndo_start_xmit         = libipw_xmit,
         .ndo_change_mtu         = libipw_change_mtu,
-        .ndo_init               = ipw2100_net_init,
         .ndo_tx_timeout         = ipw2100_tx_timeout,
         .ndo_set_mac_address    = ipw2100_set_address,
         .ndo_validate_addr      = eth_validate_addr,
@@ -6329,6 +6320,10 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
         printk(KERN_INFO DRV_NAME
                ": Detected Intel PRO/Wireless 2100 Network Connection\n");
 
+        err = ipw2100_up(priv, 1);
+        if (err)
+                goto fail;
+
         err = ipw2100_wdev_init(dev);
         if (err)
                 goto fail;
@@ -6338,12 +6333,7 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
          * network device we would call ipw2100_up.  This introduced a race
          * condition with newer hotplug configurations (network was coming
          * up and making calls before the device was initialized).
-         *
-         * If we called ipw2100_up before we registered the device, then the
-         * device name wasn't registered.  So, we instead use the net_dev->init
-         * member to call a function that then just turns and calls ipw2100_up.
-         * net_dev->init is called after name allocation but before the
-         * notifier chain is called */
+         */
         err = register_netdev(dev);
         if (err) {
                 printk(KERN_WARNING DRV_NAME
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 931002738c9..170ec330d2a 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,31 +1,19 @@
-# DVM
-obj-$(CONFIG_IWLDVM)   += iwldvm.o
-iwldvm-objs             := iwl-agn.o iwl-agn-rs.o iwl-mac80211.o
-iwldvm-objs             += iwl-ucode.o iwl-agn-tx.o
-iwldvm-objs             += iwl-agn-lib.o iwl-agn-calib.o
-iwldvm-objs             += iwl-agn-tt.o iwl-agn-sta.o iwl-agn-rx.o
-iwldvm-objs             += iwl-eeprom.o iwl-power.o
-iwldvm-objs             += iwl-scan.o iwl-led.o
-iwldvm-objs             += iwl-agn-rxon.o iwl-agn-devices.o
-
-iwldvm-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
-iwldvm-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += iwl-testmode.o
-
-CFLAGS_iwl-devtrace.o := -I$(src)
-
-# WIFI
+# common
 obj-$(CONFIG_IWLWIFI)   += iwlwifi.o
-iwlwifi-objs            += iwl-5000.o
-iwlwifi-objs            += iwl-6000.o
-iwlwifi-objs            += iwl-1000.o
-iwlwifi-objs            += iwl-2000.o
 iwlwifi-objs            += iwl-io.o
-iwlwifi-objs            += iwl-pci.o
 iwlwifi-objs            += iwl-drv.o
 iwlwifi-objs            += iwl-debug.o
 iwlwifi-objs            += iwl-notif-wait.o
-iwlwifi-objs            += iwl-trans-pcie.o iwl-trans-pcie-rx.o iwl-trans-pcie-tx.o
+iwlwifi-objs            += iwl-eeprom-read.o iwl-eeprom-parse.o
+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__
+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/Makefile b/drivers/net/wireless/iwlwifi/dvm/Makefile
new file mode 100644
index 00000000000..5ff76b20414
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/dvm/Makefile
@@ -0,0 +1,13 @@
+# DVM
+obj-$(CONFIG_IWLDVM)    += iwldvm.o
+iwldvm-objs             += main.o rs.o mac80211.o ucode.o tx.o
+iwldvm-objs             += lib.o calib.o tt.o sta.o rx.o
+
+iwldvm-objs             += power.o
+iwldvm-objs             += scan.o led.o
+iwldvm-objs             += rxon.o devices.o
+
+iwldvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o
+iwldvm-$(CONFIG_IWLWIFI_DEVICE_TESTMODE) += testmode.o
+
+ccflags-y += -D__CHECK_ENDIAN__ -I$(src)/../
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.h b/drivers/net/wireless/iwlwifi/dvm/agn.h
index 79c0fe06f4d..9bb16bdf6d2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.h
+++ b/drivers/net/wireless/iwlwifi/dvm/agn.h
@@ -63,9 +63,10 @@
 #ifndef __iwl_agn_h__
 #define __iwl_agn_h__
 
-#include "iwl-dev.h"
 #include "iwl-config.h"
 
+#include "dev.h"
+
 /* The first 11 queues (0-10) are used otherwise */
 #define IWLAGN_FIRST_AMPDU_QUEUE        11
 
@@ -91,7 +92,6 @@ extern struct iwl_lib_ops iwl6030_lib;
 #define STATUS_CT_KILL          1
 #define STATUS_ALIVE            2
 #define STATUS_READY            3
-#define STATUS_GEO_CONFIGURED   4
 #define STATUS_EXIT_PENDING     5
 #define STATUS_STATISTICS       6
 #define STATUS_SCANNING         7
@@ -101,6 +101,7 @@ extern struct iwl_lib_ops iwl6030_lib;
 #define STATUS_CHANNEL_SWITCH_PENDING 11
 #define STATUS_SCAN_COMPLETE    12
 #define STATUS_POWER_PMI        13
+#define STATUS_SCAN_ROC_EXPIRED 14
 
 struct iwl_ucode_capabilities;
 
@@ -255,6 +256,10 @@ int __must_check iwl_scan_initiate(struct iwl_priv *priv,
                                    enum iwl_scan_type scan_type,
                                    enum ieee80211_band band);
 
+void iwl_scan_roc_expired(struct iwl_priv *priv);
+void iwl_scan_offchannel_skb(struct iwl_priv *priv);
+void iwl_scan_offchannel_skb_status(struct iwl_priv *priv);
+
 /* For faster active scanning, scan will move to the next channel if fewer than
  * PLCP_QUIET_THRESH packets are heard on this channel within
  * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
@@ -264,7 +269,7 @@ int __must_check iwl_scan_initiate(struct iwl_priv *priv,
 #define IWL_ACTIVE_QUIET_TIME       cpu_to_le16(10)  /* msec */
 #define IWL_PLCP_QUIET_THRESH       cpu_to_le16(1)  /* packets */
 
-#define IWL_SCAN_CHECK_WATCHDOG         (HZ * 7)
+#define IWL_SCAN_CHECK_WATCHDOG         (HZ * 15)
 
 
 /* bt coex */
@@ -390,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,
@@ -399,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,
@@ -413,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
@@ -437,10 +447,8 @@ static inline void iwl_print_rx_config_cmd(struct iwl_priv *priv,
 
 static inline int iwl_is_ready(struct iwl_priv *priv)
 {
-        /* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
-         * set but EXIT_PENDING is not */
+        /* The adapter is 'ready' if READY EXIT_PENDING is not set */
         return test_bit(STATUS_READY, &priv->status) &&
-               test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
                !test_bit(STATUS_EXIT_PENDING, &priv->status);
 }
 
@@ -518,85 +526,4 @@ static inline const char *iwl_dvm_get_cmd_string(u8 cmd)
                 return s;
         return "UNKNOWN";
 }
-
-/* API method exported for mvm hybrid state */
-void iwl_setup_deferred_work(struct iwl_priv *priv);
-int iwl_send_wimax_coex(struct iwl_priv *priv);
-int iwl_send_bt_env(struct iwl_priv *priv, u8 action, u8 type);
-void iwl_option_config(struct iwl_priv *priv);
-void iwl_set_hw_params(struct iwl_priv *priv);
-void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags);
-int iwl_init_drv(struct iwl_priv *priv);
-void iwl_uninit_drv(struct iwl_priv *priv);
-void iwl_send_bt_config(struct iwl_priv *priv);
-void iwl_rf_kill_ct_config(struct iwl_priv *priv);
-int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwl_teardown_interface(struct iwl_priv *priv,
-                            struct ieee80211_vif *vif,
-                            bool mode_change);
-int iwl_full_rxon_required(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx);
-void iwlagn_check_needed_chains(struct iwl_priv *priv,
-                                struct iwl_rxon_context *ctx,
-                                struct ieee80211_bss_conf *bss_conf);
-void iwlagn_chain_noise_reset(struct iwl_priv *priv);
-int iwlagn_update_beacon(struct iwl_priv *priv,
-                         struct ieee80211_vif *vif);
-void iwl_tt_handler(struct iwl_priv *priv);
-void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode);
-void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue);
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state);
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb);
-void iwl_nic_error(struct iwl_op_mode *op_mode);
-void iwl_cmd_queue_full(struct iwl_op_mode *op_mode);
-void iwl_nic_config(struct iwl_op_mode *op_mode);
-int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
-                       struct ieee80211_sta *sta, bool set);
-void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-                              enum ieee80211_rssi_event rssi_event);
-int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw);
-int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw);
-void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop);
-void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue);
-void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
-                               struct ieee80211_channel_switch *ch_switch);
-int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
-                         struct ieee80211_vif *vif,
-                         struct ieee80211_sta *sta,
-                         enum ieee80211_sta_state old_state,
-                         enum ieee80211_sta_state new_state);
-int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
-                            struct ieee80211_vif *vif,
-                            enum ieee80211_ampdu_mlme_action action,
-                            struct ieee80211_sta *sta, u16 tid, u16 *ssn,
-                            u8 buf_size);
-int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
-                       struct ieee80211_vif *vif,
-                       struct cfg80211_scan_request *req);
-void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
-                           struct ieee80211_vif *vif,
-                           enum sta_notify_cmd cmd,
-                           struct ieee80211_sta *sta);
-void iwlagn_configure_filter(struct ieee80211_hw *hw,
-                             unsigned int changed_flags,
-                             unsigned int *total_flags,
-                             u64 multicast);
-int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
-                       struct ieee80211_vif *vif, u16 queue,
-                       const struct ieee80211_tx_queue_params *params);
-void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
-                               struct ieee80211_vif *vif,
-                               struct cfg80211_gtk_rekey_data *data);
-void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
-                                struct ieee80211_vif *vif,
-                                struct ieee80211_key_conf *keyconf,
-                                struct ieee80211_sta *sta,
-                                u32 iv32, u16 *phase1key);
-int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
-                       struct ieee80211_vif *vif,
-                       struct ieee80211_sta *sta,
-                       struct ieee80211_key_conf *key);
-void iwlagn_mac_stop(struct ieee80211_hw *hw);
-void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
-int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan);
 #endif /* __iwl_agn_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c b/drivers/net/wireless/iwlwifi/dvm/calib.c
index 95f27f1a423..f2dd671d7dc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.c
@@ -63,10 +63,11 @@
 #include <linux/slab.h>
 #include <net/mac80211.h>
 
-#include "iwl-dev.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
-#include "iwl-agn.h"
+
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
 
 /*****************************************************************************
  * INIT calibrations framework
@@ -832,14 +833,14 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
          * To be safe, simply mask out any chains that we know
          * are not on the device.
          */
-        active_chains &= priv->hw_params.valid_rx_ant;
+        active_chains &= priv->eeprom_data->valid_rx_ant;
 
         num_tx_chains = 0;
         for (i = 0; i < NUM_RX_CHAINS; i++) {
                 /* loops on all the bits of
                  * priv->hw_setting.valid_tx_ant */
                 u8 ant_msk = (1 << i);
-                if (!(priv->hw_params.valid_tx_ant & ant_msk))
+                if (!(priv->eeprom_data->valid_tx_ant & ant_msk))
                         continue;
 
                 num_tx_chains++;
@@ -853,7 +854,7 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
                          * connect the first valid tx chain
                          */
                         first_chain =
-                                find_first_chain(priv->hw_params.valid_tx_ant);
+                                find_first_chain(priv->eeprom_data->valid_tx_ant);
                         data->disconn_array[first_chain] = 0;
                         active_chains |= BIT(first_chain);
                         IWL_DEBUG_CALIB(priv,
@@ -863,13 +864,13 @@ static void iwl_find_disconn_antenna(struct iwl_priv *priv, u32* average_sig,
                 }
         }
 
-        if (active_chains != priv->hw_params.valid_rx_ant &&
+        if (active_chains != priv->eeprom_data->valid_rx_ant &&
             active_chains != priv->chain_noise_data.active_chains)
                 IWL_DEBUG_CALIB(priv,
                                 "Detected that not all antennas are connected! "
                                 "Connected: %#x, valid: %#x.\n",
                                 active_chains,
-                                priv->hw_params.valid_rx_ant);
+                                priv->eeprom_data->valid_rx_ant);
 
         /* Save for use within RXON, TX, SCAN commands, etc. */
         data->active_chains = active_chains;
@@ -1054,7 +1055,7 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
             priv->cfg->bt_params->advanced_bt_coexist) {
                 /* Disable disconnected antenna algorithm for advanced
                    bt coex, assuming valid antennas are connected */
-                data->active_chains = priv->hw_params.valid_rx_ant;
+                data->active_chains = priv->eeprom_data->valid_rx_ant;
                 for (i = 0; i < NUM_RX_CHAINS; i++)
                         if (!(data->active_chains & (1<<i)))
                                 data->disconn_array[i] = 1;
@@ -1083,8 +1084,9 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv)
         IWL_DEBUG_CALIB(priv, "min_average_noise = %d, antenna %d\n",
                         min_average_noise, min_average_noise_antenna_i);
 
-        iwlagn_gain_computation(priv, average_noise,
-                                find_first_chain(priv->hw_params.valid_rx_ant));
+        iwlagn_gain_computation(
+                priv, average_noise,
+                find_first_chain(priv->eeprom_data->valid_rx_ant));
 
         /* Some power changes may have been made during the calibration.
          * Update and commit the RXON
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h b/drivers/net/wireless/iwlwifi/dvm/calib.h
index dbe13787f27..2349f393cc4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-calib.h
+++ b/drivers/net/wireless/iwlwifi/dvm/calib.h
@@ -62,8 +62,8 @@
 #ifndef __iwl_calib_h__
 #define __iwl_calib_h__
 
-#include "iwl-dev.h"
-#include "iwl-commands.h"
+#include "dev.h"
+#include "commands.h"
 
 void iwl_chain_noise_calibration(struct iwl_priv *priv);
 void iwl_sensitivity_calibration(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/dvm/commands.h
index b9f7361d242..64811cd9163 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/dvm/commands.h
@@ -61,9 +61,9 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-commands.h) only for uCode API definitions.
+ * Please use this file (commands.h) only for uCode API definitions.
  * Please use iwl-xxxx-hw.h for hardware-related definitions.
- * Please use iwl-dev.h for driver implementation definitions.
+ * Please use dev.h for driver implementation definitions.
  */
 
 #ifndef __iwl_commands_h__
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
index e7c157e5ebe..8a2d9e643b1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/debugfs.c
@@ -30,16 +30,12 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/debugfs.h>
-
 #include <linux/ieee80211.h>
 #include <net/mac80211.h>
-
-
-#include "iwl-dev.h"
 #include "iwl-debug.h"
 #include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 /* create and remove of files */
 #define DEBUGFS_ADD_FILE(name, parent, mode) do {                       \
@@ -307,13 +303,13 @@ static ssize_t iwl_dbgfs_nvm_read(struct file *file,
         const u8 *ptr;
         char *buf;
         u16 eeprom_ver;
-        size_t eeprom_len = priv->cfg->base_params->eeprom_size;
+        size_t eeprom_len = priv->eeprom_blob_size;
         buf_size = 4 * eeprom_len + 256;
 
         if (eeprom_len % 16)
                 return -ENODATA;
 
-        ptr = priv->eeprom;
+        ptr = priv->eeprom_blob;
         if (!ptr)
                 return -ENOMEM;
 
@@ -322,11 +318,9 @@ static ssize_t iwl_dbgfs_nvm_read(struct file *file,
         if (!buf)
                 return -ENOMEM;
 
-        eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
-        pos += scnprintf(buf + pos, buf_size - pos, "NVM Type: %s, "
-                        "version: 0x%x\n",
-                        (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-                         ? "OTP" : "EEPROM", eeprom_ver);
+        eeprom_ver = priv->eeprom_data->eeprom_version;
+        pos += scnprintf(buf + pos, buf_size - pos,
+                         "NVM version: 0x%x\n", eeprom_ver);
         for (ofs = 0 ; ofs < eeprom_len ; ofs += 16) {
                 pos += scnprintf(buf + pos, buf_size - pos, "0x%.4x ", ofs);
                 hex_dump_to_buffer(ptr + ofs, 16 , 16, 2, buf + pos,
@@ -351,9 +345,6 @@ static ssize_t iwl_dbgfs_channels_read(struct file *file, char __user *user_buf,
         char *buf;
         ssize_t ret;
 
-        if (!test_bit(STATUS_GEO_CONFIGURED, &priv->status))
-                return -EAGAIN;
-
         buf = kzalloc(bufsz, GFP_KERNEL);
         if (!buf)
                 return -ENOMEM;
@@ -426,8 +417,6 @@ static ssize_t iwl_dbgfs_status_read(struct file *file,
                 test_bit(STATUS_ALIVE, &priv->status));
         pos += scnprintf(buf + pos, bufsz - pos, "STATUS_READY:\t\t %d\n",
                 test_bit(STATUS_READY, &priv->status));
-        pos += scnprintf(buf + pos, bufsz - pos, "STATUS_GEO_CONFIGURED:\t %d\n",
-                test_bit(STATUS_GEO_CONFIGURED, &priv->status));
         pos += scnprintf(buf + pos, bufsz - pos, "STATUS_EXIT_PENDING:\t %d\n",
                 test_bit(STATUS_EXIT_PENDING, &priv->status));
         pos += scnprintf(buf + pos, bufsz - pos, "STATUS_STATISTICS:\t %d\n",
@@ -1341,17 +1330,17 @@ static ssize_t iwl_dbgfs_ucode_tx_stats_read(struct file *file,
         if (tx->tx_power.ant_a || tx->tx_power.ant_b || tx->tx_power.ant_c) {
                 pos += scnprintf(buf + pos, bufsz - pos,
                         "tx power: (1/2 dB step)\n");
-                if ((priv->hw_params.valid_tx_ant & ANT_A) &&
+                if ((priv->eeprom_data->valid_tx_ant & ANT_A) &&
                     tx->tx_power.ant_a)
                         pos += scnprintf(buf + pos, bufsz - pos,
                                         fmt_hex, "antenna A:",
                                         tx->tx_power.ant_a);
-                if ((priv->hw_params.valid_tx_ant & ANT_B) &&
+                if ((priv->eeprom_data->valid_tx_ant & ANT_B) &&
                     tx->tx_power.ant_b)
                         pos += scnprintf(buf + pos, bufsz - pos,
                                         fmt_hex, "antenna B:",
                                         tx->tx_power.ant_b);
-                if ((priv->hw_params.valid_tx_ant & ANT_C) &&
+                if ((priv->eeprom_data->valid_tx_ant & ANT_C) &&
                     tx->tx_power.ant_c)
                         pos += scnprintf(buf + pos, bufsz - pos,
                                         fmt_hex, "antenna C:",
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/dvm/dev.h
index 70062379d0e..54cf085ddc8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/dvm/dev.h
@@ -24,8 +24,8 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-dev.h) for driver implementation definitions.
- * Please use iwl-commands.h for uCode API definitions.
+ * Please use this file (dev.h) for driver implementation definitions.
+ * Please use commands.h for uCode API definitions.
  */
 
 #ifndef __iwl_dev_h__
@@ -39,17 +39,20 @@
 #include <linux/mutex.h>
 
 #include "iwl-fw.h"
-#include "iwl-eeprom.h"
+#include "iwl-eeprom-parse.h"
 #include "iwl-csr.h"
 #include "iwl-debug.h"
 #include "iwl-agn-hw.h"
-#include "iwl-led.h"
-#include "iwl-power.h"
-#include "iwl-agn-rs.h"
-#include "iwl-agn-tt.h"
-#include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-notif-wait.h"
+#include "iwl-trans.h"
+
+#include "led.h"
+#include "power.h"
+#include "rs.h"
+#include "tt.h"
+
+#include "iwl-test.h"
 
 /* CT-KILL constants */
 #define CT_KILL_THRESHOLD_LEGACY   110 /* in Celsius */
@@ -88,33 +91,6 @@
 #define IWL_NUM_SCAN_RATES         (2)
 
 /*
- * One for each channel, holds all channel setup data
- * Some of the fields (e.g. eeprom and flags/max_power_avg) are redundant
- *     with one another!
- */
-struct iwl_channel_info {
-        struct iwl_eeprom_channel eeprom;       /* EEPROM regulatory limit */
-        struct iwl_eeprom_channel ht40_eeprom;  /* EEPROM regulatory limit for
-                                                 * HT40 channel */
-
-        u8 channel;       /* channel number */
-        u8 flags;         /* flags copied from EEPROM */
-        s8 max_power_avg; /* (dBm) regul. eeprom, normal Tx, any rate */
-        s8 curr_txpow;    /* (dBm) regulatory/spectrum/user (not h/w) limit */
-        s8 min_power;     /* always 0 */
-        s8 scan_power;    /* (dBm) regul. eeprom, direct scans, any rate */
-
-        u8 group_index;   /* 0-4, maps channel to group1/2/3/4/5 */
-        u8 band_index;    /* 0-4, maps channel to band1/2/3/4/5 */
-        enum ieee80211_band band;
-
-        /* HT40 channel info */
-        s8 ht40_max_power_avg;  /* (dBm) regul. eeprom, normal Tx, any rate */
-        u8 ht40_flags;          /* flags copied from EEPROM */
-        u8 ht40_extension_channel; /* HT_IE_EXT_CHANNEL_* */
-};
-
-/*
  * Minimum number of queues. MAX_NUM is defined in hw specific files.
  * Set the minimum to accommodate
  *  - 4 standard TX queues
@@ -153,29 +129,6 @@ union iwl_ht_rate_supp {
         };
 };
 
-#define CFG_HT_RX_AMPDU_FACTOR_8K   (0x0)
-#define CFG_HT_RX_AMPDU_FACTOR_16K  (0x1)
-#define CFG_HT_RX_AMPDU_FACTOR_32K  (0x2)
-#define CFG_HT_RX_AMPDU_FACTOR_64K  (0x3)
-#define CFG_HT_RX_AMPDU_FACTOR_DEF  CFG_HT_RX_AMPDU_FACTOR_64K
-#define CFG_HT_RX_AMPDU_FACTOR_MAX  CFG_HT_RX_AMPDU_FACTOR_64K
-#define CFG_HT_RX_AMPDU_FACTOR_MIN  CFG_HT_RX_AMPDU_FACTOR_8K
-
-/*
- * Maximal MPDU density for TX aggregation
- * 4 - 2us density
- * 5 - 4us density
- * 6 - 8us density
- * 7 - 16us density
- */
-#define CFG_HT_MPDU_DENSITY_2USEC   (0x4)
-#define CFG_HT_MPDU_DENSITY_4USEC   (0x5)
-#define CFG_HT_MPDU_DENSITY_8USEC   (0x6)
-#define CFG_HT_MPDU_DENSITY_16USEC  (0x7)
-#define CFG_HT_MPDU_DENSITY_DEF CFG_HT_MPDU_DENSITY_4USEC
-#define CFG_HT_MPDU_DENSITY_MAX CFG_HT_MPDU_DENSITY_16USEC
-#define CFG_HT_MPDU_DENSITY_MIN     (0x1)
-
 struct iwl_ht_config {
         bool single_chain_sufficient;
         enum ieee80211_smps_mode smps; /* current smps mode */
@@ -445,23 +398,6 @@ enum {
         MEASUREMENT_ACTIVE = (1 << 1),
 };
 
-enum iwl_nvm_type {
-        NVM_DEVICE_TYPE_EEPROM = 0,
-        NVM_DEVICE_TYPE_OTP,
-};
-
-/*
- * Two types of OTP memory access modes
- *   IWL_OTP_ACCESS_ABSOLUTE - absolute address mode,
- *                              based on physical memory addressing
- *   IWL_OTP_ACCESS_RELATIVE - relative address mode,
- *                             based on logical memory addressing
- */
-enum iwl_access_mode {
-        IWL_OTP_ACCESS_ABSOLUTE,
-        IWL_OTP_ACCESS_RELATIVE,
-};
-
 /* reply_tx_statistics (for _agn devices) */
 struct reply_tx_error_statistics {
         u32 pp_delay;
@@ -632,10 +568,6 @@ enum iwl_scan_type {
  *
  * @tx_chains_num: Number of TX chains
  * @rx_chains_num: Number of RX chains
- * @valid_tx_ant: usable antennas for TX
- * @valid_rx_ant: usable antennas for RX
- * @ht40_channel: is 40MHz width possible: BIT(IEEE80211_BAND_XXX)
- * @sku: sku read from EEPROM
  * @ct_kill_threshold: temperature threshold - in hw dependent unit
  * @ct_kill_exit_threshold: when to reeable the device - in hw dependent unit
  *      relevant for 1000, 6000 and up
@@ -645,11 +577,7 @@ enum iwl_scan_type {
 struct iwl_hw_params {
         u8  tx_chains_num;
         u8  rx_chains_num;
-        u8  valid_tx_ant;
-        u8  valid_rx_ant;
-        u8  ht40_channel;
         bool use_rts_for_aggregation;
-        u16 sku;
         u32 ct_kill_threshold;
         u32 ct_kill_exit_threshold;
 
@@ -664,31 +592,10 @@ struct iwl_lib_ops {
         /* device specific configuration */
         void (*nic_config)(struct iwl_priv *priv);
 
-        /* eeprom operations (as defined in iwl-eeprom.h) */
-        struct iwl_eeprom_ops eeprom_ops;
-
         /* temperature */
         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;
@@ -735,8 +642,6 @@ struct iwl_priv {
 
         /* ieee device used by generic ieee processing code */
         struct ieee80211_hw *hw;
-        struct ieee80211_channel *ieee_channels;
-        struct ieee80211_rate *ieee_rates;
 
         struct list_head calib_results;
 
@@ -747,16 +652,12 @@ 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);
 
         struct iwl_notif_wait_data notif_wait;
 
-        struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-
         /* spectrum measurement report caching */
         struct iwl_spectrum_notification measure_report;
         u8 measurement_status;
@@ -787,11 +688,6 @@ struct iwl_priv {
         bool ucode_loaded;
         bool init_ucode_run;            /* Don't run init uCode again */
 
-        /* we allocate array of iwl_channel_info for NIC's valid channels.
-         *    Access via channel # using indirect index array */
-        struct iwl_channel_info *channel_info;  /* channel info array */
-        u8 channel_count;       /* # of channels */
-
         u8 plcp_delta_threshold;
 
         /* thermal calibration */
@@ -846,6 +742,7 @@ struct iwl_priv {
         struct iwl_station_entry stations[IWLAGN_STATION_COUNT];
         unsigned long ucode_key_table;
         struct iwl_tid_data tid_data[IWLAGN_STATION_COUNT][IWL_MAX_TID_COUNT];
+        atomic_t num_aux_in_flight;
 
         u8 mac80211_registered;
 
@@ -950,10 +847,8 @@ struct iwl_priv {
 
         struct delayed_work scan_check;
 
-        /* TX Power */
+        /* TX Power settings */
         s8 tx_power_user_lmt;
-        s8 tx_power_device_lmt;
-        s8 tx_power_lmt_in_half_dbm; /* max tx power in half-dBm format */
         s8 tx_power_next;
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -964,9 +859,10 @@ struct iwl_priv {
         void *wowlan_sram;
 #endif /* CONFIG_IWLWIFI_DEBUGFS */
 
-        /* eeprom -- this is in the card's little endian byte order */
-        u8 *eeprom;
-        enum iwl_nvm_type nvm_device_type;
+        struct iwl_eeprom_data *eeprom_data;
+        /* eeprom blob for debugfs/testmode */
+        u8 *eeprom_blob;
+        size_t eeprom_blob_size;
 
         struct work_struct txpower_work;
         u32 calib_disabled;
@@ -979,9 +875,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
 
@@ -1001,8 +897,6 @@ struct iwl_priv {
         enum iwl_ucode_type cur_ucode;
 }; /*iwl_priv */
 
-extern struct kmem_cache *iwl_tx_cmd_pool;
-
 static inline struct iwl_rxon_context *
 iwl_rxon_ctx_from_vif(struct ieee80211_vif *vif)
 {
@@ -1036,36 +930,4 @@ static inline int iwl_is_any_associated(struct iwl_priv *priv)
         return false;
 }
 
-static inline int is_channel_valid(const struct iwl_channel_info *ch_info)
-{
-        if (ch_info == NULL)
-                return 0;
-        return (ch_info->flags & EEPROM_CHANNEL_VALID) ? 1 : 0;
-}
-
-static inline int is_channel_radar(const struct iwl_channel_info *ch_info)
-{
-        return (ch_info->flags & EEPROM_CHANNEL_RADAR) ? 1 : 0;
-}
-
-static inline u8 is_channel_a_band(const struct iwl_channel_info *ch_info)
-{
-        return ch_info->band == IEEE80211_BAND_5GHZ;
-}
-
-static inline u8 is_channel_bg_band(const struct iwl_channel_info *ch_info)
-{
-        return ch_info->band == IEEE80211_BAND_2GHZ;
-}
-
-static inline int is_channel_passive(const struct iwl_channel_info *ch)
-{
-        return (!(ch->flags & EEPROM_CHANNEL_ACTIVE)) ? 1 : 0;
-}
-
-static inline int is_channel_ibss(const struct iwl_channel_info *ch)
-{
-        return ((ch->flags & EEPROM_CHANNEL_IBSS)) ? 1 : 0;
-}
-
 #endif                          /* __iwl_dev_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 48533b3a0f9..349c205d5f6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -27,11 +27,14 @@
 /*
  * DVM device-specific data & functions
  */
-#include "iwl-agn.h"
-#include "iwl-dev.h"
-#include "iwl-commands.h"
 #include "iwl-io.h"
 #include "iwl-prph.h"
+#include "iwl-eeprom-parse.h"
+
+#include "agn.h"
+#include "dev.h"
+#include "commands.h"
+
 
 /*
  * 1000 series
@@ -58,11 +61,6 @@ static void iwl1000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 1000 series */
 static void iwl1000_nic_config(struct iwl_priv *priv)
 {
-        /* set CSR_HW_CONFIG_REG for uCode use */
-        iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
-                    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-                    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
-
         /* Setting digital SVR for 1000 card to 1.32V */
         /* locking is acquired in iwl_set_bits_mask_prph() function */
         iwl_set_bits_mask_prph(priv->trans, APMG_DIGITAL_SVR_REG,
@@ -170,16 +168,6 @@ static const struct iwl_sensitivity_ranges iwl1000_sensitivity = {
 
 static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
 {
-        priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-        priv->hw_params.tx_chains_num =
-                num_of_ant(priv->hw_params.valid_tx_ant);
-        if (priv->cfg->rx_with_siso_diversity)
-                priv->hw_params.rx_chains_num = 1;
-        else
-                priv->hw_params.rx_chains_num =
-                        num_of_ant(priv->hw_params.valid_rx_ant);
-
         iwl1000_set_ct_threshold(priv);
 
         /* Set initial sensitivity parameters */
@@ -189,17 +177,6 @@ static void iwl1000_hw_set_hw_params(struct iwl_priv *priv)
 struct iwl_lib_ops iwl1000_lib = {
         .set_hw_params = iwl1000_hw_set_hw_params,
         .nic_config = iwl1000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REGULATORY_BAND_NO_HT40,
-                },
-        },
         .temperature = iwlagn_temperature,
 };
 
@@ -219,8 +196,6 @@ static void iwl2000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 2000 series */
 static void iwl2000_nic_config(struct iwl_priv *priv)
 {
-        iwl_rf_config(priv);
-
         iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
                     CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
 }
@@ -251,16 +226,6 @@ static const struct iwl_sensitivity_ranges iwl2000_sensitivity = {
 
 static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
 {
-        priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ);
-
-        priv->hw_params.tx_chains_num =
-                num_of_ant(priv->hw_params.valid_tx_ant);
-        if (priv->cfg->rx_with_siso_diversity)
-                priv->hw_params.rx_chains_num = 1;
-        else
-                priv->hw_params.rx_chains_num =
-                        num_of_ant(priv->hw_params.valid_rx_ant);
-
         iwl2000_set_ct_threshold(priv);
 
         /* Set initial sensitivity parameters */
@@ -270,36 +235,12 @@ static void iwl2000_hw_set_hw_params(struct iwl_priv *priv)
 struct iwl_lib_ops iwl2000_lib = {
         .set_hw_params = iwl2000_hw_set_hw_params,
         .nic_config = iwl2000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REGULATORY_BAND_NO_HT40,
-                },
-                .enhanced_txpower = true,
-        },
         .temperature = iwlagn_temperature,
 };
 
 struct iwl_lib_ops iwl2030_lib = {
         .set_hw_params = iwl2000_hw_set_hw_params,
         .nic_config = iwl2000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REGULATORY_BAND_NO_HT40,
-                },
-                .enhanced_txpower = true,
-        },
         .temperature = iwlagn_temperature,
 };
 
@@ -309,19 +250,6 @@ struct iwl_lib_ops iwl2030_lib = {
  */
 
 /* NIC configuration for 5000 series */
-static void iwl5000_nic_config(struct iwl_priv *priv)
-{
-        iwl_rf_config(priv);
-
-        /* W/A : NIC is stuck in a reset state after Early PCIe power off
-         * (PCIe power is lost before PERST# is asserted),
-         * causing ME FW to lose ownership and not being able to obtain it back.
-         */
-        iwl_set_bits_mask_prph(priv->trans, APMG_PS_CTRL_REG,
-                                APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
-                                ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
-}
-
 static const struct iwl_sensitivity_ranges iwl5000_sensitivity = {
         .min_nrg_cck = 100,
         .auto_corr_min_ofdm = 90,
@@ -376,11 +304,9 @@ static struct iwl_sensitivity_ranges iwl5150_sensitivity = {
 static s32 iwl_temp_calib_to_offset(struct iwl_priv *priv)
 {
         u16 temperature, voltage;
-        __le16 *temp_calib = (__le16 *)iwl_eeprom_query_addr(priv,
-                                EEPROM_KELVIN_TEMPERATURE);
 
-        temperature = le16_to_cpu(temp_calib[0]);
-        voltage = le16_to_cpu(temp_calib[1]);
+        temperature = le16_to_cpu(priv->eeprom_data->kelvin_temperature);
+        voltage = le16_to_cpu(priv->eeprom_data->kelvin_voltage);
 
         /* offset = temp - volt / coeff */
         return (s32)(temperature -
@@ -404,14 +330,6 @@ static void iwl5000_set_ct_threshold(struct iwl_priv *priv)
 
 static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 {
-        priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-                                        BIT(IEEE80211_BAND_5GHZ);
-
-        priv->hw_params.tx_chains_num =
-                num_of_ant(priv->hw_params.valid_tx_ant);
-        priv->hw_params.rx_chains_num =
-                num_of_ant(priv->hw_params.valid_rx_ant);
-
         iwl5000_set_ct_threshold(priv);
 
         /* Set initial sensitivity parameters */
@@ -420,14 +338,6 @@ static void iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 
 static void iwl5150_hw_set_hw_params(struct iwl_priv *priv)
 {
-        priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-                                        BIT(IEEE80211_BAND_5GHZ);
-
-        priv->hw_params.tx_chains_num =
-                num_of_ant(priv->hw_params.valid_tx_ant);
-        priv->hw_params.rx_chains_num =
-                num_of_ant(priv->hw_params.valid_rx_ant);
-
         iwl5150_set_ct_threshold(priv);
 
         /* Set initial sensitivity parameters */
@@ -455,7 +365,6 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
          */
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
         struct iwl5000_channel_switch_cmd cmd;
-        const struct iwl_channel_info *ch_info;
         u32 switch_time_in_usec, ucode_switch_time;
         u16 ch;
         u32 tsf_low;
@@ -505,14 +414,7 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
         }
         IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
                       cmd.switch_time);
-        ch_info = iwl_get_channel_info(priv, priv->band, ch);
-        if (ch_info)
-                cmd.expect_beacon = is_channel_radar(ch_info);
-        else {
-                IWL_ERR(priv, "invalid channel switch from %u to %u\n",
-                        ctx->active.channel, ch);
-                return -EFAULT;
-        }
+        cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
         return iwl_dvm_send_cmd(priv, &hcmd);
 }
@@ -520,36 +422,12 @@ static int iwl5000_hw_channel_switch(struct iwl_priv *priv,
 struct iwl_lib_ops iwl5000_lib = {
         .set_hw_params = iwl5000_hw_set_hw_params,
         .set_channel_switch = iwl5000_hw_channel_switch,
-        .nic_config = iwl5000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REG_BAND_52_HT40_CHANNELS
-                },
-        },
         .temperature = iwlagn_temperature,
 };
 
 struct iwl_lib_ops iwl5150_lib = {
         .set_hw_params = iwl5150_hw_set_hw_params,
         .set_channel_switch = iwl5000_hw_channel_switch,
-        .nic_config = iwl5000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REG_BAND_52_HT40_CHANNELS
-                },
-        },
         .temperature = iwl5150_temperature,
 };
 
@@ -570,8 +448,6 @@ static void iwl6000_set_ct_threshold(struct iwl_priv *priv)
 /* NIC configuration for 6000 series */
 static void iwl6000_nic_config(struct iwl_priv *priv)
 {
-        iwl_rf_config(priv);
-
         switch (priv->cfg->device_family) {
         case IWL_DEVICE_FAMILY_6005:
         case IWL_DEVICE_FAMILY_6030:
@@ -584,13 +460,13 @@ static void iwl6000_nic_config(struct iwl_priv *priv)
                 break;
         case IWL_DEVICE_FAMILY_6050:
                 /* Indicate calibration version to uCode. */
-                if (iwl_eeprom_calib_version(priv) >= 6)
+                if (priv->eeprom_data->calib_version >= 6)
                         iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
                                         CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
                 break;
         case IWL_DEVICE_FAMILY_6150:
                 /* Indicate calibration version to uCode. */
-                if (iwl_eeprom_calib_version(priv) >= 6)
+                if (priv->eeprom_data->calib_version >= 6)
                         iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
                                         CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6);
                 iwl_set_bit(priv->trans, CSR_GP_DRIVER_REG,
@@ -627,17 +503,6 @@ static const struct iwl_sensitivity_ranges iwl6000_sensitivity = {
 
 static void iwl6000_hw_set_hw_params(struct iwl_priv *priv)
 {
-        priv->hw_params.ht40_channel =  BIT(IEEE80211_BAND_2GHZ) |
-                                        BIT(IEEE80211_BAND_5GHZ);
-
-        priv->hw_params.tx_chains_num =
-                num_of_ant(priv->hw_params.valid_tx_ant);
-        if (priv->cfg->rx_with_siso_diversity)
-                priv->hw_params.rx_chains_num = 1;
-        else
-                priv->hw_params.rx_chains_num =
-                        num_of_ant(priv->hw_params.valid_rx_ant);
-
         iwl6000_set_ct_threshold(priv);
 
         /* Set initial sensitivity parameters */
@@ -654,7 +519,6 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
          */
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
         struct iwl6000_channel_switch_cmd cmd;
-        const struct iwl_channel_info *ch_info;
         u32 switch_time_in_usec, ucode_switch_time;
         u16 ch;
         u32 tsf_low;
@@ -704,14 +568,7 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
         }
         IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
                       cmd.switch_time);
-        ch_info = iwl_get_channel_info(priv, priv->band, ch);
-        if (ch_info)
-                cmd.expect_beacon = is_channel_radar(ch_info);
-        else {
-                IWL_ERR(priv, "invalid channel switch from %u to %u\n",
-                        ctx->active.channel, ch);
-                return -EFAULT;
-        }
+        cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
         return iwl_dvm_send_cmd(priv, &hcmd);
 }
@@ -720,18 +577,6 @@ struct iwl_lib_ops iwl6000_lib = {
         .set_hw_params = iwl6000_hw_set_hw_params,
         .set_channel_switch = iwl6000_hw_channel_switch,
         .nic_config = iwl6000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REG_BAND_52_HT40_CHANNELS
-                },
-                .enhanced_txpower = true,
-        },
         .temperature = iwlagn_temperature,
 };
 
@@ -739,17 +584,5 @@ struct iwl_lib_ops iwl6030_lib = {
         .set_hw_params = iwl6000_hw_set_hw_params,
         .set_channel_switch = iwl6000_hw_channel_switch,
         .nic_config = iwl6000_nic_config,
-        .eeprom_ops = {
-                .regulatory_bands = {
-                        EEPROM_REG_BAND_1_CHANNELS,
-                        EEPROM_REG_BAND_2_CHANNELS,
-                        EEPROM_REG_BAND_3_CHANNELS,
-                        EEPROM_REG_BAND_4_CHANNELS,
-                        EEPROM_REG_BAND_5_CHANNELS,
-                        EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
-                        EEPROM_REG_BAND_52_HT40_CHANNELS
-                },
-                .enhanced_txpower = true,
-        },
         .temperature = iwlagn_temperature,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/dvm/led.c
index 47000419f91..bf479f70909 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/dvm/led.c
@@ -34,12 +34,11 @@
 #include <net/mac80211.h>
 #include <linux/etherdevice.h>
 #include <asm/unaligned.h>
-
-#include "iwl-dev.h"
-#include "iwl-agn.h"
 #include "iwl-io.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 /* Throughput           OFF time(ms)    ON time (ms)
  *      >300                    25              25
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.h b/drivers/net/wireless/iwlwifi/dvm/led.h
index b02a853103d..b02a853103d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.h
+++ b/drivers/net/wireless/iwlwifi/dvm/led.h
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/dvm/lib.c
index e55ec6c8a92..207ae91a83a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/dvm/lib.c
@@ -33,13 +33,14 @@
 #include <linux/sched.h>
 #include <net/mac80211.h>
 
-#include "iwl-dev.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
 
+#include "dev.h"
+#include "agn.h"
+
 int iwlagn_hw_valid_rtc_data_addr(u32 addr)
 {
         return (addr >= IWLAGN_RTC_DATA_LOWER_BOUND) &&
@@ -58,8 +59,7 @@ int iwlagn_send_tx_power(struct iwl_priv *priv)
         /* half dBm need to multiply */
         tx_power_cmd.global_lmt = (s8)(2 * priv->tx_power_user_lmt);
 
-        if (priv->tx_power_lmt_in_half_dbm &&
-            priv->tx_power_lmt_in_half_dbm < tx_power_cmd.global_lmt) {
+        if (tx_power_cmd.global_lmt > priv->eeprom_data->max_tx_pwr_half_dbm) {
                 /*
                  * For the newer devices which using enhanced/extend tx power
                  * table in EEPROM, the format is in half dBm. driver need to
@@ -71,7 +71,8 @@ int iwlagn_send_tx_power(struct iwl_priv *priv)
                  * "tx_power_user_lmt" is higher than EEPROM value (in
                  * half-dBm format), lower the tx power based on EEPROM
                  */
-                tx_power_cmd.global_lmt = priv->tx_power_lmt_in_half_dbm;
+                tx_power_cmd.global_lmt =
+                        priv->eeprom_data->max_tx_pwr_half_dbm;
         }
         tx_power_cmd.flags = IWLAGN_TX_POWER_NO_CLOSED;
         tx_power_cmd.srv_chan_lmt = IWLAGN_TX_POWER_AUTO;
@@ -159,7 +160,7 @@ int iwlagn_txfifo_flush(struct iwl_priv *priv, u16 flush_control)
                                 IWL_PAN_SCD_BK_MSK | IWL_PAN_SCD_MGMT_MSK |
                                 IWL_PAN_SCD_MULTICAST_MSK;
 
-        if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+        if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE)
                 flush_cmd.fifo_control |= IWL_AGG_TX_QUEUE_MSK;
 
         IWL_DEBUG_INFO(priv, "fifo queue control: 0X%x\n",
@@ -617,6 +618,11 @@ static bool iwlagn_fill_txpower_mode(struct iwl_priv *priv,
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
         int ave_rssi;
 
+        if (!ctx->vif || (ctx->vif->type != NL80211_IFTYPE_STATION)) {
+                IWL_DEBUG_INFO(priv, "BSS ctx not active or not in sta mode\n");
+                return false;
+        }
+
         ave_rssi = ieee80211_ave_rssi(ctx->vif);
         if (!ave_rssi) {
                 /* no rssi data, no changes to reduce tx power */
@@ -818,7 +824,7 @@ void iwlagn_set_rxon_chain(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
         if (priv->chain_noise_data.active_chains)
                 active_chains = priv->chain_noise_data.active_chains;
         else
-                active_chains = priv->hw_params.valid_rx_ant;
+                active_chains = priv->eeprom_data->valid_rx_ant;
 
         if (priv->cfg->bt_params &&
             priv->cfg->bt_params->advanced_bt_coexist &&
@@ -1259,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/iwl-mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index a55012609ca..b83ca358eb1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -38,19 +38,20 @@
 #include <linux/etherdevice.h>
 #include <linux/if_arp.h>
 
+#include <net/ieee80211_radiotap.h>
 #include <net/mac80211.h>
 
 #include <asm/div64.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-modparams.h"
 
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
+
 /*****************************************************************************
  *
  * mac80211 entry point functions
@@ -154,6 +155,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
                     IEEE80211_HW_SCAN_WHILE_IDLE;
 
         hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
+        hw->radiotap_mcs_details |= IEEE80211_RADIOTAP_MCS_HAVE_FMT;
 
         /*
          * Including the following line will crash some AP's.  This
@@ -162,7 +164,7 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
         hw->max_tx_aggregation_subframes = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
          */
 
-        if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
+        if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE)
                 hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
                              IEEE80211_HW_SUPPORTS_STATIC_SMPS;
 
@@ -237,12 +239,12 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 
         hw->max_listen_interval = IWL_CONN_MAX_LISTEN_INTERVAL;
 
-        if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
+        if (priv->eeprom_data->bands[IEEE80211_BAND_2GHZ].n_channels)
                 priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
-                        &priv->bands[IEEE80211_BAND_2GHZ];
-        if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
+                        &priv->eeprom_data->bands[IEEE80211_BAND_2GHZ];
+        if (priv->eeprom_data->bands[IEEE80211_BAND_5GHZ].n_channels)
                 priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-                        &priv->bands[IEEE80211_BAND_5GHZ];
+                        &priv->eeprom_data->bands[IEEE80211_BAND_5GHZ];
 
         hw->wiphy->hw_version = priv->trans->hw_id;
 
@@ -341,7 +343,7 @@ static int iwlagn_mac_start(struct ieee80211_hw *hw)
         return 0;
 }
 
-void iwlagn_mac_stop(struct ieee80211_hw *hw)
+static void iwlagn_mac_stop(struct ieee80211_hw *hw)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -369,9 +371,9 @@ void iwlagn_mac_stop(struct ieee80211_hw *hw)
         IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
-                               struct ieee80211_vif *vif,
-                               struct cfg80211_gtk_rekey_data *data)
+static void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
+                                      struct ieee80211_vif *vif,
+                                      struct cfg80211_gtk_rekey_data *data)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -397,7 +399,8 @@ void iwlagn_mac_set_rekey_data(struct ieee80211_hw *hw,
 
 #ifdef CONFIG_PM_SLEEP
 
-int iwlagn_mac_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
+static int iwlagn_mac_suspend(struct ieee80211_hw *hw,
+                              struct cfg80211_wowlan *wowlan)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
@@ -473,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);
@@ -508,7 +511,7 @@ static void iwlagn_mac_set_wakeup(struct ieee80211_hw *hw, bool enabled)
 }
 #endif
 
-void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+static void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -519,21 +522,21 @@ void iwlagn_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
                 dev_kfree_skb_any(skb);
 }
 
-void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
-                                struct ieee80211_vif *vif,
-                                struct ieee80211_key_conf *keyconf,
-                                struct ieee80211_sta *sta,
-                                u32 iv32, u16 *phase1key)
+static void iwlagn_mac_update_tkip_key(struct ieee80211_hw *hw,
+                                       struct ieee80211_vif *vif,
+                                       struct ieee80211_key_conf *keyconf,
+                                       struct ieee80211_sta *sta,
+                                       u32 iv32, u16 *phase1key)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
         iwl_update_tkip_key(priv, vif, keyconf, sta, iv32, phase1key);
 }
 
-int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
-                       struct ieee80211_vif *vif,
-                       struct ieee80211_sta *sta,
-                       struct ieee80211_key_conf *key)
+static int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+                              struct ieee80211_vif *vif,
+                              struct ieee80211_sta *sta,
+                              struct ieee80211_key_conf *key)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -633,11 +636,11 @@ int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
         return ret;
 }
 
-int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
-                            struct ieee80211_vif *vif,
-                            enum ieee80211_ampdu_mlme_action action,
-                            struct ieee80211_sta *sta, u16 tid, u16 *ssn,
-                            u8 buf_size)
+static int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
+                                   struct ieee80211_vif *vif,
+                                   enum ieee80211_ampdu_mlme_action action,
+                                   struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+                                   u8 buf_size)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         int ret = -EINVAL;
@@ -646,7 +649,7 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
         IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
                      sta->addr, tid);
 
-        if (!(priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE))
+        if (!(priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE))
                 return -EACCES;
 
         IWL_DEBUG_MAC80211(priv, "enter\n");
@@ -664,7 +667,7 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
                 ret = iwl_sta_rx_agg_stop(priv, sta, tid);
                 break;
         case IEEE80211_AMPDU_TX_START:
-                if (!priv->trans->ops->tx_agg_setup)
+                if (!priv->trans->ops->txq_enable)
                         break;
                 if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_TXAGG)
                         break;
@@ -759,11 +762,11 @@ static int iwlagn_mac_sta_remove(struct ieee80211_hw *hw,
         return ret;
 }
 
-int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
-                         struct ieee80211_vif *vif,
-                         struct ieee80211_sta *sta,
-                         enum ieee80211_sta_state old_state,
-                         enum ieee80211_sta_state new_state)
+static int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
+                                struct ieee80211_vif *vif,
+                                struct ieee80211_sta *sta,
+                                enum ieee80211_sta_state old_state,
+                                enum ieee80211_sta_state new_state)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -842,11 +845,10 @@ int iwlagn_mac_sta_state(struct ieee80211_hw *hw,
         return ret;
 }
 
-void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
-                               struct ieee80211_channel_switch *ch_switch)
+static void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
+                                      struct ieee80211_channel_switch *ch_switch)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        const struct iwl_channel_info *ch_info;
         struct ieee80211_conf *conf = &hw->conf;
         struct ieee80211_channel *channel = ch_switch->channel;
         struct iwl_ht_config *ht_conf = &priv->current_ht_config;
@@ -883,12 +885,6 @@ void iwlagn_mac_channel_switch(struct ieee80211_hw *hw,
         if (le16_to_cpu(ctx->active.channel) == ch)
                 goto out;
 
-        ch_info = iwl_get_channel_info(priv, channel->band, ch);
-        if (!is_channel_valid(ch_info)) {
-                IWL_DEBUG_MAC80211(priv, "invalid channel\n");
-                goto out;
-        }
-
         priv->current_ht_config.smps = conf->smps_mode;
 
         /* Configure HT40 channels */
@@ -937,10 +933,10 @@ void iwl_chswitch_done(struct iwl_priv *priv, bool is_success)
                 ieee80211_chswitch_done(ctx->vif, is_success);
 }
 
-void iwlagn_configure_filter(struct ieee80211_hw *hw,
-                             unsigned int changed_flags,
-                             unsigned int *total_flags,
-                             u64 multicast)
+static void iwlagn_configure_filter(struct ieee80211_hw *hw,
+                                    unsigned int changed_flags,
+                                    unsigned int *total_flags,
+                                    u64 multicast)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         __le32 filter_or = 0, filter_nand = 0;
@@ -987,7 +983,7 @@ void iwlagn_configure_filter(struct ieee80211_hw *hw,
                         FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
-void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
+static void iwlagn_mac_flush(struct ieee80211_hw *hw, bool drop)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1040,8 +1036,18 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
         mutex_lock(&priv->mutex);
 
         if (test_bit(STATUS_SCAN_HW, &priv->status)) {
-                err = -EBUSY;
-                goto out;
+                /* mac80211 should not scan while ROC or ROC while scanning */
+                if (WARN_ON_ONCE(priv->scan_type != IWL_SCAN_RADIO_RESET)) {
+                        err = -EBUSY;
+                        goto out;
+                }
+
+                iwl_scan_cancel_timeout(priv, 100);
+
+                if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+                        err = -EBUSY;
+                        goto out;
+                }
         }
 
         priv->hw_roc_channel = channel;
@@ -1114,7 +1120,7 @@ static int iwlagn_mac_remain_on_channel(struct ieee80211_hw *hw,
         return err;
 }
 
-int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
+static int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1131,8 +1137,8 @@ int iwlagn_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
         return 0;
 }
 
-void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
-                              enum ieee80211_rssi_event rssi_event)
+static void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
+                                     enum ieee80211_rssi_event rssi_event)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1156,8 +1162,8 @@ void iwlagn_mac_rssi_callback(struct ieee80211_hw *hw,
         IWL_DEBUG_MAC80211(priv, "leave\n");
 }
 
-int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
-                       struct ieee80211_sta *sta, bool set)
+static int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
+                              struct ieee80211_sta *sta, bool set)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1166,9 +1172,9 @@ int iwlagn_mac_set_tim(struct ieee80211_hw *hw,
         return 0;
 }
 
-int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
-                       struct ieee80211_vif *vif, u16 queue,
-                       const struct ieee80211_tx_queue_params *params)
+static int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
+                              struct ieee80211_vif *vif, u16 queue,
+                              const struct ieee80211_tx_queue_params *params)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         struct iwl_vif_priv *vif_priv = (void *)vif->drv_priv;
@@ -1210,7 +1216,7 @@ int iwlagn_mac_conf_tx(struct ieee80211_hw *hw,
         return 0;
 }
 
-int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
+static int iwlagn_mac_tx_last_beacon(struct ieee80211_hw *hw)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
 
@@ -1226,7 +1232,8 @@ static int iwl_set_mode(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
         return iwlagn_commit_rxon(priv, ctx);
 }
 
-int iwl_setup_interface(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+static int iwl_setup_interface(struct iwl_priv *priv,
+                               struct iwl_rxon_context *ctx)
 {
         struct ieee80211_vif *vif = ctx->vif;
         int err, ac;
@@ -1346,9 +1353,9 @@ static int iwlagn_mac_add_interface(struct ieee80211_hw *hw,
         return err;
 }
 
-void iwl_teardown_interface(struct iwl_priv *priv,
-                            struct ieee80211_vif *vif,
-                            bool mode_change)
+static void iwl_teardown_interface(struct iwl_priv *priv,
+                                   struct ieee80211_vif *vif,
+                                   bool mode_change)
 {
         struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
 
@@ -1404,13 +1411,11 @@ static void iwlagn_mac_remove_interface(struct ieee80211_hw *hw,
 }
 
 static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
-                                struct ieee80211_vif *vif,
-                                enum nl80211_iftype newtype, bool newp2p)
+                                       struct ieee80211_vif *vif,
+                                       enum nl80211_iftype newtype, bool newp2p)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        struct iwl_rxon_context *ctx = iwl_rxon_ctx_from_vif(vif);
-        struct iwl_rxon_context *bss_ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-        struct iwl_rxon_context *tmp;
+        struct iwl_rxon_context *ctx, *tmp;
         enum nl80211_iftype newviftype = newtype;
         u32 interface_modes;
         int err;
@@ -1421,6 +1426,18 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
 
         mutex_lock(&priv->mutex);
 
+        ctx = iwl_rxon_ctx_from_vif(vif);
+
+        /*
+         * To simplify this code, only support changes on the
+         * BSS context. The PAN context is usually reassigned
+         * by creating/removing P2P interfaces anyway.
+         */
+        if (ctx->ctxid != IWL_RXON_CTX_BSS) {
+                err = -EBUSY;
+                goto out;
+        }
+
         if (!ctx->vif || !iwl_is_ready_rf(priv)) {
                 /*
                  * Huh? But wait ... this can maybe happen when
@@ -1430,32 +1447,19 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
                 goto out;
         }
 
+        /* Check if the switch is supported in the same context */
         interface_modes = ctx->interface_modes | ctx->exclusive_interface_modes;
-
         if (!(interface_modes & BIT(newtype))) {
                 err = -EBUSY;
                 goto out;
         }
 
-        /*
-         * Refuse a change that should be done by moving from the PAN
-         * context to the BSS context instead, if the BSS context is
-         * available and can support the new interface type.
-         */
-        if (ctx->ctxid == IWL_RXON_CTX_PAN && !bss_ctx->vif &&
-            (bss_ctx->interface_modes & BIT(newtype) ||
-             bss_ctx->exclusive_interface_modes & BIT(newtype))) {
-                BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
-                err = -EBUSY;
-                goto out;
-        }
-
         if (ctx->exclusive_interface_modes & BIT(newtype)) {
                 for_each_context(priv, tmp) {
                         if (ctx == tmp)
                                 continue;
 
-                        if (!tmp->vif)
+                        if (!tmp->is_active)
                                 continue;
 
                         /*
@@ -1489,9 +1493,9 @@ static int iwlagn_mac_change_interface(struct ieee80211_hw *hw,
         return err;
 }
 
-int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
-                       struct ieee80211_vif *vif,
-                       struct cfg80211_scan_request *req)
+static int iwlagn_mac_hw_scan(struct ieee80211_hw *hw,
+                              struct ieee80211_vif *vif,
+                              struct cfg80211_scan_request *req)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         int ret;
@@ -1546,10 +1550,10 @@ static void iwl_sta_modify_ps_wake(struct iwl_priv *priv, int sta_id)
         iwl_send_add_sta(priv, &cmd, CMD_ASYNC);
 }
 
-void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
-                           struct ieee80211_vif *vif,
-                           enum sta_notify_cmd cmd,
-                           struct ieee80211_sta *sta)
+static void iwlagn_mac_sta_notify(struct ieee80211_hw *hw,
+                                  struct ieee80211_vif *vif,
+                                  enum sta_notify_cmd cmd,
+                                  struct ieee80211_sta *sta)
 {
         struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
         struct iwl_station_priv *sta_priv = (void *)sta->drv_priv;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index 5149e6f7294..abfd7916bde 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -44,15 +44,19 @@
 
 #include <asm/div64.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
+#include "iwl-eeprom-read.h"
+#include "iwl-eeprom-parse.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
 #include "iwl-drv.h"
 #include "iwl-modparams.h"
+#include "iwl-prph.h"
+
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
+
 
 /******************************************************************************
  *
@@ -79,6 +83,8 @@ MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
 
+static const struct iwl_op_mode_ops iwl_dvm_ops;
+
 void iwl_update_chain_flags(struct iwl_priv *priv)
 {
         struct iwl_rxon_context *ctx;
@@ -179,7 +185,7 @@ int iwlagn_send_beacon_cmd(struct iwl_priv *priv)
                 rate = info->control.rates[0].idx;
 
         priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-                                              priv->hw_params.valid_tx_ant);
+                                              priv->eeprom_data->valid_tx_ant);
         rate_flags = iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
         /* In mac80211, rates for 5 GHz start at 0 */
@@ -402,7 +408,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;
@@ -577,7 +583,7 @@ static const u8 iwlagn_pan_ac_to_queue[] = {
         7, 6, 5, 4,
 };
 
-void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
+static void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
 {
         int i;
 
@@ -644,7 +650,7 @@ void iwl_init_context(struct iwl_priv *priv, u32 ucode_flags)
         BUILD_BUG_ON(NUM_IWL_RXON_CTX != 2);
 }
 
-void iwl_rf_kill_ct_config(struct iwl_priv *priv)
+static void iwl_rf_kill_ct_config(struct iwl_priv *priv)
 {
         struct iwl_ct_kill_config cmd;
         struct iwl_ct_kill_throttling_config adv_cmd;
@@ -725,7 +731,7 @@ static int iwlagn_send_tx_ant_config(struct iwl_priv *priv, u8 valid_tx_ant)
         }
 }
 
-void iwl_send_bt_config(struct iwl_priv *priv)
+static void iwl_send_bt_config(struct iwl_priv *priv)
 {
         struct iwl_bt_cmd bt_cmd = {
                 .lead_time = BT_LEAD_TIME_DEF,
@@ -813,7 +819,7 @@ int iwl_alive_start(struct iwl_priv *priv)
         ieee80211_wake_queues(priv->hw);
 
         /* Configure Tx antenna selection based on H/W config */
-        iwlagn_send_tx_ant_config(priv, priv->hw_params.valid_tx_ant);
+        iwlagn_send_tx_ant_config(priv, priv->eeprom_data->valid_tx_ant);
 
         if (iwl_is_associated_ctx(ctx) && !priv->wowlan) {
                 struct iwl_rxon_cmd *active_rxon =
@@ -931,11 +937,12 @@ void iwl_down(struct iwl_priv *priv)
         priv->ucode_loaded = false;
         iwl_trans_stop_device(priv->trans);
 
+        /* Set num_aux_in_flight must be done after the transport is stopped */
+        atomic_set(&priv->num_aux_in_flight, 0);
+
         /* Clear out all status bits but a few that are stable across reset */
         priv->status &= test_bit(STATUS_RF_KILL_HW, &priv->status) <<
                                 STATUS_RF_KILL_HW |
-                        test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
-                                STATUS_GEO_CONFIGURED |
                         test_bit(STATUS_FW_ERROR, &priv->status) <<
                                 STATUS_FW_ERROR |
                         test_bit(STATUS_EXIT_PENDING, &priv->status) <<
@@ -1077,7 +1084,7 @@ static void iwlagn_disable_roc_work(struct work_struct *work)
  *
  *****************************************************************************/
 
-void iwl_setup_deferred_work(struct iwl_priv *priv)
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
 {
         priv->workqueue = create_singlethread_workqueue(DRV_NAME);
 
@@ -1122,224 +1129,14 @@ void iwl_cancel_deferred_work(struct iwl_priv *priv)
         del_timer_sync(&priv->ucode_trace);
 }
 
-static void iwl_init_hw_rates(struct ieee80211_rate *rates)
-{
-        int i;
-
-        for (i = 0; i < IWL_RATE_COUNT_LEGACY; i++) {
-                rates[i].bitrate = iwl_rates[i].ieee * 5;
-                rates[i].hw_value = i; /* Rate scaling will work on indexes */
-                rates[i].hw_value_short = i;
-                rates[i].flags = 0;
-                if ((i >= IWL_FIRST_CCK_RATE) && (i <= IWL_LAST_CCK_RATE)) {
-                        /*
-                         * If CCK != 1M then set short preamble rate flag.
-                         */
-                        rates[i].flags |=
-                                (iwl_rates[i].plcp == IWL_RATE_1M_PLCP) ?
-                                        0 : IEEE80211_RATE_SHORT_PREAMBLE;
-                }
-        }
-}
-
-#define MAX_BIT_RATE_40_MHZ 150 /* Mbps */
-#define MAX_BIT_RATE_20_MHZ 72 /* Mbps */
-static void iwl_init_ht_hw_capab(const struct iwl_priv *priv,
-                              struct ieee80211_sta_ht_cap *ht_info,
-                              enum ieee80211_band band)
+static int iwl_init_drv(struct iwl_priv *priv)
 {
-        u16 max_bit_rate = 0;
-        u8 rx_chains_num = priv->hw_params.rx_chains_num;
-        u8 tx_chains_num = priv->hw_params.tx_chains_num;
-
-        ht_info->cap = 0;
-        memset(&ht_info->mcs, 0, sizeof(ht_info->mcs));
-
-        ht_info->ht_supported = true;
-
-        if (priv->cfg->ht_params &&
-            priv->cfg->ht_params->ht_greenfield_support)
-                ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
-        ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
-        max_bit_rate = MAX_BIT_RATE_20_MHZ;
-        if (priv->hw_params.ht40_channel & BIT(band)) {
-                ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
-                ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
-                ht_info->mcs.rx_mask[4] = 0x01;
-                max_bit_rate = MAX_BIT_RATE_40_MHZ;
-        }
-
-        if (iwlwifi_mod_params.amsdu_size_8K)
-                ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
-
-        ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
-        ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
-
-        ht_info->mcs.rx_mask[0] = 0xFF;
-        if (rx_chains_num >= 2)
-                ht_info->mcs.rx_mask[1] = 0xFF;
-        if (rx_chains_num >= 3)
-                ht_info->mcs.rx_mask[2] = 0xFF;
-
-        /* Highest supported Rx data rate */
-        max_bit_rate *= rx_chains_num;
-        WARN_ON(max_bit_rate & ~IEEE80211_HT_MCS_RX_HIGHEST_MASK);
-        ht_info->mcs.rx_highest = cpu_to_le16(max_bit_rate);
-
-        /* Tx MCS capabilities */
-        ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
-        if (tx_chains_num != rx_chains_num) {
-                ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
-                ht_info->mcs.tx_params |= ((tx_chains_num - 1) <<
-                                IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
-        }
-}
-
-/**
- * iwl_init_geos - Initialize mac80211's geo/channel info based from eeprom
- */
-static int iwl_init_geos(struct iwl_priv *priv)
-{
-        struct iwl_channel_info *ch;
-        struct ieee80211_supported_band *sband;
-        struct ieee80211_channel *channels;
-        struct ieee80211_channel *geo_ch;
-        struct ieee80211_rate *rates;
-        int i = 0;
-        s8 max_tx_power = IWLAGN_TX_POWER_TARGET_POWER_MIN;
-
-        if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
-            priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
-                IWL_DEBUG_INFO(priv, "Geography modes already initialized.\n");
-                set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-                return 0;
-        }
-
-        channels = kcalloc(priv->channel_count,
-                           sizeof(struct ieee80211_channel), GFP_KERNEL);
-        if (!channels)
-                return -ENOMEM;
-
-        rates = kcalloc(IWL_RATE_COUNT_LEGACY, sizeof(struct ieee80211_rate),
-                        GFP_KERNEL);
-        if (!rates) {
-                kfree(channels);
-                return -ENOMEM;
-        }
-
-        /* 5.2GHz channels start after the 2.4GHz channels */
-        sband = &priv->bands[IEEE80211_BAND_5GHZ];
-        sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
-        /* just OFDM */
-        sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
-        sband->n_bitrates = IWL_RATE_COUNT_LEGACY - IWL_FIRST_OFDM_RATE;
-
-        if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
-                iwl_init_ht_hw_capab(priv, &sband->ht_cap,
-                                         IEEE80211_BAND_5GHZ);
-
-        sband = &priv->bands[IEEE80211_BAND_2GHZ];
-        sband->channels = channels;
-        /* OFDM & CCK */
-        sband->bitrates = rates;
-        sband->n_bitrates = IWL_RATE_COUNT_LEGACY;
-
-        if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE)
-                iwl_init_ht_hw_capab(priv, &sband->ht_cap,
-                                         IEEE80211_BAND_2GHZ);
-
-        priv->ieee_channels = channels;
-        priv->ieee_rates = rates;
-
-        for (i = 0;  i < priv->channel_count; i++) {
-                ch = &priv->channel_info[i];
-
-                /* FIXME: might be removed if scan is OK */
-                if (!is_channel_valid(ch))
-                        continue;
-
-                sband =  &priv->bands[ch->band];
-
-                geo_ch = &sband->channels[sband->n_channels++];
-
-                geo_ch->center_freq =
-                        ieee80211_channel_to_frequency(ch->channel, ch->band);
-                geo_ch->max_power = ch->max_power_avg;
-                geo_ch->max_antenna_gain = 0xff;
-                geo_ch->hw_value = ch->channel;
-
-                if (is_channel_valid(ch)) {
-                        if (!(ch->flags & EEPROM_CHANNEL_IBSS))
-                                geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
-
-                        if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
-                                geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
-
-                        if (ch->flags & EEPROM_CHANNEL_RADAR)
-                                geo_ch->flags |= IEEE80211_CHAN_RADAR;
-
-                        geo_ch->flags |= ch->ht40_extension_channel;
-
-                        if (ch->max_power_avg > max_tx_power)
-                                max_tx_power = ch->max_power_avg;
-                } else {
-                        geo_ch->flags |= IEEE80211_CHAN_DISABLED;
-                }
-
-                IWL_DEBUG_INFO(priv, "Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
-                                ch->channel, geo_ch->center_freq,
-                                is_channel_a_band(ch) ?  "5.2" : "2.4",
-                                geo_ch->flags & IEEE80211_CHAN_DISABLED ?
-                                "restricted" : "valid",
-                                 geo_ch->flags);
-        }
-
-        priv->tx_power_device_lmt = max_tx_power;
-        priv->tx_power_user_lmt = max_tx_power;
-        priv->tx_power_next = max_tx_power;
-
-        if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
-             priv->hw_params.sku & EEPROM_SKU_CAP_BAND_52GHZ) {
-                IWL_INFO(priv, "Incorrectly detected BG card as ABG. "
-                        "Please send your %s to maintainer.\n",
-                        priv->trans->hw_id_str);
-                priv->hw_params.sku &= ~EEPROM_SKU_CAP_BAND_52GHZ;
-        }
-
-        if (iwlwifi_mod_params.disable_5ghz)
-                priv->bands[IEEE80211_BAND_5GHZ].n_channels = 0;
-
-        IWL_INFO(priv, "Tunable channels: %d 802.11bg, %d 802.11a channels\n",
-                   priv->bands[IEEE80211_BAND_2GHZ].n_channels,
-                   priv->bands[IEEE80211_BAND_5GHZ].n_channels);
-
-        set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-
-        return 0;
-}
-
-/*
- * iwl_free_geos - undo allocations in iwl_init_geos
- */
-static void iwl_free_geos(struct iwl_priv *priv)
-{
-        kfree(priv->ieee_channels);
-        kfree(priv->ieee_rates);
-        clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
-}
-
-int iwl_init_drv(struct iwl_priv *priv)
-{
-        int ret;
-
         spin_lock_init(&priv->sta_lock);
 
         mutex_init(&priv->mutex);
 
         INIT_LIST_HEAD(&priv->calib_results);
 
-        priv->ieee_channels = NULL;
-        priv->ieee_rates = NULL;
         priv->band = IEEE80211_BAND_2GHZ;
 
         priv->plcp_delta_threshold =
@@ -1370,31 +1167,11 @@ int iwl_init_drv(struct iwl_priv *priv)
                 priv->dynamic_frag_thresh = BT_FRAG_THRESHOLD_DEF;
         }
 
-        ret = iwl_init_channel_map(priv);
-        if (ret) {
-                IWL_ERR(priv, "initializing regulatory failed: %d\n", ret);
-                goto err;
-        }
-
-        ret = iwl_init_geos(priv);
-        if (ret) {
-                IWL_ERR(priv, "initializing geos failed: %d\n", ret);
-                goto err_free_channel_map;
-        }
-        iwl_init_hw_rates(priv->ieee_rates);
-
         return 0;
-
-err_free_channel_map:
-        iwl_free_channel_map(priv);
-err:
-        return ret;
 }
 
-void iwl_uninit_drv(struct iwl_priv *priv)
+static void iwl_uninit_drv(struct iwl_priv *priv)
 {
-        iwl_free_geos(priv);
-        iwl_free_channel_map(priv);
         kfree(priv->scan_cmd);
         kfree(priv->beacon_cmd);
         kfree(rcu_dereference_raw(priv->noa_data));
@@ -1404,15 +1181,12 @@ void iwl_uninit_drv(struct iwl_priv *priv)
 #endif
 }
 
-void iwl_set_hw_params(struct iwl_priv *priv)
+static void iwl_set_hw_params(struct iwl_priv *priv)
 {
         if (priv->cfg->ht_params)
                 priv->hw_params.use_rts_for_aggregation =
                         priv->cfg->ht_params->use_rts_for_aggregation;
 
-        if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
-                priv->hw_params.sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
-
         /* Device-specific setup */
         priv->lib->set_hw_params(priv);
 }
@@ -1420,7 +1194,7 @@ void iwl_set_hw_params(struct iwl_priv *priv)
 
 
 /* show what optional capabilities we have */
-void iwl_option_config(struct iwl_priv *priv)
+static void iwl_option_config(struct iwl_priv *priv)
 {
 #ifdef CONFIG_IWLWIFI_DEBUG
         IWL_INFO(priv, "CONFIG_IWLWIFI_DEBUG enabled\n");
@@ -1453,6 +1227,42 @@ void iwl_option_config(struct iwl_priv *priv)
 #endif
 }
 
+static int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
+{
+        u16 radio_cfg;
+
+        priv->eeprom_data->sku = priv->eeprom_data->sku;
+
+        if (priv->eeprom_data->sku & EEPROM_SKU_CAP_11N_ENABLE &&
+            !priv->cfg->ht_params) {
+                IWL_ERR(priv, "Invalid 11n configuration\n");
+                return -EINVAL;
+        }
+
+        if (!priv->eeprom_data->sku) {
+                IWL_ERR(priv, "Invalid device sku\n");
+                return -EINVAL;
+        }
+
+        IWL_INFO(priv, "Device SKU: 0x%X\n", priv->eeprom_data->sku);
+
+        radio_cfg = priv->eeprom_data->radio_cfg;
+
+        priv->hw_params.tx_chains_num =
+                num_of_ant(priv->eeprom_data->valid_tx_ant);
+        if (priv->cfg->rx_with_siso_diversity)
+                priv->hw_params.rx_chains_num = 1;
+        else
+                priv->hw_params.rx_chains_num =
+                        num_of_ant(priv->eeprom_data->valid_rx_ant);
+
+        IWL_INFO(priv, "Valid Tx ant: 0x%X, Valid Rx ant: 0x%X\n",
+                 priv->eeprom_data->valid_tx_ant,
+                 priv->eeprom_data->valid_rx_ant);
+
+        return 0;
+}
+
 static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
                                                  const struct iwl_cfg *cfg,
                                                  const struct iwl_fw *fw)
@@ -1538,9 +1348,12 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
                 trans_cfg.queue_watchdog_timeout =
                         priv->cfg->base_params->wd_timeout;
         else
-                trans_cfg.queue_watchdog_timeout = IWL_WATCHHDOG_DISABLED;
+                trans_cfg.queue_watchdog_timeout = IWL_WATCHDOG_DISABLED;
         trans_cfg.command_names = iwl_dvm_cmd_strings;
 
+        WARN_ON(sizeof(priv->transport_queue_stop) * BITS_PER_BYTE <
+                priv->cfg->base_params->num_of_queues);
+
         ucode_flags = fw->ucode_capa.flags;
 
 #ifndef CONFIG_IWLWIFI_P2P
@@ -1598,25 +1411,33 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
                 goto out_free_hw;
 
         /* Read the EEPROM */
-        if (iwl_eeprom_init(priv, priv->trans->hw_rev)) {
+        if (iwl_read_eeprom(priv->trans, &priv->eeprom_blob,
+                            &priv->eeprom_blob_size)) {
                 IWL_ERR(priv, "Unable to init EEPROM\n");
                 goto out_free_hw;
         }
+
         /* Reset chip to save power until we load uCode during "up". */
         iwl_trans_stop_hw(priv->trans, false);
 
-        if (iwl_eeprom_check_version(priv))
+        priv->eeprom_data = iwl_parse_eeprom_data(priv->trans->dev, priv->cfg,
+                                                  priv->eeprom_blob,
+                                                  priv->eeprom_blob_size);
+        if (!priv->eeprom_data)
+                goto out_free_eeprom_blob;
+
+        if (iwl_eeprom_check_version(priv->eeprom_data, priv->trans))
                 goto out_free_eeprom;
 
         if (iwl_eeprom_init_hw_params(priv))
                 goto out_free_eeprom;
 
         /* extract MAC Address */
-        iwl_eeprom_get_mac(priv, priv->addresses[0].addr);
+        memcpy(priv->addresses[0].addr, priv->eeprom_data->hw_addr, ETH_ALEN);
         IWL_DEBUG_INFO(priv, "MAC address: %pM\n", priv->addresses[0].addr);
         priv->hw->wiphy->addresses = priv->addresses;
         priv->hw->wiphy->n_addresses = 1;
-        num_mac = iwl_eeprom_query16(priv, EEPROM_NUM_MAC_ADDRESS);
+        num_mac = priv->eeprom_data->n_hw_addrs;
         if (num_mac > 1) {
                 memcpy(priv->addresses[1].addr, priv->addresses[0].addr,
                        ETH_ALEN);
@@ -1629,7 +1450,7 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
          ************************/
         iwl_set_hw_params(priv);
 
-        if (!(priv->hw_params.sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
+        if (!(priv->eeprom_data->sku & EEPROM_SKU_CAP_IPAN_ENABLE)) {
                 IWL_DEBUG_INFO(priv, "Your EEPROM disabled PAN");
                 ucode_flags &= ~IWL_UCODE_TLV_FLAGS_PAN;
                 /*
@@ -1710,8 +1531,10 @@ out_destroy_workqueue:
         destroy_workqueue(priv->workqueue);
         priv->workqueue = NULL;
         iwl_uninit_drv(priv);
+out_free_eeprom_blob:
+        kfree(priv->eeprom_blob);
 out_free_eeprom:
-        iwl_eeprom_free(priv);
+        iwl_free_eeprom_data(priv->eeprom_data);
 out_free_hw:
         ieee80211_free_hw(priv->hw);
 out:
@@ -1719,7 +1542,7 @@ out:
         return op_mode;
 }
 
-void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
+static void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -1727,7 +1550,7 @@ 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);
@@ -1736,7 +1559,8 @@ void iwl_op_mode_dvm_stop(struct iwl_op_mode *op_mode)
         priv->ucode_loaded = false;
         iwl_trans_stop_device(priv->trans);
 
-        iwl_eeprom_free(priv);
+        kfree(priv->eeprom_blob);
+        iwl_free_eeprom_data(priv->eeprom_data);
 
         /*netif_stop_queue(dev); */
         flush_workqueue(priv->workqueue);
@@ -1849,7 +1673,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");
@@ -2184,7 +2008,7 @@ static void iwlagn_fw_error(struct iwl_priv *priv, bool ondemand)
         }
 }
 
-void iwl_nic_error(struct iwl_op_mode *op_mode)
+static void iwl_nic_error(struct iwl_op_mode *op_mode)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2197,7 +2021,7 @@ void iwl_nic_error(struct iwl_op_mode *op_mode)
         iwlagn_fw_error(priv, false);
 }
 
-void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
+static void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2207,11 +2031,60 @@ void iwl_cmd_queue_full(struct iwl_op_mode *op_mode)
         }
 }
 
-void iwl_nic_config(struct iwl_op_mode *op_mode)
+#define EEPROM_RF_CONFIG_TYPE_MAX      0x3
+
+static void iwl_nic_config(struct iwl_op_mode *op_mode)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        u16 radio_cfg = priv->eeprom_data->radio_cfg;
+
+        /* SKU Control */
+        iwl_set_bits_mask(priv->trans, CSR_HW_IF_CONFIG_REG,
+                          CSR_HW_IF_CONFIG_REG_MSK_MAC_DASH |
+                          CSR_HW_IF_CONFIG_REG_MSK_MAC_STEP,
+                          (CSR_HW_REV_STEP(priv->trans->hw_rev) <<
+                                CSR_HW_IF_CONFIG_REG_POS_MAC_STEP) |
+                          (CSR_HW_REV_DASH(priv->trans->hw_rev) <<
+                                CSR_HW_IF_CONFIG_REG_POS_MAC_DASH));
+
+        /* write radio config values to register */
+        if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <= EEPROM_RF_CONFIG_TYPE_MAX) {
+                u32 reg_val =
+                        EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <<
+                                CSR_HW_IF_CONFIG_REG_POS_PHY_TYPE |
+                        EEPROM_RF_CFG_STEP_MSK(radio_cfg) <<
+                                CSR_HW_IF_CONFIG_REG_POS_PHY_STEP |
+                        EEPROM_RF_CFG_DASH_MSK(radio_cfg) <<
+                                CSR_HW_IF_CONFIG_REG_POS_PHY_DASH;
+
+                iwl_set_bits_mask(priv->trans, CSR_HW_IF_CONFIG_REG,
+                                  CSR_HW_IF_CONFIG_REG_MSK_PHY_TYPE |
+                                  CSR_HW_IF_CONFIG_REG_MSK_PHY_STEP |
+                                  CSR_HW_IF_CONFIG_REG_MSK_PHY_DASH, reg_val);
+
+                IWL_INFO(priv, "Radio type=0x%x-0x%x-0x%x\n",
+                         EEPROM_RF_CFG_TYPE_MSK(radio_cfg),
+                         EEPROM_RF_CFG_STEP_MSK(radio_cfg),
+                         EEPROM_RF_CFG_DASH_MSK(radio_cfg));
+        } else {
+                WARN_ON(1);
+        }
 
-        priv->lib->nic_config(priv);
+        /* set CSR_HW_CONFIG_REG for uCode use */
+        iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
+                    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+                    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+        /* W/A : NIC is stuck in a reset state after Early PCIe power off
+         * (PCIe power is lost before PERST# is asserted),
+         * causing ME FW to lose ownership and not being able to obtain it back.
+         */
+        iwl_set_bits_mask_prph(priv->trans, APMG_PS_CTRL_REG,
+                               APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS,
+                               ~APMG_PS_CTRL_EARLY_PWR_OFF_RESET_DIS);
+
+        if (priv->lib->nic_config)
+                priv->lib->nic_config(priv);
 }
 
 static void iwl_wimax_active(struct iwl_op_mode *op_mode)
@@ -2222,7 +2095,7 @@ static void iwl_wimax_active(struct iwl_op_mode *op_mode)
         IWL_ERR(priv, "RF is used by WiMAX\n");
 }
 
-void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
+static void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
         int mq = priv->queue_to_mac80211[queue];
@@ -2241,7 +2114,7 @@ void iwl_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
         ieee80211_stop_queue(priv->hw, mq);
 }
 
-void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
+static void iwl_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
         int mq = priv->queue_to_mac80211[queue];
@@ -2281,16 +2154,17 @@ void iwlagn_lift_passive_no_rx(struct iwl_priv *priv)
         priv->passive_no_rx = false;
 }
 
-void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+static void iwl_free_skb(struct iwl_op_mode *op_mode, struct sk_buff *skb)
 {
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
         struct ieee80211_tx_info *info;
 
         info = IEEE80211_SKB_CB(skb);
-        kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
+        iwl_trans_free_tx_cmd(priv->trans, info->driver_data[1]);
         dev_kfree_skb_any(skb);
 }
 
-void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
+static void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
 {
         struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
 
@@ -2302,7 +2176,7 @@ void iwl_set_hw_rfkill_state(struct iwl_op_mode *op_mode, bool state)
         wiphy_rfkill_set_hw_state(priv->hw->wiphy, state);
 }
 
-const struct iwl_op_mode_ops iwl_dvm_ops = {
+static const struct iwl_op_mode_ops iwl_dvm_ops = {
         .start = iwl_op_mode_dvm_start,
         .stop = iwl_op_mode_dvm_stop,
         .rx = iwl_rx_dispatch,
@@ -2321,9 +2195,6 @@ const struct iwl_op_mode_ops iwl_dvm_ops = {
  * driver and module entry point
  *
  *****************************************************************************/
-
-struct kmem_cache *iwl_tx_cmd_pool;
-
 static int __init iwl_init(void)
 {
 
@@ -2331,29 +2202,18 @@ static int __init iwl_init(void)
         pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
         pr_info(DRV_COPYRIGHT "\n");
 
-        iwl_tx_cmd_pool = kmem_cache_create("iwl_dev_cmd",
-                                            sizeof(struct iwl_device_cmd),
-                                            sizeof(void *), 0, NULL);
-        if (!iwl_tx_cmd_pool)
-                return -ENOMEM;
-
         ret = iwlagn_rate_control_register();
         if (ret) {
                 pr_err("Unable to register rate control algorithm: %d\n", ret);
-                goto error_rc_register;
+                return ret;
         }
 
         ret = iwl_opmode_register("iwldvm", &iwl_dvm_ops);
         if (ret) {
                 pr_err("Unable to register op_mode: %d\n", ret);
-                goto error_opmode_register;
+                iwlagn_rate_control_unregister();
         }
-        return ret;
 
-error_opmode_register:
-        iwlagn_rate_control_unregister();
-error_rc_register:
-        kmem_cache_destroy(iwl_tx_cmd_pool);
         return ret;
 }
 module_init(iwl_init);
@@ -2362,6 +2222,5 @@ static void __exit iwl_exit(void)
 {
         iwl_opmode_deregister("iwldvm");
         iwlagn_rate_control_unregister();
-        kmem_cache_destroy(iwl_tx_cmd_pool);
 }
 module_exit(iwl_exit);
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/dvm/power.c
index 544ddf17f5b..518cf371580 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/dvm/power.c
@@ -31,18 +31,15 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/init.h>
-
 #include <net/mac80211.h>
-
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-agn.h"
 #include "iwl-io.h"
-#include "iwl-commands.h"
 #include "iwl-debug.h"
-#include "iwl-power.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
+#include "commands.h"
+#include "power.h"
 
 /*
  * Setting power level allows the card to go to sleep when not busy.
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.h b/drivers/net/wireless/iwlwifi/dvm/power.h
index 21afc92efac..a2cee7f0484 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.h
+++ b/drivers/net/wireless/iwlwifi/dvm/power.h
@@ -28,7 +28,7 @@
 #ifndef __iwl_power_setting_h__
 #define __iwl_power_setting_h__
 
-#include "iwl-commands.h"
+#include "commands.h"
 
 struct iwl_power_mgr {
         struct iwl_powertable_cmd sleep_cmd;
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/dvm/rs.c
index 8cebd7c363f..6fddd2785e6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.c
@@ -35,10 +35,8 @@
 
 #include <linux/workqueue.h>
 
-#include "iwl-dev.h"
-#include "iwl-agn.h"
-#include "iwl-op-mode.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
 
 #define RS_NAME "iwl-agn-rs"
 
@@ -819,7 +817,7 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
 
                 if (num_of_ant(tbl->ant_type) > 1)
                         tbl->ant_type =
-                            first_antenna(priv->hw_params.valid_tx_ant);
+                            first_antenna(priv->eeprom_data->valid_tx_ant);
 
                 tbl->is_ht40 = 0;
                 tbl->is_SGI = 0;
@@ -1447,7 +1445,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
-        u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+        u8 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         u8 tx_chains_num = priv->hw_params.tx_chains_num;
         int ret = 0;
         u8 update_search_tbl_counter = 0;
@@ -1465,7 +1463,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
         case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
                 /* avoid antenna B and MIMO */
                 valid_tx_ant =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
                 if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2 &&
                     tbl->action != IWL_LEGACY_SWITCH_SISO)
                         tbl->action = IWL_LEGACY_SWITCH_SISO;
@@ -1489,7 +1487,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
                 else if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
                         tbl->action = IWL_LEGACY_SWITCH_SISO;
                 valid_tx_ant =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
         }
 
         start_action = tbl->action;
@@ -1623,7 +1621,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
-        u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+        u8 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         u8 tx_chains_num = priv->hw_params.tx_chains_num;
         u8 update_search_tbl_counter = 0;
         int ret;
@@ -1641,7 +1639,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
         case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
                 /* avoid antenna B and MIMO */
                 valid_tx_ant =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
                 if (tbl->action != IWL_SISO_SWITCH_ANTENNA1)
                         tbl->action = IWL_SISO_SWITCH_ANTENNA1;
                 break;
@@ -1659,7 +1657,7 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
         /* configure as 1x1 if bt full concurrency */
         if (priv->bt_full_concurrent) {
                 valid_tx_ant =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
                 if (tbl->action >= IWL_LEGACY_SWITCH_ANTENNA2)
                         tbl->action = IWL_SISO_SWITCH_ANTENNA1;
         }
@@ -1795,7 +1793,7 @@ static int rs_move_mimo2_to_other(struct iwl_priv *priv,
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
-        u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+        u8 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         u8 tx_chains_num = priv->hw_params.tx_chains_num;
         u8 update_search_tbl_counter = 0;
         int ret;
@@ -1965,7 +1963,7 @@ static int rs_move_mimo3_to_other(struct iwl_priv *priv,
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
-        u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+        u8 valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         u8 tx_chains_num = priv->hw_params.tx_chains_num;
         int ret;
         u8 update_search_tbl_counter = 0;
@@ -2699,7 +2697,7 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 
         i = lq_sta->last_txrate_idx;
 
-        valid_tx_ant = priv->hw_params.valid_tx_ant;
+        valid_tx_ant = priv->eeprom_data->valid_tx_ant;
 
         if (!lq_sta->search_better_tbl)
                 active_tbl = lq_sta->active_tbl;
@@ -2893,15 +2891,15 @@ void iwl_rs_rate_init(struct iwl_priv *priv, struct ieee80211_sta *sta, u8 sta_i
 
         /* These values will be overridden later */
         lq_sta->lq.general_params.single_stream_ant_msk =
-                first_antenna(priv->hw_params.valid_tx_ant);
+                first_antenna(priv->eeprom_data->valid_tx_ant);
         lq_sta->lq.general_params.dual_stream_ant_msk =
-                priv->hw_params.valid_tx_ant &
-                ~first_antenna(priv->hw_params.valid_tx_ant);
+                priv->eeprom_data->valid_tx_ant &
+                ~first_antenna(priv->eeprom_data->valid_tx_ant);
         if (!lq_sta->lq.general_params.dual_stream_ant_msk) {
                 lq_sta->lq.general_params.dual_stream_ant_msk = ANT_AB;
-        } else if (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+        } else if (num_of_ant(priv->eeprom_data->valid_tx_ant) == 2) {
                 lq_sta->lq.general_params.dual_stream_ant_msk =
-                        priv->hw_params.valid_tx_ant;
+                        priv->eeprom_data->valid_tx_ant;
         }
 
         /* as default allow aggregation for all tids */
@@ -2947,7 +2945,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
         if (priv && priv->bt_full_concurrent) {
                 /* 1x1 only */
                 tbl_type.ant_type =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
         }
 
         /* How many times should we repeat the initial rate? */
@@ -2979,7 +2977,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
                 if (priv->bt_full_concurrent)
                         valid_tx_ant = ANT_A;
                 else
-                        valid_tx_ant = priv->hw_params.valid_tx_ant;
+                        valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         }
 
         /* Fill rest of rate table */
@@ -3013,7 +3011,7 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
                 if (priv && priv->bt_full_concurrent) {
                         /* 1x1 only */
                         tbl_type.ant_type =
-                            first_antenna(priv->hw_params.valid_tx_ant);
+                            first_antenna(priv->eeprom_data->valid_tx_ant);
                 }
 
                 /* Indicate to uCode which entries might be MIMO.
@@ -3100,7 +3098,7 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
         u8 ant_sel_tx;
 
         priv = lq_sta->drv;
-        valid_tx_ant = priv->hw_params.valid_tx_ant;
+        valid_tx_ant = priv->eeprom_data->valid_tx_ant;
         if (lq_sta->dbg_fixed_rate) {
                 ant_sel_tx =
                   ((lq_sta->dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK)
@@ -3171,9 +3169,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
         desc += sprintf(buff+desc, "fixed rate 0x%X\n",
                         lq_sta->dbg_fixed_rate);
         desc += sprintf(buff+desc, "valid_tx_ant %s%s%s\n",
-            (priv->hw_params.valid_tx_ant & ANT_A) ? "ANT_A," : "",
-            (priv->hw_params.valid_tx_ant & ANT_B) ? "ANT_B," : "",
-            (priv->hw_params.valid_tx_ant & ANT_C) ? "ANT_C" : "");
+            (priv->eeprom_data->valid_tx_ant & ANT_A) ? "ANT_A," : "",
+            (priv->eeprom_data->valid_tx_ant & ANT_B) ? "ANT_B," : "",
+            (priv->eeprom_data->valid_tx_ant & ANT_C) ? "ANT_C" : "");
         desc += sprintf(buff+desc, "lq type %s\n",
            (is_legacy(tbl->lq_type)) ? "legacy" : "HT");
         if (is_Ht(tbl->lq_type)) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h b/drivers/net/wireless/iwlwifi/dvm/rs.h
index 82d02e1ae89..ad3aea8f626 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.h
+++ b/drivers/net/wireless/iwlwifi/dvm/rs.h
@@ -29,9 +29,10 @@
 
 #include <net/mac80211.h>
 
-#include "iwl-commands.h"
 #include "iwl-config.h"
 
+#include "commands.h"
+
 struct iwl_rate_info {
         u8 plcp;        /* uCode API:  IWL_RATE_6M_PLCP, etc. */
         u8 plcp_siso;   /* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c b/drivers/net/wireless/iwlwifi/dvm/rx.c
index 403de96f974..c1f7a18e08d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rx.c
@@ -32,12 +32,10 @@
 #include <linux/sched.h>
 #include <net/mac80211.h>
 #include <asm/unaligned.h>
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-calib.h"
-#include "iwl-agn.h"
-#include "iwl-modparams.h"
+#include "dev.h"
+#include "calib.h"
+#include "agn.h"
 
 #define IWL_CMD_ENTRY(x) [x] = #x
 
@@ -1012,6 +1010,8 @@ static int iwlagn_rx_reply_rx(struct iwl_priv *priv,
                 rx_status.flag |= RX_FLAG_40MHZ;
         if (rate_n_flags & RATE_MCS_SGI_MSK)
                 rx_status.flag |= RX_FLAG_SHORT_GI;
+        if (rate_n_flags & RATE_MCS_GF_MSK)
+                rx_status.flag |= RX_FLAG_HT_GF;
 
         iwlagn_pass_packet_to_mac80211(priv, header, len, ampdu_status,
                                     rxb, &rx_status);
@@ -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/iwl-agn-rxon.c b/drivers/net/wireless/iwlwifi/dvm/rxon.c
index 0a3aa7c8300..6ee940f497f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rxon.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rxon.c
@@ -25,11 +25,11 @@
  *****************************************************************************/
 
 #include <linux/etherdevice.h>
-#include "iwl-dev.h"
-#include "iwl-agn.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
 #include "iwl-modparams.h"
+#include "dev.h"
+#include "agn.h"
+#include "calib.h"
 
 /*
  * initialize rxon structure with default values from eeprom
@@ -37,8 +37,6 @@
 void iwl_connection_init_rx_config(struct iwl_priv *priv,
                                    struct iwl_rxon_context *ctx)
 {
-        const struct iwl_channel_info *ch_info;
-
         memset(&ctx->staging, 0, sizeof(ctx->staging));
 
         if (!ctx->vif) {
@@ -80,14 +78,8 @@ void iwl_connection_init_rx_config(struct iwl_priv *priv,
                 ctx->staging.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
 #endif
 
-        ch_info = iwl_get_channel_info(priv, priv->band,
-                                       le16_to_cpu(ctx->active.channel));
-
-        if (!ch_info)
-                ch_info = &priv->channel_info[0];
-
-        ctx->staging.channel = cpu_to_le16(ch_info->channel);
-        priv->band = ch_info->band;
+        ctx->staging.channel = cpu_to_le16(priv->hw->conf.channel->hw_value);
+        priv->band = priv->hw->conf.channel->band;
 
         iwl_set_flags_for_band(priv, ctx, priv->band, ctx->vif);
 
@@ -175,7 +167,8 @@ static int iwlagn_disconn_pan(struct iwl_priv *priv,
         return ret;
 }
 
-void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
+static void iwlagn_update_qos(struct iwl_priv *priv,
+                              struct iwl_rxon_context *ctx)
 {
         int ret;
 
@@ -202,8 +195,8 @@ void iwlagn_update_qos(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
                 IWL_DEBUG_QUIET_RFKILL(priv, "Failed to update QoS\n");
 }
 
-int iwlagn_update_beacon(struct iwl_priv *priv,
-                         struct ieee80211_vif *vif)
+static int iwlagn_update_beacon(struct iwl_priv *priv,
+                                struct ieee80211_vif *vif)
 {
         lockdep_assert_held(&priv->mutex);
 
@@ -215,7 +208,7 @@ int iwlagn_update_beacon(struct iwl_priv *priv,
 }
 
 static int iwlagn_send_rxon_assoc(struct iwl_priv *priv,
-                           struct iwl_rxon_context *ctx)
+                                  struct iwl_rxon_context *ctx)
 {
         int ret = 0;
         struct iwl_rxon_assoc_cmd rxon_assoc;
@@ -427,10 +420,10 @@ static int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
                 return -EINVAL;
         }
 
-        if (tx_power > priv->tx_power_device_lmt) {
+        if (tx_power > DIV_ROUND_UP(priv->eeprom_data->max_tx_pwr_half_dbm, 2)) {
                 IWL_WARN(priv,
                         "Requested user TXPOWER %d above upper limit %d.\n",
-                         tx_power, priv->tx_power_device_lmt);
+                         tx_power, priv->eeprom_data->max_tx_pwr_half_dbm);
                 return -EINVAL;
         }
 
@@ -863,8 +856,8 @@ static int iwl_check_rxon_cmd(struct iwl_priv *priv,
  * or is clearing the RXON_FILTER_ASSOC_MSK, then return 1 to indicate that
  * a new tune (full RXON command, rather than RXON_ASSOC cmd) is required.
  */
-int iwl_full_rxon_required(struct iwl_priv *priv,
-                           struct iwl_rxon_context *ctx)
+static int iwl_full_rxon_required(struct iwl_priv *priv,
+                                  struct iwl_rxon_context *ctx)
 {
         const struct iwl_rxon_cmd *staging = &ctx->staging;
         const struct iwl_rxon_cmd *active = &ctx->active;
@@ -1189,7 +1182,6 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
         struct iwl_rxon_context *ctx;
         struct ieee80211_conf *conf = &hw->conf;
         struct ieee80211_channel *channel = conf->channel;
-        const struct iwl_channel_info *ch_info;
         int ret = 0;
 
         IWL_DEBUG_MAC80211(priv, "enter: changed %#x\n", changed);
@@ -1223,14 +1215,6 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
         }
 
         if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-                ch_info = iwl_get_channel_info(priv, channel->band,
-                                               channel->hw_value);
-                if (!is_channel_valid(ch_info)) {
-                        IWL_DEBUG_MAC80211(priv, "leave - invalid channel\n");
-                        ret = -EINVAL;
-                        goto out;
-                }
-
                 for_each_context(priv, ctx) {
                         /* Configure HT40 channels */
                         if (ctx->ht.enabled != conf_is_ht(conf))
@@ -1294,9 +1278,9 @@ int iwlagn_mac_config(struct ieee80211_hw *hw, u32 changed)
         return ret;
 }
 
-void iwlagn_check_needed_chains(struct iwl_priv *priv,
-                                struct iwl_rxon_context *ctx,
-                                struct ieee80211_bss_conf *bss_conf)
+static void iwlagn_check_needed_chains(struct iwl_priv *priv,
+                                       struct iwl_rxon_context *ctx,
+                                       struct ieee80211_bss_conf *bss_conf)
 {
         struct ieee80211_vif *vif = ctx->vif;
         struct iwl_rxon_context *tmp;
@@ -1388,7 +1372,7 @@ void iwlagn_check_needed_chains(struct iwl_priv *priv,
         ht_conf->single_chain_sufficient = !need_multiple;
 }
 
-void iwlagn_chain_noise_reset(struct iwl_priv *priv)
+static void iwlagn_chain_noise_reset(struct iwl_priv *priv)
 {
         struct iwl_chain_noise_data *data = &priv->chain_noise_data;
         int ret;
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/dvm/scan.c
index 031d8e21f82..6633074258c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scan.c
+++ b/drivers/net/wireless/iwlwifi/dvm/scan.c
@@ -30,11 +30,8 @@
 #include <linux/etherdevice.h>
 #include <net/mac80211.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-trans.h"
+#include "dev.h"
+#include "agn.h"
 
 /* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
  * sending probe req.  This should be set long enough to hear probe responses
@@ -54,6 +51,9 @@
 #define IWL_CHANNEL_TUNE_TIME       5
 #define MAX_SCAN_CHANNEL            50
 
+/* For reset radio, need minimal dwell time only */
+#define IWL_RADIO_RESET_DWELL_TIME      5
+
 static int iwl_send_scan_abort(struct iwl_priv *priv)
 {
         int ret;
@@ -67,7 +67,6 @@ static int iwl_send_scan_abort(struct iwl_priv *priv)
          * to receive scan abort command or it does not perform
          * hardware scan currently */
         if (!test_bit(STATUS_READY, &priv->status) ||
-            !test_bit(STATUS_GEO_CONFIGURED, &priv->status) ||
             !test_bit(STATUS_SCAN_HW, &priv->status) ||
             test_bit(STATUS_FW_ERROR, &priv->status))
                 return -EIO;
@@ -101,11 +100,8 @@ static void iwl_complete_scan(struct iwl_priv *priv, bool aborted)
                 ieee80211_scan_completed(priv->hw, aborted);
         }
 
-        if (priv->scan_type == IWL_SCAN_ROC) {
-                ieee80211_remain_on_channel_expired(priv->hw);
-                priv->hw_roc_channel = NULL;
-                schedule_delayed_work(&priv->hw_roc_disable_work, 10 * HZ);
-        }
+        if (priv->scan_type == IWL_SCAN_ROC)
+                iwl_scan_roc_expired(priv);
 
         priv->scan_type = IWL_SCAN_NORMAL;
         priv->scan_vif = NULL;
@@ -134,11 +130,8 @@ static void iwl_process_scan_complete(struct iwl_priv *priv)
                 goto out_settings;
         }
 
-        if (priv->scan_type == IWL_SCAN_ROC) {
-                ieee80211_remain_on_channel_expired(priv->hw);
-                priv->hw_roc_channel = NULL;
-                schedule_delayed_work(&priv->hw_roc_disable_work, 10 * HZ);
-        }
+        if (priv->scan_type == IWL_SCAN_ROC)
+                iwl_scan_roc_expired(priv);
 
         if (priv->scan_type != IWL_SCAN_NORMAL && !aborted) {
                 int err;
@@ -453,27 +446,17 @@ static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
 
 /* Return valid, unused, channel for a passive scan to reset the RF */
 static u8 iwl_get_single_channel_number(struct iwl_priv *priv,
-                                 enum ieee80211_band band)
+                                        enum ieee80211_band band)
 {
-        const struct iwl_channel_info *ch_info;
-        int i;
-        u8 channel = 0;
-        u8 min, max;
+        struct ieee80211_supported_band *sband = priv->hw->wiphy->bands[band];
         struct iwl_rxon_context *ctx;
+        int i;
 
-        if (band == IEEE80211_BAND_5GHZ) {
-                min = 14;
-                max = priv->channel_count;
-        } else {
-                min = 0;
-                max = 14;
-        }
-
-        for (i = min; i < max; i++) {
+        for (i = 0; i < sband->n_channels; i++) {
                 bool busy = false;
 
                 for_each_context(priv, ctx) {
-                        busy = priv->channel_info[i].channel ==
+                        busy = sband->channels[i].hw_value ==
                                 le16_to_cpu(ctx->staging.channel);
                         if (busy)
                                 break;
@@ -482,54 +465,46 @@ static u8 iwl_get_single_channel_number(struct iwl_priv *priv,
                 if (busy)
                         continue;
 
-                channel = priv->channel_info[i].channel;
-                ch_info = iwl_get_channel_info(priv, band, channel);
-                if (is_channel_valid(ch_info))
-                        break;
+                if (!(sband->channels[i].flags & IEEE80211_CHAN_DISABLED))
+                        return sband->channels[i].hw_value;
         }
 
-        return channel;
+        return 0;
 }
 
-static int iwl_get_single_channel_for_scan(struct iwl_priv *priv,
-                                           struct ieee80211_vif *vif,
-                                           enum ieee80211_band band,
-                                           struct iwl_scan_channel *scan_ch)
+static int iwl_get_channel_for_reset_scan(struct iwl_priv *priv,
+                                          struct ieee80211_vif *vif,
+                                          enum ieee80211_band band,
+                                          struct iwl_scan_channel *scan_ch)
 {
         const struct ieee80211_supported_band *sband;
-        u16 passive_dwell = 0;
-        u16 active_dwell = 0;
-        int added = 0;
-        u16 channel = 0;
+        u16 channel;
 
         sband = iwl_get_hw_mode(priv, band);
         if (!sband) {
                 IWL_ERR(priv, "invalid band\n");
-                return added;
+                return 0;
         }
 
-        active_dwell = iwl_get_active_dwell_time(priv, band, 0);
-        passive_dwell = iwl_get_passive_dwell_time(priv, band);
-
-        if (passive_dwell <= active_dwell)
-                passive_dwell = active_dwell + 1;
-
         channel = iwl_get_single_channel_number(priv, band);
         if (channel) {
                 scan_ch->channel = cpu_to_le16(channel);
                 scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
-                scan_ch->active_dwell = cpu_to_le16(active_dwell);
-                scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+                scan_ch->active_dwell =
+                        cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
+                scan_ch->passive_dwell =
+                        cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
                 /* Set txpower levels to defaults */
                 scan_ch->dsp_atten = 110;
                 if (band == IEEE80211_BAND_5GHZ)
                         scan_ch->tx_gain = ((1 << 5) | (3 << 3)) | 3;
                 else
                         scan_ch->tx_gain = ((1 << 5) | (5 << 3));
-                added++;
-        } else
-                IWL_ERR(priv, "no valid channel found\n");
-        return added;
+                return 1;
+        }
+
+        IWL_ERR(priv, "no valid channel found\n");
+        return 0;
 }
 
 static int iwl_get_channels_for_scan(struct iwl_priv *priv,
@@ -540,7 +515,6 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
 {
         struct ieee80211_channel *chan;
         const struct ieee80211_supported_band *sband;
-        const struct iwl_channel_info *ch_info;
         u16 passive_dwell = 0;
         u16 active_dwell = 0;
         int added, i;
@@ -565,16 +539,7 @@ static int iwl_get_channels_for_scan(struct iwl_priv *priv,
                 channel = chan->hw_value;
                 scan_ch->channel = cpu_to_le16(channel);
 
-                ch_info = iwl_get_channel_info(priv, band, channel);
-                if (!is_channel_valid(ch_info)) {
-                        IWL_DEBUG_SCAN(priv,
-                                       "Channel %d is INVALID for this band.\n",
-                                       channel);
-                        continue;
-                }
-
-                if (!is_active || is_channel_passive(ch_info) ||
-                    (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN))
+                if (!is_active || (chan->flags & IEEE80211_CHAN_PASSIVE_SCAN))
                         scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
                 else
                         scan_ch->type = SCAN_CHANNEL_TYPE_ACTIVE;
@@ -678,12 +643,12 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         u16 rx_chain = 0;
         enum ieee80211_band band;
         u8 n_probes = 0;
-        u8 rx_ant = priv->hw_params.valid_rx_ant;
+        u8 rx_ant = priv->eeprom_data->valid_rx_ant;
         u8 rate;
         bool is_active = false;
         int  chan_mod;
         u8 active_chains;
-        u8 scan_tx_antennas = priv->hw_params.valid_tx_ant;
+        u8 scan_tx_antennas = priv->eeprom_data->valid_tx_ant;
         int ret;
         int scan_cmd_size = sizeof(struct iwl_scan_cmd) +
                             MAX_SCAN_CHANNEL * sizeof(struct iwl_scan_channel) +
@@ -755,6 +720,12 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         switch (priv->scan_type) {
         case IWL_SCAN_RADIO_RESET:
                 IWL_DEBUG_SCAN(priv, "Start internal passive scan.\n");
+                /*
+                 * Override quiet time as firmware checks that active
+                 * dwell is >= quiet; since we use passive scan it'll
+                 * not actually be used.
+                 */
+                scan->quiet_time = cpu_to_le16(IWL_RADIO_RESET_DWELL_TIME);
                 break;
         case IWL_SCAN_NORMAL:
                 if (priv->scan_request->n_ssids) {
@@ -893,7 +864,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
         /* MIMO is not used here, but value is required */
         rx_chain |=
-                priv->hw_params.valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
+                priv->eeprom_data->valid_rx_ant << RXON_RX_CHAIN_VALID_POS;
         rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS;
         rx_chain |= rx_ant << RXON_RX_CHAIN_FORCE_SEL_POS;
         rx_chain |= 0x1 << RXON_RX_CHAIN_DRIVER_FORCE_POS;
@@ -928,7 +899,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         switch (priv->scan_type) {
         case IWL_SCAN_RADIO_RESET:
                 scan->channel_count =
-                        iwl_get_single_channel_for_scan(priv, vif, band,
+                        iwl_get_channel_for_reset_scan(priv, vif, band,
                                 (void *)&scan->data[cmd_len]);
                 break;
         case IWL_SCAN_NORMAL:
@@ -994,8 +965,10 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
         set_bit(STATUS_SCAN_HW, &priv->status);
 
         ret = iwlagn_set_pan_params(priv);
-        if (ret)
+        if (ret) {
+                clear_bit(STATUS_SCAN_HW, &priv->status);
                 return ret;
+        }
 
         ret = iwl_dvm_send_cmd(priv, &cmd);
         if (ret) {
@@ -1008,7 +981,7 @@ static int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
 
 void iwl_init_scan_params(struct iwl_priv *priv)
 {
-        u8 ant_idx = fls(priv->hw_params.valid_tx_ant) - 1;
+        u8 ant_idx = fls(priv->eeprom_data->valid_tx_ant) - 1;
         if (!priv->scan_tx_ant[IEEE80211_BAND_5GHZ])
                 priv->scan_tx_ant[IEEE80211_BAND_5GHZ] = ant_idx;
         if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
@@ -1158,3 +1131,40 @@ void iwl_cancel_scan_deferred_work(struct iwl_priv *priv)
                 mutex_unlock(&priv->mutex);
         }
 }
+
+void iwl_scan_roc_expired(struct iwl_priv *priv)
+{
+        /*
+         * The status bit should be set here, to prevent a race
+         * where the atomic_read returns 1, but before the execution continues
+         * iwl_scan_offchannel_skb_status() checks if the status bit is set
+         */
+        set_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status);
+
+        if (atomic_read(&priv->num_aux_in_flight) == 0) {
+                ieee80211_remain_on_channel_expired(priv->hw);
+                priv->hw_roc_channel = NULL;
+                schedule_delayed_work(&priv->hw_roc_disable_work,
+                                      10 * HZ);
+
+                clear_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status);
+        } else {
+                IWL_DEBUG_SCAN(priv, "ROC done with %d frames in aux\n",
+                               atomic_read(&priv->num_aux_in_flight));
+        }
+}
+
+void iwl_scan_offchannel_skb(struct iwl_priv *priv)
+{
+        WARN_ON(!priv->hw_roc_start_notified);
+        atomic_inc(&priv->num_aux_in_flight);
+}
+
+void iwl_scan_offchannel_skb_status(struct iwl_priv *priv)
+{
+        if (atomic_dec_return(&priv->num_aux_in_flight) == 0 &&
+            test_bit(STATUS_SCAN_ROC_EXPIRED, &priv->status)) {
+                IWL_DEBUG_SCAN(priv, "0 aux frames. Calling ROC expired\n");
+                iwl_scan_roc_expired(priv);
+        }
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c b/drivers/net/wireless/iwlwifi/dvm/sta.c
index 36055ed1c06..b29b798f755 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-sta.c
+++ b/drivers/net/wireless/iwlwifi/dvm/sta.c
@@ -28,10 +28,9 @@
  *****************************************************************************/
 #include <linux/etherdevice.h>
 #include <net/mac80211.h>
-
-#include "iwl-dev.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
+#include "dev.h"
+#include "agn.h"
 
 const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 
@@ -171,26 +170,6 @@ int iwl_send_add_sta(struct iwl_priv *priv,
         return cmd.handler_status;
 }
 
-static bool iwl_is_channel_extension(struct iwl_priv *priv,
-                                     enum ieee80211_band band,
-                                     u16 channel, u8 extension_chan_offset)
-{
-        const struct iwl_channel_info *ch_info;
-
-        ch_info = iwl_get_channel_info(priv, band, channel);
-        if (!is_channel_valid(ch_info))
-                return false;
-
-        if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_ABOVE)
-                return !(ch_info->ht40_extension_channel &
-                                        IEEE80211_CHAN_NO_HT40PLUS);
-        else if (extension_chan_offset == IEEE80211_HT_PARAM_CHA_SEC_BELOW)
-                return !(ch_info->ht40_extension_channel &
-                                        IEEE80211_CHAN_NO_HT40MINUS);
-
-        return false;
-}
-
 bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
                             struct iwl_rxon_context *ctx,
                             struct ieee80211_sta_ht_cap *ht_cap)
@@ -198,21 +177,25 @@ bool iwl_is_ht40_tx_allowed(struct iwl_priv *priv,
         if (!ctx->ht.enabled || !ctx->ht.is_40mhz)
                 return false;
 
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+        if (priv->disable_ht40)
+                return false;
+#endif
+
         /*
-         * We do not check for IEEE80211_HT_CAP_SUP_WIDTH_20_40
-         * the bit will not set if it is pure 40MHz case
+         * Remainder of this function checks ht_cap, but if it's
+         * NULL then we can do HT40 (special case for RXON)
          */
-        if (ht_cap && !ht_cap->ht_supported)
+        if (!ht_cap)
+                return true;
+
+        if (!ht_cap->ht_supported)
                 return false;
 
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-        if (priv->disable_ht40)
+        if (!(ht_cap->cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
                 return false;
-#endif
 
-        return iwl_is_channel_extension(priv, priv->band,
-                        le16_to_cpu(ctx->staging.channel),
-                        ctx->ht.extension_chan_offset);
+        return true;
 }
 
 static void iwl_sta_calc_ht_flags(struct iwl_priv *priv,
@@ -650,23 +633,23 @@ static void iwl_sta_fill_lq(struct iwl_priv *priv, struct iwl_rxon_context *ctx,
         if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
                 rate_flags |= RATE_MCS_CCK_MSK;
 
-        rate_flags |= first_antenna(priv->hw_params.valid_tx_ant) <<
+        rate_flags |= first_antenna(priv->eeprom_data->valid_tx_ant) <<
                                 RATE_MCS_ANT_POS;
         rate_n_flags = iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
         for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
                 link_cmd->rs_table[i].rate_n_flags = rate_n_flags;
 
         link_cmd->general_params.single_stream_ant_msk =
-                        first_antenna(priv->hw_params.valid_tx_ant);
+                        first_antenna(priv->eeprom_data->valid_tx_ant);
 
         link_cmd->general_params.dual_stream_ant_msk =
-                priv->hw_params.valid_tx_ant &
-                ~first_antenna(priv->hw_params.valid_tx_ant);
+                priv->eeprom_data->valid_tx_ant &
+                ~first_antenna(priv->eeprom_data->valid_tx_ant);
         if (!link_cmd->general_params.dual_stream_ant_msk) {
                 link_cmd->general_params.dual_stream_ant_msk = ANT_AB;
-        } else if (num_of_ant(priv->hw_params.valid_tx_ant) == 2) {
+        } else if (num_of_ant(priv->eeprom_data->valid_tx_ant) == 2) {
                 link_cmd->general_params.dual_stream_ant_msk =
-                        priv->hw_params.valid_tx_ant;
+                        priv->eeprom_data->valid_tx_ant;
         }
 
         link_cmd->agg_params.agg_dis_start_th =
@@ -1268,7 +1251,7 @@ int iwl_remove_dynamic_key(struct iwl_priv *priv,
                 key_flags |= STA_KEY_MULTICAST_MSK;
 
         sta_cmd.key.key_flags = key_flags;
-        sta_cmd.key.key_offset = WEP_INVALID_OFFSET;
+        sta_cmd.key.key_offset = keyconf->hw_key_idx;
         sta_cmd.sta.modify_mask = STA_MODIFY_KEY_MASK;
         sta_cmd.mode = STA_CONTROL_MODIFY_MSK;
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/testmode.c b/drivers/net/wireless/iwlwifi/dvm/testmode.c
new file mode 100644
index 00000000000..57b918ce3b5
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/dvm/testmode.c
@@ -0,0 +1,471 @@
+/******************************************************************************
+ *
+ * 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/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/dma-mapping.h>
+#include <net/net_namespace.h>
+#include <linux/netdevice.h>
+#include <net/cfg80211.h>
+#include <net/mac80211.h>
+#include <net/netlink.h>
+
+#include "iwl-debug.h"
+#include "iwl-trans.h"
+#include "dev.h"
+#include "agn.h"
+#include "iwl-test.h"
+#include "iwl-testmode.h"
+
+static int iwl_testmode_send_cmd(struct iwl_op_mode *op_mode,
+                                 struct iwl_host_cmd *cmd)
+{
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return iwl_dvm_send_cmd(priv, cmd);
+}
+
+static bool iwl_testmode_valid_hw_addr(u32 addr)
+{
+        if (iwlagn_hw_valid_rtc_data_addr(addr))
+                return true;
+
+        if (IWLAGN_RTC_INST_LOWER_BOUND <= addr &&
+            addr < IWLAGN_RTC_INST_UPPER_BOUND)
+                return true;
+
+        return false;
+}
+
+static u32 iwl_testmode_get_fw_ver(struct iwl_op_mode *op_mode)
+{
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return priv->fw->ucode_ver;
+}
+
+static struct sk_buff*
+iwl_testmode_alloc_reply(struct iwl_op_mode *op_mode, int len)
+{
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return cfg80211_testmode_alloc_reply_skb(priv->hw->wiphy, len);
+}
+
+static int iwl_testmode_reply(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+{
+        return cfg80211_testmode_reply(skb);
+}
+
+static struct sk_buff *iwl_testmode_alloc_event(struct iwl_op_mode *op_mode,
+                                                int len)
+{
+        struct iwl_priv *priv = IWL_OP_MODE_GET_DVM(op_mode);
+        return cfg80211_testmode_alloc_event_skb(priv->hw->wiphy, len,
+                                                 GFP_ATOMIC);
+}
+
+static void iwl_testmode_event(struct iwl_op_mode *op_mode, struct sk_buff *skb)
+{
+        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,
+};
+
+void iwl_testmode_init(struct iwl_priv *priv)
+{
+        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)
+{
+        struct iwl_notification_wait calib_wait;
+        static const u8 calib_complete[] = {
+                CALIBRATION_COMPLETE_NOTIFICATION
+        };
+        int ret;
+
+        iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
+                                   calib_complete, ARRAY_SIZE(calib_complete),
+                                   NULL, NULL);
+        ret = iwl_init_alive_start(priv);
+        if (ret) {
+                IWL_ERR(priv, "Fail init calibration: %d\n", ret);
+                goto cfg_init_calib_error;
+        }
+
+        ret = iwl_wait_notification(&priv->notif_wait, &calib_wait, 2 * HZ);
+        if (ret)
+                IWL_ERR(priv, "Error detecting"
+                        " CALIBRATION_COMPLETE_NOTIFICATION: %d\n", ret);
+        return ret;
+
+cfg_init_calib_error:
+        iwl_remove_notification(&priv->notif_wait, &calib_wait);
+        return ret;
+}
+
+/*
+ * This function handles the user application commands for driver.
+ *
+ * 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.
+ *
+ * If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
+ * is used for carry the message while IWL_TM_ATTR_COMMAND must set to
+ * IWL_TM_CMD_DEV2APP_SYNC_RSP.
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @tb: gnl message fields from the user space
+ */
+static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
+{
+        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+        struct iwl_trans *trans = priv->trans;
+        struct sk_buff *skb;
+        unsigned char *rsp_data_ptr = NULL;
+        int status = 0, rsp_data_len = 0;
+        u32 inst_size = 0, data_size = 0;
+        const struct fw_img *img;
+
+        switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
+        case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
+                rsp_data_ptr = (unsigned char *)priv->cfg->name;
+                rsp_data_len = strlen(priv->cfg->name);
+                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+                                                        rsp_data_len + 20);
+                if (!skb) {
+                        IWL_ERR(priv, "Memory allocation fail\n");
+                        return -ENOMEM;
+                }
+                if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+                                IWL_TM_CMD_DEV2APP_SYNC_RSP) ||
+                    nla_put(skb, IWL_TM_ATTR_SYNC_RSP,
+                            rsp_data_len, rsp_data_ptr))
+                        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_LOAD_INIT_FW:
+                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
+                if (status)
+                        IWL_ERR(priv, "Error loading init ucode: %d\n", status);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
+                iwl_testmode_cfg_init_calib(priv);
+                priv->ucode_loaded = false;
+                iwl_trans_stop_device(trans);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
+                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_REGULAR);
+                if (status) {
+                        IWL_ERR(priv,
+                                "Error loading runtime ucode: %d\n", status);
+                        break;
+                }
+                status = iwl_alive_start(priv);
+                if (status)
+                        IWL_ERR(priv,
+                                "Error starting the device: %d\n", status);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
+                iwl_scan_cancel_timeout(priv, 200);
+                priv->ucode_loaded = false;
+                iwl_trans_stop_device(trans);
+                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_WOWLAN);
+                if (status) {
+                        IWL_ERR(priv,
+                                "Error loading WOWLAN ucode: %d\n", status);
+                        break;
+                }
+                status = iwl_alive_start(priv);
+                if (status)
+                        IWL_ERR(priv,
+                                "Error starting the device: %d\n", status);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_GET_EEPROM:
+                if (priv->eeprom_blob) {
+                        skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
+                                priv->eeprom_blob_size + 20);
+                        if (!skb) {
+                                IWL_ERR(priv, "Memory allocation fail\n");
+                                return -ENOMEM;
+                        }
+                        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
+                                        IWL_TM_CMD_DEV2APP_EEPROM_RSP) ||
+                            nla_put(skb, IWL_TM_ATTR_EEPROM,
+                                    priv->eeprom_blob_size,
+                                    priv->eeprom_blob))
+                                goto nla_put_failure;
+                        status = cfg80211_testmode_reply(skb);
+                        if (status < 0)
+                                IWL_ERR(priv, "Error sending msg : %d\n",
+                                        status);
+                } else
+                        return -ENODATA;
+                break;
+
+        case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
+                if (!tb[IWL_TM_ATTR_FIXRATE]) {
+                        IWL_ERR(priv, "Missing fixrate setting\n");
+                        return -ENOMSG;
+                }
+                priv->tm_fixed_rate = nla_get_u32(tb[IWL_TM_ATTR_FIXRATE]);
+                break;
+
+        case IWL_TM_CMD_APP2DEV_GET_FW_INFO:
+                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy, 20 + 8);
+                if (!skb) {
+                        IWL_ERR(priv, "Memory allocation fail\n");
+                        return -ENOMEM;
+                }
+                if (!priv->ucode_loaded) {
+                        IWL_ERR(priv, "No uCode has not been loaded\n");
+                        return -EINVAL;
+                } else {
+                        img = &priv->fw->img[priv->cur_ucode];
+                        inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
+                        data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
+                }
+                if (nla_put_u32(skb, IWL_TM_ATTR_FW_TYPE, priv->cur_ucode) ||
+                    nla_put_u32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size) ||
+                    nla_put_u32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size))
+                        goto nla_put_failure;
+                status = cfg80211_testmode_reply(skb);
+                if (status < 0)
+                        IWL_ERR(priv, "Error sending msg : %d\n", status);
+                break;
+
+        default:
+                IWL_ERR(priv, "Unknown testmode driver command ID\n");
+                return -ENOSYS;
+        }
+        return status;
+
+nla_put_failure:
+        kfree_skb(skb);
+        return -EMSGSIZE;
+}
+
+/*
+ * This function handles the user application switch ucode ownership.
+ *
+ * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_OWNER and
+ * decide who the current owner of the uCode
+ *
+ * If the current owner is OWNERSHIP_TM, then the only host command
+ * can deliver to uCode is from testmode, all the other host commands
+ * will dropped.
+ *
+ * default driver is the owner of uCode in normal operational mode
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @tb: gnl message fields from the user space
+ */
+static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
+{
+        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+        u8 owner;
+
+        if (!tb[IWL_TM_ATTR_UCODE_OWNER]) {
+                IWL_ERR(priv, "Missing ucode owner\n");
+                return -ENOMSG;
+        }
+
+        owner = nla_get_u8(tb[IWL_TM_ATTR_UCODE_OWNER]);
+        if (owner == IWL_OWNERSHIP_DRIVER) {
+                priv->ucode_owner = owner;
+                iwl_test_enable_notifications(&priv->tst, false);
+        } else if (owner == IWL_OWNERSHIP_TM) {
+                priv->ucode_owner = owner;
+                iwl_test_enable_notifications(&priv->tst, true);
+        } else {
+                IWL_ERR(priv, "Invalid owner\n");
+                return -EINVAL;
+        }
+        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.
+ *
+ * This function is invoked when there is user space application sending
+ * gnl message through the testmode tunnel NL80211_CMD_TESTMODE regulated
+ * by nl80211.
+ *
+ * It retrieves the mandatory field, IWL_TM_ATTR_COMMAND, before
+ * dispatching it to the corresponding handler.
+ *
+ * If IWL_TM_ATTR_COMMAND is missing, -ENOMSG is replied to user application;
+ * -ENOSYS is replied to the user application if the command is unknown;
+ * Otherwise, the command is dispatched to the respective handler.
+ *
+ * @hw: ieee80211_hw object that represents the device
+ * @data: pointer to user space message
+ * @len: length in byte of @data
+ */
+int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
+{
+        struct nlattr *tb[IWL_TM_ATTR_MAX];
+        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+        int result;
+
+        result = iwl_test_parse(&priv->tst, tb, data, len);
+        if (result)
+                return result;
+
+        /* 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:
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_READ32:
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE32:
+        case IWL_TM_CMD_APP2DEV_DIRECT_REG_WRITE8:
+        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:
+        case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
+        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_INFO:
+                IWL_DEBUG_INFO(priv, "testmode cmd to driver\n");
+                result = iwl_testmode_driver(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;
+
+        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;
+}
+
+int iwlagn_mac_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *skb,
+                      struct netlink_callback *cb,
+                      void *data, int len)
+{
+        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
+        int result;
+        u32 cmd;
+
+        if (cb->args[3]) {
+                /* offset by 1 since commands start at 0 */
+                cmd = cb->args[3] - 1;
+        } else {
+                struct nlattr *tb[IWL_TM_ATTR_MAX];
+
+                result = iwl_test_parse(&priv->tst, tb, data, len);
+                if (result)
+                        return result;
+
+                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);
+        result = iwl_test_dump(&priv->tst, cmd, skb, cb);
+        mutex_unlock(&priv->mutex);
+        return result;
+}
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c b/drivers/net/wireless/iwlwifi/dvm/tt.c
index a5cfe0aceed..eb864433e59 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tt.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tt.c
@@ -31,17 +31,14 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/init.h>
-
 #include <net/mac80211.h>
-
-#include "iwl-agn.h"
-#include "iwl-eeprom.h"
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-commands.h"
-#include "iwl-debug.h"
-#include "iwl-agn-tt.h"
 #include "iwl-modparams.h"
+#include "iwl-debug.h"
+#include "agn.h"
+#include "dev.h"
+#include "commands.h"
+#include "tt.h"
 
 /* default Thermal Throttling transaction table
  * Current state   |         Throttling Down               |  Throttling Up
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tt.h b/drivers/net/wireless/iwlwifi/dvm/tt.h
index 86bbf47501c..44c7c8f30a2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tt.h
+++ b/drivers/net/wireless/iwlwifi/dvm/tt.h
@@ -28,7 +28,7 @@
 #ifndef __iwl_tt_setting_h__
 #define __iwl_tt_setting_h__
 
-#include "iwl-commands.h"
+#include "commands.h"
 
 #define IWL_ABSOLUTE_ZERO               0
 #define IWL_ABSOLUTE_MAX                0xFFFFFFFF
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 3366e2e2f00..5971a23aa47 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -32,12 +32,11 @@
 #include <linux/init.h>
 #include <linux/sched.h>
 #include <linux/ieee80211.h>
-
-#include "iwl-dev.h"
 #include "iwl-io.h"
-#include "iwl-agn-hw.h"
-#include "iwl-agn.h"
 #include "iwl-trans.h"
+#include "iwl-agn-hw.h"
+#include "dev.h"
+#include "agn.h"
 
 static const u8 tid_to_ac[] = {
         IEEE80211_AC_BE,
@@ -187,7 +186,8 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
         rate_idx = info->control.rates[0].idx;
         if (info->control.rates[0].flags & IEEE80211_TX_RC_MCS ||
                         (rate_idx < 0) || (rate_idx > IWL_RATE_COUNT_LEGACY))
-                rate_idx = rate_lowest_index(&priv->bands[info->band],
+                rate_idx = rate_lowest_index(
+                                &priv->eeprom_data->bands[info->band],
                                 info->control.sta);
         /* For 5 GHZ band, remap mac80211 rate indices into driver indices */
         if (info->band == IEEE80211_BAND_5GHZ)
@@ -207,10 +207,11 @@ static void iwlagn_tx_cmd_build_rate(struct iwl_priv *priv,
              priv->bt_full_concurrent) {
                 /* operated as 1x1 in full concurrency mode */
                 priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-                                first_antenna(priv->hw_params.valid_tx_ant));
+                                first_antenna(priv->eeprom_data->valid_tx_ant));
         } else
-                priv->mgmt_tx_ant = iwl_toggle_tx_ant(priv, priv->mgmt_tx_ant,
-                                                priv->hw_params.valid_tx_ant);
+                priv->mgmt_tx_ant = iwl_toggle_tx_ant(
+                                        priv, priv->mgmt_tx_ant,
+                                        priv->eeprom_data->valid_tx_ant);
         rate_flags |= iwl_ant_idx_to_flags(priv->mgmt_tx_ant);
 
         /* Set the rate in the TX cmd */
@@ -296,7 +297,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct iwl_station_priv *sta_priv = NULL;
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-        struct iwl_device_cmd *dev_cmd = NULL;
+        struct iwl_device_cmd *dev_cmd;
         struct iwl_tx_cmd *tx_cmd;
         __le16 fc;
         u8 hdr_len;
@@ -378,7 +379,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
         if (info->flags & IEEE80211_TX_CTL_AMPDU)
                 is_agg = true;
 
-        dev_cmd = kmem_cache_alloc(iwl_tx_cmd_pool, GFP_ATOMIC);
+        dev_cmd = iwl_trans_alloc_tx_cmd(priv->trans);
 
         if (unlikely(!dev_cmd))
                 goto drop_unlock_priv;
@@ -402,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);
 
@@ -486,11 +488,14 @@ int iwlagn_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
         if (sta_priv && sta_priv->client && !is_agg)
                 atomic_inc(&sta_priv->pending_frames);
 
+        if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
+                iwl_scan_offchannel_skb(priv);
+
         return 0;
 
 drop_unlock_sta:
         if (dev_cmd)
-                kmem_cache_free(iwl_tx_cmd_pool, dev_cmd);
+                iwl_trans_free_tx_cmd(priv->trans, dev_cmd);
         spin_unlock(&priv->sta_lock);
 drop_unlock_priv:
         return -1;
@@ -597,7 +602,7 @@ turn_off:
                  * time, or we hadn't time to drain the AC queues.
                  */
                 if (agg_state == IWL_AGG_ON)
-                        iwl_trans_tx_agg_disable(priv->trans, txq_id);
+                        iwl_trans_txq_disable(priv->trans, txq_id);
                 else
                         IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
                                             agg_state);
@@ -686,9 +691,8 @@ int iwlagn_tx_agg_oper(struct iwl_priv *priv, struct ieee80211_vif *vif,
 
         fifo = ctx->ac_to_fifo[tid_to_ac[tid]];
 
-        iwl_trans_tx_agg_setup(priv->trans, q, fifo,
-                               sta_priv->sta_id, tid,
-                               buf_size, ssn);
+        iwl_trans_txq_enable(priv->trans, q, fifo, sta_priv->sta_id, tid,
+                             buf_size, ssn);
 
         /*
          * If the limit is 0, then it wasn't initialised yet,
@@ -753,8 +757,8 @@ static void iwlagn_check_ratid_empty(struct iwl_priv *priv, int sta_id, u8 tid)
                         IWL_DEBUG_TX_QUEUES(priv,
                                 "Can continue DELBA flow ssn = next_recl ="
                                 " %d", tid_data->next_reclaimed);
-                        iwl_trans_tx_agg_disable(priv->trans,
-                                                 tid_data->agg.txq_id);
+                        iwl_trans_txq_disable(priv->trans,
+                                              tid_data->agg.txq_id);
                         iwlagn_dealloc_agg_txq(priv, tid_data->agg.txq_id);
                         tid_data->agg.state = IWL_AGG_OFF;
                         ieee80211_stop_tx_ba_cb_irqsafe(vif, addr, tid);
@@ -1136,6 +1140,7 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
         struct sk_buff *skb;
         struct iwl_rxon_context *ctx;
         bool is_agg = (txq_id >= IWLAGN_FIRST_AMPDU_QUEUE);
+        bool is_offchannel_skb;
 
         tid = (tx_resp->ra_tid & IWLAGN_TX_RES_TID_MSK) >>
                 IWLAGN_TX_RES_TID_POS;
@@ -1149,6 +1154,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 
         __skb_queue_head_init(&skbs);
 
+        is_offchannel_skb = false;
+
         if (tx_resp->frame_count == 1) {
                 u16 next_reclaimed = le16_to_cpu(tx_resp->seq_ctl);
                 next_reclaimed = SEQ_TO_SN(next_reclaimed + 0x10);
@@ -1176,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;
 
@@ -1189,8 +1197,8 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
 
                         info = IEEE80211_SKB_CB(skb);
                         ctx = info->driver_data[0];
-                        kmem_cache_free(iwl_tx_cmd_pool,
-                                        (info->driver_data[1]));
+                        iwl_trans_free_tx_cmd(priv->trans,
+                                              info->driver_data[1]);
 
                         memset(&info->status, 0, sizeof(info->status));
 
@@ -1225,10 +1233,19 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
                         if (!is_agg)
                                 iwlagn_non_agg_tx_status(priv, ctx, hdr->addr1);
 
+                        is_offchannel_skb =
+                                (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN);
                         freed++;
                 }
 
                 WARN_ON(!is_agg && freed != 1);
+
+                /*
+                 * An offchannel frame can be send only on the AUX queue, where
+                 * there is no aggregation (and reordering) so it only is single
+                 * skb is expected to be processed.
+                 */
+                WARN_ON(is_offchannel_skb && freed != 1);
         }
 
         iwl_check_abort_status(priv, tx_resp->frame_count, status);
@@ -1239,6 +1256,9 @@ int iwlagn_rx_reply_tx(struct iwl_priv *priv, struct iwl_rx_cmd_buffer *rxb,
                 ieee80211_tx_status(priv->hw, skb);
         }
 
+        if (is_offchannel_skb)
+                iwl_scan_offchannel_skb_status(priv);
+
         return 0;
 }
 
@@ -1341,7 +1361,7 @@ int iwlagn_rx_reply_compressed_ba(struct iwl_priv *priv,
                         WARN_ON_ONCE(1);
 
                 info = IEEE80211_SKB_CB(skb);
-                kmem_cache_free(iwl_tx_cmd_pool, (info->driver_data[1]));
+                iwl_trans_free_tx_cmd(priv->trans, info->driver_data[1]);
 
                 if (freed == 1) {
                         /* this is the first skb we deliver in this batch */
diff --git a/drivers/net/wireless/iwlwifi/iwl-ucode.c b/drivers/net/wireless/iwlwifi/dvm/ucode.c
index bc40dc68b0f..b3a314ba48c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-ucode.c
+++ b/drivers/net/wireless/iwlwifi/dvm/ucode.c
@@ -30,15 +30,16 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
 
-#include "iwl-dev.h"
 #include "iwl-io.h"
 #include "iwl-agn-hw.h"
-#include "iwl-agn.h"
-#include "iwl-agn-calib.h"
 #include "iwl-trans.h"
 #include "iwl-fh.h"
 #include "iwl-op-mode.h"
 
+#include "dev.h"
+#include "agn.h"
+#include "calib.h"
+
 /******************************************************************************
  *
  * uCode download functions
@@ -60,8 +61,7 @@ iwl_get_ucode_image(struct iwl_priv *priv, enum iwl_ucode_type ucode_type)
 static int iwl_set_Xtal_calib(struct iwl_priv *priv)
 {
         struct iwl_calib_xtal_freq_cmd cmd;
-        __le16 *xtal_calib =
-                (__le16 *)iwl_eeprom_query_addr(priv, EEPROM_XTAL);
+        __le16 *xtal_calib = priv->eeprom_data->xtal_calib;
 
         iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_CRYSTAL_FRQ_CMD);
         cmd.cap_pin1 = le16_to_cpu(xtal_calib[0]);
@@ -72,12 +72,10 @@ static int iwl_set_Xtal_calib(struct iwl_priv *priv)
 static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 {
         struct iwl_calib_temperature_offset_cmd cmd;
-        __le16 *offset_calib =
-                (__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
 
         memset(&cmd, 0, sizeof(cmd));
         iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
-        memcpy(&cmd.radio_sensor_offset, offset_calib, sizeof(*offset_calib));
+        cmd.radio_sensor_offset = priv->eeprom_data->raw_temperature;
         if (!(cmd.radio_sensor_offset))
                 cmd.radio_sensor_offset = DEFAULT_RADIO_SENSOR_OFFSET;
 
@@ -89,27 +87,17 @@ static int iwl_set_temperature_offset_calib(struct iwl_priv *priv)
 static int iwl_set_temperature_offset_calib_v2(struct iwl_priv *priv)
 {
         struct iwl_calib_temperature_offset_v2_cmd cmd;
-        __le16 *offset_calib_high = (__le16 *)iwl_eeprom_query_addr(priv,
-                                     EEPROM_KELVIN_TEMPERATURE);
-        __le16 *offset_calib_low =
-                (__le16 *)iwl_eeprom_query_addr(priv, EEPROM_RAW_TEMPERATURE);
-        struct iwl_eeprom_calib_hdr *hdr;
 
         memset(&cmd, 0, sizeof(cmd));
         iwl_set_calib_hdr(&cmd.hdr, IWL_PHY_CALIBRATE_TEMP_OFFSET_CMD);
-        hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
-                                                        EEPROM_CALIB_ALL);
-        memcpy(&cmd.radio_sensor_offset_high, offset_calib_high,
-                sizeof(*offset_calib_high));
-        memcpy(&cmd.radio_sensor_offset_low, offset_calib_low,
-                sizeof(*offset_calib_low));
-        if (!(cmd.radio_sensor_offset_low)) {
+        cmd.radio_sensor_offset_high = priv->eeprom_data->kelvin_temperature;
+        cmd.radio_sensor_offset_low = priv->eeprom_data->raw_temperature;
+        if (!cmd.radio_sensor_offset_low) {
                 IWL_DEBUG_CALIB(priv, "no info in EEPROM, use default\n");
                 cmd.radio_sensor_offset_low = DEFAULT_RADIO_SENSOR_OFFSET;
                 cmd.radio_sensor_offset_high = DEFAULT_RADIO_SENSOR_OFFSET;
         }
-        memcpy(&cmd.burntVoltageRef, &hdr->voltage,
-                sizeof(hdr->voltage));
+        cmd.burntVoltageRef = priv->eeprom_data->calib_voltage;
 
         IWL_DEBUG_CALIB(priv, "Radio sensor offset high: %d\n",
                         le16_to_cpu(cmd.radio_sensor_offset_high));
@@ -177,7 +165,7 @@ int iwl_init_alive_start(struct iwl_priv *priv)
         return 0;
 }
 
-int iwl_send_wimax_coex(struct iwl_priv *priv)
+static int iwl_send_wimax_coex(struct iwl_priv *priv)
 {
         struct iwl_wimax_coex_cmd coex_cmd;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index a52818bbfe9..10e47938b63 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -113,7 +113,7 @@ enum iwl_led_mode {
 #define IWL_MAX_PLCP_ERR_THRESHOLD_DISABLE      0
 
 /* TX queue watchdog timeouts in mSecs */
-#define IWL_WATCHHDOG_DISABLED  0
+#define IWL_WATCHDOG_DISABLED   0
 #define IWL_DEF_WD_TIMEOUT      2000
 #define IWL_LONG_WD_TIMEOUT     10000
 #define IWL_MAX_WD_TIMEOUT      120000
@@ -182,13 +182,34 @@ struct iwl_bt_params {
         bool bt_sco_disable;
         bool bt_session_2;
 };
+
 /*
  * @use_rts_for_aggregation: use rts/cts protection for HT traffic
+ * @ht40_bands: bitmap of bands (using %IEEE80211_BAND_*) that support HT40
  */
 struct iwl_ht_params {
+        enum ieee80211_smps_mode smps_mode;
         const bool ht_greenfield_support; /* if used set to true */
         bool use_rts_for_aggregation;
-        enum ieee80211_smps_mode smps_mode;
+        u8 ht40_bands;
+};
+
+/*
+ * information on how to parse the EEPROM
+ */
+#define EEPROM_REG_BAND_1_CHANNELS              0x08
+#define EEPROM_REG_BAND_2_CHANNELS              0x26
+#define EEPROM_REG_BAND_3_CHANNELS              0x42
+#define EEPROM_REG_BAND_4_CHANNELS              0x5C
+#define EEPROM_REG_BAND_5_CHANNELS              0x74
+#define EEPROM_REG_BAND_24_HT40_CHANNELS        0x82
+#define EEPROM_REG_BAND_52_HT40_CHANNELS        0x92
+#define EEPROM_6000_REG_BAND_24_HT40_CHANNELS   0x80
+#define EEPROM_REGULATORY_BAND_NO_HT40          0
+
+struct iwl_eeprom_params {
+        const u8 regulatory_bands[7];
+        bool enhanced_txpower;
 };
 
 /**
@@ -243,6 +264,7 @@ struct iwl_cfg {
         /* params likely to change within a device family */
         const struct iwl_ht_params *ht_params;
         const struct iwl_bt_params *bt_params;
+        const struct iwl_eeprom_params *eeprom_params;
         const bool need_temp_offset_calib; /* if used set to true */
         const bool no_xtal_calib;
         enum iwl_led_mode led_mode;
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 59750543fce..34a5287dfc2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -97,13 +97,10 @@
 /*
  * Hardware revision info
  * Bit fields:
- * 31-8:  Reserved
- *  7-4:  Type of device:  see CSR_HW_REV_TYPE_xxx definitions
+ * 31-16:  Reserved
+ *  15-4:  Type of device:  see CSR_HW_REV_TYPE_xxx definitions
  *  3-2:  Revision step:  0 = A, 1 = B, 2 = C, 3 = D
  *  1-0:  "Dash" (-) value, as in A-1, etc.
- *
- * NOTE:  Revision step affects calculation of CCK txpower for 4965.
- * NOTE:  See also CSR_HW_REV_WA_REG (work-around for bug in 4965).
  */
 #define CSR_HW_REV              (CSR_BASE+0x028)
 
@@ -155,9 +152,21 @@
 #define CSR_DBG_LINK_PWR_MGMT_REG       (CSR_BASE+0x250)
 
 /* Bits for CSR_HW_IF_CONFIG_REG */
-#define CSR_HW_IF_CONFIG_REG_MSK_BOARD_VER      (0x00000C00)
-#define CSR_HW_IF_CONFIG_REG_BIT_MAC_SI         (0x00000100)
+#define CSR_HW_IF_CONFIG_REG_MSK_MAC_DASH       (0x00000003)
+#define CSR_HW_IF_CONFIG_REG_MSK_MAC_STEP       (0x0000000C)
+#define CSR_HW_IF_CONFIG_REG_MSK_BOARD_VER      (0x000000C0)
+#define CSR_HW_IF_CONFIG_REG_BIT_MAC_SI         (0x00000100)
 #define CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI       (0x00000200)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_TYPE       (0x00000C00)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_DASH       (0x00003000)
+#define CSR_HW_IF_CONFIG_REG_MSK_PHY_STEP       (0x0000C000)
+
+#define CSR_HW_IF_CONFIG_REG_POS_MAC_DASH       (0)
+#define CSR_HW_IF_CONFIG_REG_POS_MAC_STEP       (2)
+#define CSR_HW_IF_CONFIG_REG_POS_BOARD_VER      (6)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_TYPE       (10)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_DASH       (12)
+#define CSR_HW_IF_CONFIG_REG_POS_PHY_STEP       (14)
 
 #define CSR_HW_IF_CONFIG_REG_BIT_HAP_WAKE_L1A   (0x00080000)
 #define CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM (0x00200000)
@@ -270,7 +279,10 @@
 
 
 /* HW REV */
-#define CSR_HW_REV_TYPE_MSK            (0x00001F0)
+#define CSR_HW_REV_DASH(_val)          (((_val) & 0x0000003) >> 0)
+#define CSR_HW_REV_STEP(_val)          (((_val) & 0x000000C) >> 2)
+
+#define CSR_HW_REV_TYPE_MSK            (0x000FFF0)
 #define CSR_HW_REV_TYPE_5300           (0x0000020)
 #define CSR_HW_REV_TYPE_5350           (0x0000030)
 #define CSR_HW_REV_TYPE_5100           (0x0000050)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index f6bf91c8f77..42b20b0e83b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -45,6 +45,7 @@ void __iwl_crit(struct device *dev, const char *fmt, ...) __printf(2, 3);
 
 /* No matter what is m (priv, bus, trans), this will work */
 #define IWL_ERR(m, f, a...) __iwl_err((m)->dev, false, false, f, ## a)
+#define IWL_ERR_DEV(d, f, a...) __iwl_err((d), false, false, f, ## a)
 #define IWL_WARN(m, f, a...) __iwl_warn((m)->dev, f, ## a)
 #define IWL_INFO(m, f, a...) __iwl_info((m)->dev, f, ## a)
 #define IWL_CRIT(m, f, a...) __iwl_crit((m)->dev, f, ## a)
@@ -69,6 +70,8 @@ do {									\
 
 #define IWL_DEBUG(m, level, fmt, args...)                               \
         __iwl_dbg((m)->dev, level, false, __func__, fmt, ##args)
+#define IWL_DEBUG_DEV(dev, level, fmt, args...)                         \
+        __iwl_dbg((dev), level, false, __func__, fmt, ##args)
 #define IWL_DEBUG_LIMIT(m, level, fmt, args...)                         \
         __iwl_dbg((m)->dev, level, true, __func__, fmt, ##args)
 
@@ -153,7 +156,7 @@ do {                                            			\
 #define IWL_DEBUG_LED(p, f, a...)       IWL_DEBUG(p, IWL_DL_LED, f, ## a)
 #define IWL_DEBUG_WEP(p, f, a...)       IWL_DEBUG(p, IWL_DL_WEP, f, ## a)
 #define IWL_DEBUG_HC(p, f, a...)        IWL_DEBUG(p, IWL_DL_HCMD, f, ## a)
-#define IWL_DEBUG_EEPROM(p, f, a...)    IWL_DEBUG(p, IWL_DL_EEPROM, f, ## a)
+#define IWL_DEBUG_EEPROM(d, f, a...)    IWL_DEBUG_DEV(d, IWL_DL_EEPROM, f, ## a)
 #define IWL_DEBUG_CALIB(p, f, a...)     IWL_DEBUG(p, IWL_DL_CALIB, f, ## a)
 #define IWL_DEBUG_FW(p, f, a...)        IWL_DEBUG(p, IWL_DL_FW, f, ## a)
 #define IWL_DEBUG_RF_KILL(p, f, a...)   IWL_DEBUG(p, IWL_DL_RF_KILL, f, ## a)
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
index 06203d6a1d8..65364793021 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
@@ -28,6 +28,7 @@
 #define __IWLWIFI_DEVICE_TRACE
 
 #include <linux/tracepoint.h>
+#include <linux/device.h>
 
 
 #if !defined(CONFIG_IWLWIFI_DEVICE_TRACING) || defined(__CHECKER__)
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 49df0e9d5c5..a175997e782 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-eeprom-parse.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
new file mode 100644
index 00000000000..f10170fe879
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
@@ -0,0 +1,903 @@
+/******************************************************************************
+ *
+ * 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) 2008 - 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) 2005 - 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/types.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+#include "iwl-modparams.h"
+#include "iwl-eeprom-parse.h"
+
+/* EEPROM offset definitions */
+
+/* indirect access definitions */
+#define ADDRESS_MSK                 0x0000FFFF
+#define INDIRECT_TYPE_MSK           0x000F0000
+#define INDIRECT_HOST               0x00010000
+#define INDIRECT_GENERAL            0x00020000
+#define INDIRECT_REGULATORY         0x00030000
+#define INDIRECT_CALIBRATION        0x00040000
+#define INDIRECT_PROCESS_ADJST      0x00050000
+#define INDIRECT_OTHERS             0x00060000
+#define INDIRECT_TXP_LIMIT          0x00070000
+#define INDIRECT_TXP_LIMIT_SIZE     0x00080000
+#define INDIRECT_ADDRESS            0x00100000
+
+/* corresponding link offsets in EEPROM */
+#define EEPROM_LINK_HOST             (2*0x64)
+#define EEPROM_LINK_GENERAL          (2*0x65)
+#define EEPROM_LINK_REGULATORY       (2*0x66)
+#define EEPROM_LINK_CALIBRATION      (2*0x67)
+#define EEPROM_LINK_PROCESS_ADJST    (2*0x68)
+#define EEPROM_LINK_OTHERS           (2*0x69)
+#define EEPROM_LINK_TXP_LIMIT        (2*0x6a)
+#define EEPROM_LINK_TXP_LIMIT_SIZE   (2*0x6b)
+
+/* General */
+#define EEPROM_DEVICE_ID                    (2*0x08)    /* 2 bytes */
+#define EEPROM_SUBSYSTEM_ID                 (2*0x0A)    /* 2 bytes */
+#define EEPROM_MAC_ADDRESS                  (2*0x15)    /* 6  bytes */
+#define EEPROM_BOARD_REVISION               (2*0x35)    /* 2  bytes */
+#define EEPROM_BOARD_PBA_NUMBER             (2*0x3B+1)  /* 9  bytes */
+#define EEPROM_VERSION                      (2*0x44)    /* 2  bytes */
+#define EEPROM_SKU_CAP                      (2*0x45)    /* 2  bytes */
+#define EEPROM_OEM_MODE                     (2*0x46)    /* 2  bytes */
+#define EEPROM_RADIO_CONFIG                 (2*0x48)    /* 2  bytes */
+#define EEPROM_NUM_MAC_ADDRESS              (2*0x4C)    /* 2  bytes */
+
+/* calibration */
+struct iwl_eeprom_calib_hdr {
+        u8 version;
+        u8 pa_type;
+        __le16 voltage;
+} __packed;
+
+#define EEPROM_CALIB_ALL        (INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
+#define EEPROM_XTAL             ((2*0x128) | EEPROM_CALIB_ALL)
+
+/* temperature */
+#define EEPROM_KELVIN_TEMPERATURE       ((2*0x12A) | EEPROM_CALIB_ALL)
+#define EEPROM_RAW_TEMPERATURE          ((2*0x12B) | EEPROM_CALIB_ALL)
+
+/*
+ * EEPROM bands
+ * These are the channel numbers from each band in the order
+ * that they are stored in the EEPROM band information. Note
+ * that EEPROM bands aren't the same as mac80211 bands, and
+ * there are even special "ht40 bands" in the EEPROM.
+ */
+static const u8 iwl_eeprom_band_1[14] = { /* 2.4 GHz */
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+};
+
+static const u8 iwl_eeprom_band_2[] = { /* 4915-5080MHz */
+        183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
+};
+
+static const u8 iwl_eeprom_band_3[] = { /* 5170-5320MHz */
+        34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+};
+
+static const u8 iwl_eeprom_band_4[] = { /* 5500-5700MHz */
+        100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+};
+
+static const u8 iwl_eeprom_band_5[] = { /* 5725-5825MHz */
+        145, 149, 153, 157, 161, 165
+};
+
+static const u8 iwl_eeprom_band_6[] = { /* 2.4 ht40 channel */
+        1, 2, 3, 4, 5, 6, 7
+};
+
+static const u8 iwl_eeprom_band_7[] = { /* 5.2 ht40 channel */
+        36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
+};
+
+#define IWL_NUM_CHANNELS        (ARRAY_SIZE(iwl_eeprom_band_1) + \
+                                 ARRAY_SIZE(iwl_eeprom_band_2) + \
+                                 ARRAY_SIZE(iwl_eeprom_band_3) + \
+                                 ARRAY_SIZE(iwl_eeprom_band_4) + \
+                                 ARRAY_SIZE(iwl_eeprom_band_5))
+
+/* rate data (static) */
+static struct ieee80211_rate iwl_cfg80211_rates[] = {
+        { .bitrate = 1 * 10, .hw_value = 0, .hw_value_short = 0, },
+        { .bitrate = 2 * 10, .hw_value = 1, .hw_value_short = 1,
+          .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+        { .bitrate = 5.5 * 10, .hw_value = 2, .hw_value_short = 2,
+          .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+        { .bitrate = 11 * 10, .hw_value = 3, .hw_value_short = 3,
+          .flags = IEEE80211_RATE_SHORT_PREAMBLE, },
+        { .bitrate = 6 * 10, .hw_value = 4, .hw_value_short = 4, },
+        { .bitrate = 9 * 10, .hw_value = 5, .hw_value_short = 5, },
+        { .bitrate = 12 * 10, .hw_value = 6, .hw_value_short = 6, },
+        { .bitrate = 18 * 10, .hw_value = 7, .hw_value_short = 7, },
+        { .bitrate = 24 * 10, .hw_value = 8, .hw_value_short = 8, },
+        { .bitrate = 36 * 10, .hw_value = 9, .hw_value_short = 9, },
+        { .bitrate = 48 * 10, .hw_value = 10, .hw_value_short = 10, },
+        { .bitrate = 54 * 10, .hw_value = 11, .hw_value_short = 11, },
+};
+#define RATES_24_OFFS   0
+#define N_RATES_24      ARRAY_SIZE(iwl_cfg80211_rates)
+#define RATES_52_OFFS   4
+#define N_RATES_52      (N_RATES_24 - RATES_52_OFFS)
+
+/* EEPROM reading functions */
+
+static u16 iwl_eeprom_query16(const u8 *eeprom, size_t eeprom_size, int offset)
+{
+        if (WARN_ON(offset + sizeof(u16) > eeprom_size))
+                return 0;
+        return le16_to_cpup((__le16 *)(eeprom + offset));
+}
+
+static u32 eeprom_indirect_address(const u8 *eeprom, size_t eeprom_size,
+                                   u32 address)
+{
+        u16 offset = 0;
+
+        if ((address & INDIRECT_ADDRESS) == 0)
+                return address;
+
+        switch (address & INDIRECT_TYPE_MSK) {
+        case INDIRECT_HOST:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_HOST);
+                break;
+        case INDIRECT_GENERAL:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_GENERAL);
+                break;
+        case INDIRECT_REGULATORY:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_REGULATORY);
+                break;
+        case INDIRECT_TXP_LIMIT:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_TXP_LIMIT);
+                break;
+        case INDIRECT_TXP_LIMIT_SIZE:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_TXP_LIMIT_SIZE);
+                break;
+        case INDIRECT_CALIBRATION:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_CALIBRATION);
+                break;
+        case INDIRECT_PROCESS_ADJST:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_PROCESS_ADJST);
+                break;
+        case INDIRECT_OTHERS:
+                offset = iwl_eeprom_query16(eeprom, eeprom_size,
+                                            EEPROM_LINK_OTHERS);
+                break;
+        default:
+                WARN_ON(1);
+                break;
+        }
+
+        /* translate the offset from words to byte */
+        return (address & ADDRESS_MSK) + (offset << 1);
+}
+
+static const u8 *iwl_eeprom_query_addr(const u8 *eeprom, size_t eeprom_size,
+                                       u32 offset)
+{
+        u32 address = eeprom_indirect_address(eeprom, eeprom_size, offset);
+
+        if (WARN_ON(address >= eeprom_size))
+                return NULL;
+
+        return &eeprom[address];
+}
+
+static int iwl_eeprom_read_calib(const u8 *eeprom, size_t eeprom_size,
+                                 struct iwl_eeprom_data *data)
+{
+        struct iwl_eeprom_calib_hdr *hdr;
+
+        hdr = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+                                            EEPROM_CALIB_ALL);
+        if (!hdr)
+                return -ENODATA;
+        data->calib_version = hdr->version;
+        data->calib_voltage = hdr->voltage;
+
+        return 0;
+}
+
+/**
+ * enum iwl_eeprom_channel_flags - channel flags in EEPROM
+ * @EEPROM_CHANNEL_VALID: channel is usable for this SKU/geo
+ * @EEPROM_CHANNEL_IBSS: usable as an IBSS channel
+ * @EEPROM_CHANNEL_ACTIVE: active scanning allowed
+ * @EEPROM_CHANNEL_RADAR: radar detection required
+ * @EEPROM_CHANNEL_WIDE: 20 MHz channel okay (?)
+ * @EEPROM_CHANNEL_DFS: dynamic freq selection candidate
+ */
+enum iwl_eeprom_channel_flags {
+        EEPROM_CHANNEL_VALID = BIT(0),
+        EEPROM_CHANNEL_IBSS = BIT(1),
+        EEPROM_CHANNEL_ACTIVE = BIT(3),
+        EEPROM_CHANNEL_RADAR = BIT(4),
+        EEPROM_CHANNEL_WIDE = BIT(5),
+        EEPROM_CHANNEL_DFS = BIT(7),
+};
+
+/**
+ * struct iwl_eeprom_channel - EEPROM channel data
+ * @flags: %EEPROM_CHANNEL_* flags
+ * @max_power_avg: max power (in dBm) on this channel, at most 31 dBm
+ */
+struct iwl_eeprom_channel {
+        u8 flags;
+        s8 max_power_avg;
+} __packed;
+
+
+enum iwl_eeprom_enhanced_txpwr_flags {
+        IWL_EEPROM_ENH_TXP_FL_VALID = BIT(0),
+        IWL_EEPROM_ENH_TXP_FL_BAND_52G = BIT(1),
+        IWL_EEPROM_ENH_TXP_FL_OFDM = BIT(2),
+        IWL_EEPROM_ENH_TXP_FL_40MHZ = BIT(3),
+        IWL_EEPROM_ENH_TXP_FL_HT_AP = BIT(4),
+        IWL_EEPROM_ENH_TXP_FL_RES1 = BIT(5),
+        IWL_EEPROM_ENH_TXP_FL_RES2 = BIT(6),
+        IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE = BIT(7),
+};
+
+/**
+ * iwl_eeprom_enhanced_txpwr structure
+ * @flags: entry flags
+ * @channel: channel number
+ * @chain_a_max_pwr: chain a max power in 1/2 dBm
+ * @chain_b_max_pwr: chain b max power in 1/2 dBm
+ * @chain_c_max_pwr: chain c max power in 1/2 dBm
+ * @delta_20_in_40: 20-in-40 deltas (hi/lo)
+ * @mimo2_max_pwr: mimo2 max power in 1/2 dBm
+ * @mimo3_max_pwr: mimo3 max power in 1/2 dBm
+ *
+ * This structure presents the enhanced regulatory tx power limit layout
+ * in an EEPROM image.
+ */
+struct iwl_eeprom_enhanced_txpwr {
+        u8 flags;
+        u8 channel;
+        s8 chain_a_max;
+        s8 chain_b_max;
+        s8 chain_c_max;
+        u8 delta_20_in_40;
+        s8 mimo2_max;
+        s8 mimo3_max;
+} __packed;
+
+static s8 iwl_get_max_txpwr_half_dbm(const struct iwl_eeprom_data *data,
+                                     struct iwl_eeprom_enhanced_txpwr *txp)
+{
+        s8 result = 0; /* (.5 dBm) */
+
+        /* Take the highest tx power from any valid chains */
+        if (data->valid_tx_ant & ANT_A && txp->chain_a_max > result)
+                result = txp->chain_a_max;
+
+        if (data->valid_tx_ant & ANT_B && txp->chain_b_max > result)
+                result = txp->chain_b_max;
+
+        if (data->valid_tx_ant & ANT_C && txp->chain_c_max > result)
+                result = txp->chain_c_max;
+
+        if ((data->valid_tx_ant == ANT_AB ||
+             data->valid_tx_ant == ANT_BC ||
+             data->valid_tx_ant == ANT_AC) && txp->mimo2_max > result)
+                result = txp->mimo2_max;
+
+        if (data->valid_tx_ant == ANT_ABC && txp->mimo3_max > result)
+                result = txp->mimo3_max;
+
+        return result;
+}
+
+#define EEPROM_TXP_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT)
+#define EEPROM_TXP_ENTRY_LEN sizeof(struct iwl_eeprom_enhanced_txpwr)
+#define EEPROM_TXP_SZ_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT_SIZE)
+
+#define TXP_CHECK_AND_PRINT(x) \
+        ((txp->flags & IWL_EEPROM_ENH_TXP_FL_##x) ? # x " " : "")
+
+static void
+iwl_eeprom_enh_txp_read_element(struct iwl_eeprom_data *data,
+                                struct iwl_eeprom_enhanced_txpwr *txp,
+                                int n_channels, s8 max_txpower_avg)
+{
+        int ch_idx;
+        enum ieee80211_band band;
+
+        band = txp->flags & IWL_EEPROM_ENH_TXP_FL_BAND_52G ?
+                IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
+
+        for (ch_idx = 0; ch_idx < n_channels; ch_idx++) {
+                struct ieee80211_channel *chan = &data->channels[ch_idx];
+
+                /* update matching channel or from common data only */
+                if (txp->channel != 0 && chan->hw_value != txp->channel)
+                        continue;
+
+                /* update matching band only */
+                if (band != chan->band)
+                        continue;
+
+                if (chan->max_power < max_txpower_avg &&
+                    !(txp->flags & IWL_EEPROM_ENH_TXP_FL_40MHZ))
+                        chan->max_power = max_txpower_avg;
+        }
+}
+
+static void iwl_eeprom_enhanced_txpower(struct device *dev,
+                                        struct iwl_eeprom_data *data,
+                                        const u8 *eeprom, size_t eeprom_size,
+                                        int n_channels)
+{
+        struct iwl_eeprom_enhanced_txpwr *txp_array, *txp;
+        int idx, entries;
+        __le16 *txp_len;
+        s8 max_txp_avg_halfdbm;
+
+        BUILD_BUG_ON(sizeof(struct iwl_eeprom_enhanced_txpwr) != 8);
+
+        /* the length is in 16-bit words, but we want entries */
+        txp_len = (__le16 *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+                                                  EEPROM_TXP_SZ_OFFS);
+        entries = le16_to_cpup(txp_len) * 2 / EEPROM_TXP_ENTRY_LEN;
+
+        txp_array = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size,
+                                                  EEPROM_TXP_OFFS);
+
+        for (idx = 0; idx < entries; idx++) {
+                txp = &txp_array[idx];
+                /* skip invalid entries */
+                if (!(txp->flags & IWL_EEPROM_ENH_TXP_FL_VALID))
+                        continue;
+
+                IWL_DEBUG_EEPROM(dev, "%s %d:\t %s%s%s%s%s%s%s%s (0x%02x)\n",
+                                 (txp->channel && (txp->flags &
+                                        IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE)) ?
+                                        "Common " : (txp->channel) ?
+                                        "Channel" : "Common",
+                                 (txp->channel),
+                                 TXP_CHECK_AND_PRINT(VALID),
+                                 TXP_CHECK_AND_PRINT(BAND_52G),
+                                 TXP_CHECK_AND_PRINT(OFDM),
+                                 TXP_CHECK_AND_PRINT(40MHZ),
+                                 TXP_CHECK_AND_PRINT(HT_AP),
+                                 TXP_CHECK_AND_PRINT(RES1),
+                                 TXP_CHECK_AND_PRINT(RES2),
+                                 TXP_CHECK_AND_PRINT(COMMON_TYPE),
+                                 txp->flags);
+                IWL_DEBUG_EEPROM(dev,
+                                 "\t\t chain_A: 0x%02x chain_B: 0X%02x chain_C: 0X%02x\n",
+                                 txp->chain_a_max, txp->chain_b_max,
+                                 txp->chain_c_max);
+                IWL_DEBUG_EEPROM(dev,
+                                 "\t\t MIMO2: 0x%02x MIMO3: 0x%02x High 20_on_40: 0x%02x Low 20_on_40: 0x%02x\n",
+                                 txp->mimo2_max, txp->mimo3_max,
+                                 ((txp->delta_20_in_40 & 0xf0) >> 4),
+                                 (txp->delta_20_in_40 & 0x0f));
+
+                max_txp_avg_halfdbm = iwl_get_max_txpwr_half_dbm(data, txp);
+
+                iwl_eeprom_enh_txp_read_element(data, txp, n_channels,
+                                DIV_ROUND_UP(max_txp_avg_halfdbm, 2));
+
+                if (max_txp_avg_halfdbm > data->max_tx_pwr_half_dbm)
+                        data->max_tx_pwr_half_dbm = max_txp_avg_halfdbm;
+        }
+}
+
+static void iwl_init_band_reference(const struct iwl_cfg *cfg,
+                                    const u8 *eeprom, size_t eeprom_size,
+                                    int eeprom_band, int *eeprom_ch_count,
+                                    const struct iwl_eeprom_channel **ch_info,
+                                    const u8 **eeprom_ch_array)
+{
+        u32 offset = cfg->eeprom_params->regulatory_bands[eeprom_band - 1];
+
+        offset |= INDIRECT_ADDRESS | INDIRECT_REGULATORY;
+
+        *ch_info = (void *)iwl_eeprom_query_addr(eeprom, eeprom_size, offset);
+
+        switch (eeprom_band) {
+        case 1:         /* 2.4GHz band */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
+                *eeprom_ch_array = iwl_eeprom_band_1;
+                break;
+        case 2:         /* 4.9GHz band */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
+                *eeprom_ch_array = iwl_eeprom_band_2;
+                break;
+        case 3:         /* 5.2GHz band */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
+                *eeprom_ch_array = iwl_eeprom_band_3;
+                break;
+        case 4:         /* 5.5GHz band */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
+                *eeprom_ch_array = iwl_eeprom_band_4;
+                break;
+        case 5:         /* 5.7GHz band */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
+                *eeprom_ch_array = iwl_eeprom_band_5;
+                break;
+        case 6:         /* 2.4GHz ht40 channels */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
+                *eeprom_ch_array = iwl_eeprom_band_6;
+                break;
+        case 7:         /* 5 GHz ht40 channels */
+                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
+                *eeprom_ch_array = iwl_eeprom_band_7;
+                break;
+        default:
+                *eeprom_ch_count = 0;
+                *eeprom_ch_array = NULL;
+                WARN_ON(1);
+        }
+}
+
+#define CHECK_AND_PRINT(x) \
+        ((eeprom_ch->flags & EEPROM_CHANNEL_##x) ? # x " " : "")
+
+static void iwl_mod_ht40_chan_info(struct device *dev,
+                                   struct iwl_eeprom_data *data, int n_channels,
+                                   enum ieee80211_band band, u16 channel,
+                                   const struct iwl_eeprom_channel *eeprom_ch,
+                                   u8 clear_ht40_extension_channel)
+{
+        struct ieee80211_channel *chan = NULL;
+        int i;
+
+        for (i = 0; i < n_channels; i++) {
+                if (data->channels[i].band != band)
+                        continue;
+                if (data->channels[i].hw_value != channel)
+                        continue;
+                chan = &data->channels[i];
+                break;
+        }
+
+        if (!chan)
+                return;
+
+        IWL_DEBUG_EEPROM(dev,
+                         "HT40 Ch. %d [%sGHz] %s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
+                         channel,
+                         band == IEEE80211_BAND_5GHZ ? "5.2" : "2.4",
+                         CHECK_AND_PRINT(IBSS),
+                         CHECK_AND_PRINT(ACTIVE),
+                         CHECK_AND_PRINT(RADAR),
+                         CHECK_AND_PRINT(WIDE),
+                         CHECK_AND_PRINT(DFS),
+                         eeprom_ch->flags,
+                         eeprom_ch->max_power_avg,
+                         ((eeprom_ch->flags & EEPROM_CHANNEL_IBSS) &&
+                          !(eeprom_ch->flags & EEPROM_CHANNEL_RADAR)) ? ""
+                                                                      : "not ");
+
+        if (eeprom_ch->flags & EEPROM_CHANNEL_VALID)
+                chan->flags &= ~clear_ht40_extension_channel;
+}
+
+#define CHECK_AND_PRINT_I(x)    \
+        ((eeprom_ch_info[ch_idx].flags & EEPROM_CHANNEL_##x) ? # x " " : "")
+
+static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
+                                struct iwl_eeprom_data *data,
+                                const u8 *eeprom, size_t eeprom_size)
+{
+        int band, ch_idx;
+        const struct iwl_eeprom_channel *eeprom_ch_info;
+        const u8 *eeprom_ch_array;
+        int eeprom_ch_count;
+        int n_channels = 0;
+
+        /*
+         * Loop through the 5 EEPROM bands and add them to the parse list
+         */
+        for (band = 1; band <= 5; band++) {
+                struct ieee80211_channel *channel;
+
+                iwl_init_band_reference(cfg, eeprom, eeprom_size, band,
+                                        &eeprom_ch_count, &eeprom_ch_info,
+                                        &eeprom_ch_array);
+
+                /* Loop through each band adding each of the channels */
+                for (ch_idx = 0; ch_idx < eeprom_ch_count; ch_idx++) {
+                        const struct iwl_eeprom_channel *eeprom_ch;
+
+                        eeprom_ch = &eeprom_ch_info[ch_idx];
+
+                        if (!(eeprom_ch->flags & EEPROM_CHANNEL_VALID)) {
+                                IWL_DEBUG_EEPROM(dev,
+                                                 "Ch. %d Flags %x [%sGHz] - No traffic\n",
+                                                 eeprom_ch_array[ch_idx],
+                                                 eeprom_ch_info[ch_idx].flags,
+                                                 (band != 1) ? "5.2" : "2.4");
+                                continue;
+                        }
+
+                        channel = &data->channels[n_channels];
+                        n_channels++;
+
+                        channel->hw_value = eeprom_ch_array[ch_idx];
+                        channel->band = (band == 1) ? IEEE80211_BAND_2GHZ
+                                                    : IEEE80211_BAND_5GHZ;
+                        channel->center_freq =
+                                ieee80211_channel_to_frequency(
+                                        channel->hw_value, channel->band);
+
+                        /* set no-HT40, will enable as appropriate later */
+                        channel->flags = IEEE80211_CHAN_NO_HT40;
+
+                        if (!(eeprom_ch->flags & EEPROM_CHANNEL_IBSS))
+                                channel->flags |= IEEE80211_CHAN_NO_IBSS;
+
+                        if (!(eeprom_ch->flags & EEPROM_CHANNEL_ACTIVE))
+                                channel->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
+
+                        if (eeprom_ch->flags & EEPROM_CHANNEL_RADAR)
+                                channel->flags |= IEEE80211_CHAN_RADAR;
+
+                        /* Initialize regulatory-based run-time data */
+                        channel->max_power =
+                                eeprom_ch_info[ch_idx].max_power_avg;
+                        IWL_DEBUG_EEPROM(dev,
+                                         "Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
+                                         channel->hw_value,
+                                         (band != 1) ? "5.2" : "2.4",
+                                         CHECK_AND_PRINT_I(VALID),
+                                         CHECK_AND_PRINT_I(IBSS),
+                                         CHECK_AND_PRINT_I(ACTIVE),
+                                         CHECK_AND_PRINT_I(RADAR),
+                                         CHECK_AND_PRINT_I(WIDE),
+                                         CHECK_AND_PRINT_I(DFS),
+                                         eeprom_ch_info[ch_idx].flags,
+                                         eeprom_ch_info[ch_idx].max_power_avg,
+                                         ((eeprom_ch_info[ch_idx].flags &
+                                                        EEPROM_CHANNEL_IBSS) &&
+                                          !(eeprom_ch_info[ch_idx].flags &
+                                                        EEPROM_CHANNEL_RADAR))
+                                                ? "" : "not ");
+                }
+        }
+
+        if (cfg->eeprom_params->enhanced_txpower) {
+                /*
+                 * for newer device (6000 series and up)
+                 * EEPROM contain enhanced tx power information
+                 * driver need to process addition information
+                 * to determine the max channel tx power limits
+                 */
+                iwl_eeprom_enhanced_txpower(dev, data, eeprom, eeprom_size,
+                                            n_channels);
+        } else {
+                /* All others use data from channel map */
+                int i;
+
+                data->max_tx_pwr_half_dbm = -128;
+
+                for (i = 0; i < n_channels; i++)
+                        data->max_tx_pwr_half_dbm =
+                                max_t(s8, data->max_tx_pwr_half_dbm,
+                                      data->channels[i].max_power * 2);
+        }
+
+        /* Check if we do have HT40 channels */
+        if (cfg->eeprom_params->regulatory_bands[5] ==
+                                EEPROM_REGULATORY_BAND_NO_HT40 &&
+            cfg->eeprom_params->regulatory_bands[6] ==
+                                EEPROM_REGULATORY_BAND_NO_HT40)
+                return n_channels;
+
+        /* Two additional EEPROM bands for 2.4 and 5 GHz HT40 channels */
+        for (band = 6; band <= 7; band++) {
+                enum ieee80211_band ieeeband;
+
+                iwl_init_band_reference(cfg, eeprom, eeprom_size, band,
+                                        &eeprom_ch_count, &eeprom_ch_info,
+                                        &eeprom_ch_array);
+
+                /* EEPROM band 6 is 2.4, band 7 is 5 GHz */
+                ieeeband = (band == 6) ? IEEE80211_BAND_2GHZ
+                                       : IEEE80211_BAND_5GHZ;
+
+                /* Loop through each band adding each of the channels */
+                for (ch_idx = 0; ch_idx < eeprom_ch_count; ch_idx++) {
+                        /* Set up driver's info for lower half */
+                        iwl_mod_ht40_chan_info(dev, data, n_channels, ieeeband,
+                                               eeprom_ch_array[ch_idx],
+                                               &eeprom_ch_info[ch_idx],
+                                               IEEE80211_CHAN_NO_HT40PLUS);
+
+                        /* Set up driver's info for upper half */
+                        iwl_mod_ht40_chan_info(dev, data, n_channels, ieeeband,
+                                               eeprom_ch_array[ch_idx] + 4,
+                                               &eeprom_ch_info[ch_idx],
+                                               IEEE80211_CHAN_NO_HT40MINUS);
+                }
+        }
+
+        return n_channels;
+}
+
+static int iwl_init_sband_channels(struct iwl_eeprom_data *data,
+                                   struct ieee80211_supported_band *sband,
+                                   int n_channels, enum ieee80211_band band)
+{
+        struct ieee80211_channel *chan = &data->channels[0];
+        int n = 0, idx = 0;
+
+        while (chan->band != band && idx < n_channels)
+                chan = &data->channels[++idx];
+
+        sband->channels = &data->channels[idx];
+
+        while (chan->band == band && idx < n_channels) {
+                chan = &data->channels[++idx];
+                n++;
+        }
+
+        sband->n_channels = n;
+
+        return n;
+}
+
+#define MAX_BIT_RATE_40_MHZ     150 /* Mbps */
+#define MAX_BIT_RATE_20_MHZ     72 /* Mbps */
+
+static void iwl_init_ht_hw_capab(const struct iwl_cfg *cfg,
+                                 struct iwl_eeprom_data *data,
+                                 struct ieee80211_sta_ht_cap *ht_info,
+                                 enum ieee80211_band band)
+{
+        int max_bit_rate = 0;
+        u8 rx_chains;
+        u8 tx_chains;
+
+        tx_chains = hweight8(data->valid_tx_ant);
+        if (cfg->rx_with_siso_diversity)
+                rx_chains = 1;
+        else
+                rx_chains = hweight8(data->valid_rx_ant);
+
+        if (!(data->sku & EEPROM_SKU_CAP_11N_ENABLE) || !cfg->ht_params) {
+                ht_info->ht_supported = false;
+                return;
+        }
+
+        ht_info->ht_supported = true;
+        ht_info->cap = 0;
+
+        if (iwlwifi_mod_params.amsdu_size_8K)
+                ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
+
+        ht_info->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
+        ht_info->ampdu_density = IEEE80211_HT_MPDU_DENSITY_4;
+
+        ht_info->mcs.rx_mask[0] = 0xFF;
+        if (rx_chains >= 2)
+                ht_info->mcs.rx_mask[1] = 0xFF;
+        if (rx_chains >= 3)
+                ht_info->mcs.rx_mask[2] = 0xFF;
+
+        if (cfg->ht_params->ht_greenfield_support)
+                ht_info->cap |= IEEE80211_HT_CAP_GRN_FLD;
+        ht_info->cap |= IEEE80211_HT_CAP_SGI_20;
+
+        max_bit_rate = MAX_BIT_RATE_20_MHZ;
+
+        if (cfg->ht_params->ht40_bands & BIT(band)) {
+                ht_info->cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+                ht_info->cap |= IEEE80211_HT_CAP_SGI_40;
+                ht_info->mcs.rx_mask[4] = 0x01;
+                max_bit_rate = MAX_BIT_RATE_40_MHZ;
+        }
+
+        /* Highest supported Rx data rate */
+        max_bit_rate *= rx_chains;
+        WARN_ON(max_bit_rate & ~IEEE80211_HT_MCS_RX_HIGHEST_MASK);
+        ht_info->mcs.rx_highest = cpu_to_le16(max_bit_rate);
+
+        /* Tx MCS capabilities */
+        ht_info->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
+        if (tx_chains != rx_chains) {
+                ht_info->mcs.tx_params |= IEEE80211_HT_MCS_TX_RX_DIFF;
+                ht_info->mcs.tx_params |= ((tx_chains - 1) <<
+                                IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+        }
+}
+
+static void iwl_init_sbands(struct device *dev, const struct iwl_cfg *cfg,
+                            struct iwl_eeprom_data *data,
+                            const u8 *eeprom, size_t eeprom_size)
+{
+        int n_channels = iwl_init_channel_map(dev, cfg, data,
+                                              eeprom, eeprom_size);
+        int n_used = 0;
+        struct ieee80211_supported_band *sband;
+
+        sband = &data->bands[IEEE80211_BAND_2GHZ];
+        sband->band = IEEE80211_BAND_2GHZ;
+        sband->bitrates = &iwl_cfg80211_rates[RATES_24_OFFS];
+        sband->n_bitrates = N_RATES_24;
+        n_used += iwl_init_sband_channels(data, sband, n_channels,
+                                          IEEE80211_BAND_2GHZ);
+        iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_2GHZ);
+
+        sband = &data->bands[IEEE80211_BAND_5GHZ];
+        sband->band = IEEE80211_BAND_5GHZ;
+        sband->bitrates = &iwl_cfg80211_rates[RATES_52_OFFS];
+        sband->n_bitrates = N_RATES_52;
+        n_used += iwl_init_sband_channels(data, sband, n_channels,
+                                          IEEE80211_BAND_5GHZ);
+        iwl_init_ht_hw_capab(cfg, data, &sband->ht_cap, IEEE80211_BAND_5GHZ);
+
+        if (n_channels != n_used)
+                IWL_ERR_DEV(dev, "EEPROM: used only %d of %d channels\n",
+                            n_used, n_channels);
+}
+
+/* EEPROM data functions */
+
+struct iwl_eeprom_data *
+iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
+                      const u8 *eeprom, size_t eeprom_size)
+{
+        struct iwl_eeprom_data *data;
+        const void *tmp;
+
+        if (WARN_ON(!cfg || !cfg->eeprom_params))
+                return NULL;
+
+        data = kzalloc(sizeof(*data) +
+                       sizeof(struct ieee80211_channel) * IWL_NUM_CHANNELS,
+                       GFP_KERNEL);
+        if (!data)
+                return NULL;
+
+        /* get MAC address(es) */
+        tmp = iwl_eeprom_query_addr(eeprom, eeprom_size, EEPROM_MAC_ADDRESS);
+        if (!tmp)
+                goto err_free;
+        memcpy(data->hw_addr, tmp, ETH_ALEN);
+        data->n_hw_addrs = iwl_eeprom_query16(eeprom, eeprom_size,
+                                              EEPROM_NUM_MAC_ADDRESS);
+
+        if (iwl_eeprom_read_calib(eeprom, eeprom_size, data))
+                goto err_free;
+
+        tmp = iwl_eeprom_query_addr(eeprom, eeprom_size, EEPROM_XTAL);
+        if (!tmp)
+                goto err_free;
+        memcpy(data->xtal_calib, tmp, sizeof(data->xtal_calib));
+
+        tmp = iwl_eeprom_query_addr(eeprom, eeprom_size,
+                                    EEPROM_RAW_TEMPERATURE);
+        if (!tmp)
+                goto err_free;
+        data->raw_temperature = *(__le16 *)tmp;
+
+        tmp = iwl_eeprom_query_addr(eeprom, eeprom_size,
+                                    EEPROM_KELVIN_TEMPERATURE);
+        if (!tmp)
+                goto err_free;
+        data->kelvin_temperature = *(__le16 *)tmp;
+        data->kelvin_voltage = *((__le16 *)tmp + 1);
+
+        data->radio_cfg = iwl_eeprom_query16(eeprom, eeprom_size,
+                                             EEPROM_RADIO_CONFIG);
+        data->sku = iwl_eeprom_query16(eeprom, eeprom_size,
+                                       EEPROM_SKU_CAP);
+        if (iwlwifi_mod_params.disable_11n & IWL_DISABLE_HT_ALL)
+                data->sku &= ~EEPROM_SKU_CAP_11N_ENABLE;
+
+        data->eeprom_version = iwl_eeprom_query16(eeprom, eeprom_size,
+                                                  EEPROM_VERSION);
+
+        data->valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(data->radio_cfg);
+        data->valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(data->radio_cfg);
+
+        /* check overrides (some devices have wrong EEPROM) */
+        if (cfg->valid_tx_ant)
+                data->valid_tx_ant = cfg->valid_tx_ant;
+        if (cfg->valid_rx_ant)
+                data->valid_rx_ant = cfg->valid_rx_ant;
+
+        if (!data->valid_tx_ant || !data->valid_rx_ant) {
+                IWL_ERR_DEV(dev, "invalid antennas (0x%x, 0x%x)\n",
+                            data->valid_tx_ant, data->valid_rx_ant);
+                goto err_free;
+        }
+
+        iwl_init_sbands(dev, cfg, data, eeprom, eeprom_size);
+
+        return data;
+ err_free:
+        kfree(data);
+        return NULL;
+}
+EXPORT_SYMBOL_GPL(iwl_parse_eeprom_data);
+
+/* helper functions */
+int iwl_eeprom_check_version(struct iwl_eeprom_data *data,
+                             struct iwl_trans *trans)
+{
+        if (data->eeprom_version >= trans->cfg->eeprom_ver ||
+            data->calib_version >= trans->cfg->eeprom_calib_ver) {
+                IWL_INFO(trans, "device EEPROM VER=0x%x, CALIB=0x%x\n",
+                         data->eeprom_version, data->calib_version);
+                return 0;
+        }
+
+        IWL_ERR(trans,
+                "Unsupported (too old) EEPROM VER=0x%x < 0x%x CALIB=0x%x < 0x%x\n",
+                data->eeprom_version, trans->cfg->eeprom_ver,
+                data->calib_version,  trans->cfg->eeprom_calib_ver);
+        return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(iwl_eeprom_check_version);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
new file mode 100644
index 00000000000..9c07c670a1c
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
@@ -0,0 +1,138 @@
+/******************************************************************************
+ *
+ * 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) 2008 - 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) 2005 - 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_eeprom_parse_h__
+#define __iwl_eeprom_parse_h__
+
+#include <linux/types.h>
+#include <linux/if_ether.h>
+#include "iwl-trans.h"
+
+/* SKU Capabilities (actual values from EEPROM definition) */
+#define EEPROM_SKU_CAP_BAND_24GHZ       (1 << 4)
+#define EEPROM_SKU_CAP_BAND_52GHZ       (1 << 5)
+#define EEPROM_SKU_CAP_11N_ENABLE       (1 << 6)
+#define EEPROM_SKU_CAP_AMT_ENABLE       (1 << 7)
+#define EEPROM_SKU_CAP_IPAN_ENABLE      (1 << 8)
+
+/* radio config bits (actual values from EEPROM definition) */
+#define EEPROM_RF_CFG_TYPE_MSK(x)   (x & 0x3)         /* bits 0-1   */
+#define EEPROM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
+#define EEPROM_RF_CFG_DASH_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
+#define EEPROM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
+#define EEPROM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
+#define EEPROM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
+
+struct iwl_eeprom_data {
+        int n_hw_addrs;
+        u8 hw_addr[ETH_ALEN];
+
+        u16 radio_config;
+
+        u8 calib_version;
+        __le16 calib_voltage;
+
+        __le16 raw_temperature;
+        __le16 kelvin_temperature;
+        __le16 kelvin_voltage;
+        __le16 xtal_calib[2];
+
+        u16 sku;
+        u16 radio_cfg;
+        u16 eeprom_version;
+        s8 max_tx_pwr_half_dbm;
+
+        u8 valid_tx_ant, valid_rx_ant;
+
+        struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
+        struct ieee80211_channel channels[];
+};
+
+/**
+ * iwl_parse_eeprom_data - parse EEPROM data and return values
+ *
+ * @dev: device pointer we're parsing for, for debug only
+ * @cfg: device configuration for parsing and overrides
+ * @eeprom: the EEPROM data
+ * @eeprom_size: length of the EEPROM data
+ *
+ * This function parses all EEPROM values we need and then
+ * returns a (newly allocated) struct containing all the
+ * relevant values for driver use. The struct must be freed
+ * later with iwl_free_eeprom_data().
+ */
+struct iwl_eeprom_data *
+iwl_parse_eeprom_data(struct device *dev, const struct iwl_cfg *cfg,
+                      const u8 *eeprom, size_t eeprom_size);
+
+/**
+ * iwl_free_eeprom_data - free EEPROM data
+ * @data: the data to free
+ */
+static inline void iwl_free_eeprom_data(struct iwl_eeprom_data *data)
+{
+        kfree(data);
+}
+
+int iwl_eeprom_check_version(struct iwl_eeprom_data *data,
+                             struct iwl_trans *trans);
+
+#endif /* __iwl_eeprom_parse_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
new file mode 100644
index 00000000000..27c7da3c6ed
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
@@ -0,0 +1,463 @@
+/******************************************************************************
+ *
+ * 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) 2008 - 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) 2005 - 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/types.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+
+#include "iwl-debug.h"
+#include "iwl-eeprom-read.h"
+#include "iwl-io.h"
+#include "iwl-prph.h"
+#include "iwl-csr.h"
+
+/*
+ * EEPROM access time values:
+ *
+ * Driver initiates EEPROM read by writing byte address << 1 to CSR_EEPROM_REG.
+ * Driver then polls CSR_EEPROM_REG for CSR_EEPROM_REG_READ_VALID_MSK (0x1).
+ * When polling, wait 10 uSec between polling loops, up to a maximum 5000 uSec.
+ * Driver reads 16-bit value from bits 31-16 of CSR_EEPROM_REG.
+ */
+#define IWL_EEPROM_ACCESS_TIMEOUT       5000 /* uSec */
+
+#define IWL_EEPROM_SEM_TIMEOUT          10   /* microseconds */
+#define IWL_EEPROM_SEM_RETRY_LIMIT      1000 /* number of attempts (not time) */
+
+
+/*
+ * The device's EEPROM semaphore prevents conflicts between driver and uCode
+ * when accessing the EEPROM; each access is a series of pulses to/from the
+ * EEPROM chip, not a single event, so even reads could conflict if they
+ * weren't arbitrated by the semaphore.
+ */
+
+#define EEPROM_SEM_TIMEOUT 10           /* milliseconds */
+#define EEPROM_SEM_RETRY_LIMIT 1000     /* number of attempts (not time) */
+
+static int iwl_eeprom_acquire_semaphore(struct iwl_trans *trans)
+{
+        u16 count;
+        int ret;
+
+        for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
+                /* Request semaphore */
+                iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
+                            CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+
+                /* See if we got it */
+                ret = iwl_poll_bit(trans, CSR_HW_IF_CONFIG_REG,
+                                CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+                                CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+                                EEPROM_SEM_TIMEOUT);
+                if (ret >= 0) {
+                        IWL_DEBUG_EEPROM(trans->dev,
+                                         "Acquired semaphore after %d tries.\n",
+                                         count+1);
+                        return ret;
+                }
+        }
+
+        return ret;
+}
+
+static void iwl_eeprom_release_semaphore(struct iwl_trans *trans)
+{
+        iwl_clear_bit(trans, CSR_HW_IF_CONFIG_REG,
+                      CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+}
+
+static int iwl_eeprom_verify_signature(struct iwl_trans *trans, bool nvm_is_otp)
+{
+        u32 gp = iwl_read32(trans, CSR_EEPROM_GP) & CSR_EEPROM_GP_VALID_MSK;
+
+        IWL_DEBUG_EEPROM(trans->dev, "EEPROM signature=0x%08x\n", gp);
+
+        switch (gp) {
+        case CSR_EEPROM_GP_BAD_SIG_EEP_GOOD_SIG_OTP:
+                if (!nvm_is_otp) {
+                        IWL_ERR(trans, "EEPROM with bad signature: 0x%08x\n",
+                                gp);
+                        return -ENOENT;
+                }
+                return 0;
+        case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
+        case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
+                if (nvm_is_otp) {
+                        IWL_ERR(trans, "OTP with bad signature: 0x%08x\n", gp);
+                        return -ENOENT;
+                }
+                return 0;
+        case CSR_EEPROM_GP_BAD_SIGNATURE_BOTH_EEP_AND_OTP:
+        default:
+                IWL_ERR(trans,
+                        "bad EEPROM/OTP signature, type=%s, EEPROM_GP=0x%08x\n",
+                        nvm_is_otp ? "OTP" : "EEPROM", gp);
+                return -ENOENT;
+        }
+}
+
+/******************************************************************************
+ *
+ * OTP related functions
+ *
+******************************************************************************/
+
+static void iwl_set_otp_access_absolute(struct iwl_trans *trans)
+{
+        iwl_read32(trans, CSR_OTP_GP_REG);
+
+        iwl_clear_bit(trans, CSR_OTP_GP_REG,
+                      CSR_OTP_GP_REG_OTP_ACCESS_MODE);
+}
+
+static int iwl_nvm_is_otp(struct iwl_trans *trans)
+{
+        u32 otpgp;
+
+        /* OTP only valid for CP/PP and after */
+        switch (trans->hw_rev & CSR_HW_REV_TYPE_MSK) {
+        case CSR_HW_REV_TYPE_NONE:
+                IWL_ERR(trans, "Unknown hardware type\n");
+                return -EIO;
+        case CSR_HW_REV_TYPE_5300:
+        case CSR_HW_REV_TYPE_5350:
+        case CSR_HW_REV_TYPE_5100:
+        case CSR_HW_REV_TYPE_5150:
+                return 0;
+        default:
+                otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
+                if (otpgp & CSR_OTP_GP_REG_DEVICE_SELECT)
+                        return 1;
+                return 0;
+        }
+}
+
+static int iwl_init_otp_access(struct iwl_trans *trans)
+{
+        int ret;
+
+        /* Enable 40MHz radio clock */
+        iwl_write32(trans, CSR_GP_CNTRL,
+                    iwl_read32(trans, CSR_GP_CNTRL) |
+                    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+        /* wait for clock to be ready */
+        ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
+                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+                           25000);
+        if (ret < 0) {
+                IWL_ERR(trans, "Time out access OTP\n");
+        } else {
+                iwl_set_bits_prph(trans, APMG_PS_CTRL_REG,
+                                  APMG_PS_CTRL_VAL_RESET_REQ);
+                udelay(5);
+                iwl_clear_bits_prph(trans, APMG_PS_CTRL_REG,
+                                    APMG_PS_CTRL_VAL_RESET_REQ);
+
+                /*
+                 * CSR auto clock gate disable bit -
+                 * this is only applicable for HW with OTP shadow RAM
+                 */
+                if (trans->cfg->base_params->shadow_ram_support)
+                        iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
+                                    CSR_RESET_LINK_PWR_MGMT_DISABLED);
+        }
+        return ret;
+}
+
+static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
+                             __le16 *eeprom_data)
+{
+        int ret = 0;
+        u32 r;
+        u32 otpgp;
+
+        iwl_write32(trans, CSR_EEPROM_REG,
+                    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
+        ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
+                                 CSR_EEPROM_REG_READ_VALID_MSK,
+                                 CSR_EEPROM_REG_READ_VALID_MSK,
+                                 IWL_EEPROM_ACCESS_TIMEOUT);
+        if (ret < 0) {
+                IWL_ERR(trans, "Time out reading OTP[%d]\n", addr);
+                return ret;
+        }
+        r = iwl_read32(trans, CSR_EEPROM_REG);
+        /* check for ECC errors: */
+        otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
+        if (otpgp & CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK) {
+                /* stop in this case */
+                /* set the uncorrectable OTP ECC bit for acknowledgement */
+                iwl_set_bit(trans, CSR_OTP_GP_REG,
+                            CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
+                IWL_ERR(trans, "Uncorrectable OTP ECC error, abort OTP read\n");
+                return -EINVAL;
+        }
+        if (otpgp & CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK) {
+                /* continue in this case */
+                /* set the correctable OTP ECC bit for acknowledgement */
+                iwl_set_bit(trans, CSR_OTP_GP_REG,
+                            CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK);
+                IWL_ERR(trans, "Correctable OTP ECC error, continue read\n");
+        }
+        *eeprom_data = cpu_to_le16(r >> 16);
+        return 0;
+}
+
+/*
+ * iwl_is_otp_empty: check for empty OTP
+ */
+static bool iwl_is_otp_empty(struct iwl_trans *trans)
+{
+        u16 next_link_addr = 0;
+        __le16 link_value;
+        bool is_empty = false;
+
+        /* locate the beginning of OTP link list */
+        if (!iwl_read_otp_word(trans, next_link_addr, &link_value)) {
+                if (!link_value) {
+                        IWL_ERR(trans, "OTP is empty\n");
+                        is_empty = true;
+                }
+        } else {
+                IWL_ERR(trans, "Unable to read first block of OTP list.\n");
+                is_empty = true;
+        }
+
+        return is_empty;
+}
+
+
+/*
+ * iwl_find_otp_image: find EEPROM image in OTP
+ *   finding the OTP block that contains the EEPROM image.
+ *   the last valid block on the link list (the block _before_ the last block)
+ *   is the block we should read and used to configure the device.
+ *   If all the available OTP blocks are full, the last block will be the block
+ *   we should read and used to configure the device.
+ *   only perform this operation if shadow RAM is disabled
+ */
+static int iwl_find_otp_image(struct iwl_trans *trans,
+                                        u16 *validblockaddr)
+{
+        u16 next_link_addr = 0, valid_addr;
+        __le16 link_value = 0;
+        int usedblocks = 0;
+
+        /* set addressing mode to absolute to traverse the link list */
+        iwl_set_otp_access_absolute(trans);
+
+        /* checking for empty OTP or error */
+        if (iwl_is_otp_empty(trans))
+                return -EINVAL;
+
+        /*
+         * start traverse link list
+         * until reach the max number of OTP blocks
+         * different devices have different number of OTP blocks
+         */
+        do {
+                /* save current valid block address
+                 * check for more block on the link list
+                 */
+                valid_addr = next_link_addr;
+                next_link_addr = le16_to_cpu(link_value) * sizeof(u16);
+                IWL_DEBUG_EEPROM(trans->dev, "OTP blocks %d addr 0x%x\n",
+                                 usedblocks, next_link_addr);
+                if (iwl_read_otp_word(trans, next_link_addr, &link_value))
+                        return -EINVAL;
+                if (!link_value) {
+                        /*
+                         * reach the end of link list, return success and
+                         * set address point to the starting address
+                         * of the image
+                         */
+                        *validblockaddr = valid_addr;
+                        /* skip first 2 bytes (link list pointer) */
+                        *validblockaddr += 2;
+                        return 0;
+                }
+                /* more in the link list, continue */
+                usedblocks++;
+        } while (usedblocks <= trans->cfg->base_params->max_ll_items);
+
+        /* OTP has no valid blocks */
+        IWL_DEBUG_EEPROM(trans->dev, "OTP has no valid blocks\n");
+        return -EINVAL;
+}
+
+/**
+ * iwl_read_eeprom - read EEPROM contents
+ *
+ * Load the EEPROM contents from adapter and return it
+ * and its size.
+ *
+ * NOTE:  This routine uses the non-debug IO access functions.
+ */
+int iwl_read_eeprom(struct iwl_trans *trans, u8 **eeprom, size_t *eeprom_size)
+{
+        __le16 *e;
+        u32 gp = iwl_read32(trans, CSR_EEPROM_GP);
+        int sz;
+        int ret;
+        u16 addr;
+        u16 validblockaddr = 0;
+        u16 cache_addr = 0;
+        int nvm_is_otp;
+
+        if (!eeprom || !eeprom_size)
+                return -EINVAL;
+
+        nvm_is_otp = iwl_nvm_is_otp(trans);
+        if (nvm_is_otp < 0)
+                return nvm_is_otp;
+
+        sz = trans->cfg->base_params->eeprom_size;
+        IWL_DEBUG_EEPROM(trans->dev, "NVM size = %d\n", sz);
+
+        e = kmalloc(sz, GFP_KERNEL);
+        if (!e)
+                return -ENOMEM;
+
+        ret = iwl_eeprom_verify_signature(trans, nvm_is_otp);
+        if (ret < 0) {
+                IWL_ERR(trans, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
+                goto err_free;
+        }
+
+        /* Make sure driver (instead of uCode) is allowed to read EEPROM */
+        ret = iwl_eeprom_acquire_semaphore(trans);
+        if (ret < 0) {
+                IWL_ERR(trans, "Failed to acquire EEPROM semaphore.\n");
+                goto err_free;
+        }
+
+        if (nvm_is_otp) {
+                ret = iwl_init_otp_access(trans);
+                if (ret) {
+                        IWL_ERR(trans, "Failed to initialize OTP access.\n");
+                        goto err_unlock;
+                }
+
+                iwl_write32(trans, CSR_EEPROM_GP,
+                            iwl_read32(trans, CSR_EEPROM_GP) &
+                            ~CSR_EEPROM_GP_IF_OWNER_MSK);
+
+                iwl_set_bit(trans, CSR_OTP_GP_REG,
+                            CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK |
+                            CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
+                /* traversing the linked list if no shadow ram supported */
+                if (!trans->cfg->base_params->shadow_ram_support) {
+                        ret = iwl_find_otp_image(trans, &validblockaddr);
+                        if (ret)
+                                goto err_unlock;
+                }
+                for (addr = validblockaddr; addr < validblockaddr + sz;
+                     addr += sizeof(u16)) {
+                        __le16 eeprom_data;
+
+                        ret = iwl_read_otp_word(trans, addr, &eeprom_data);
+                        if (ret)
+                                goto err_unlock;
+                        e[cache_addr / 2] = eeprom_data;
+                        cache_addr += sizeof(u16);
+                }
+        } else {
+                /* eeprom is an array of 16bit values */
+                for (addr = 0; addr < sz; addr += sizeof(u16)) {
+                        u32 r;
+
+                        iwl_write32(trans, CSR_EEPROM_REG,
+                                    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
+
+                        ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
+                                           CSR_EEPROM_REG_READ_VALID_MSK,
+                                           CSR_EEPROM_REG_READ_VALID_MSK,
+                                           IWL_EEPROM_ACCESS_TIMEOUT);
+                        if (ret < 0) {
+                                IWL_ERR(trans,
+                                        "Time out reading EEPROM[%d]\n", addr);
+                                goto err_unlock;
+                        }
+                        r = iwl_read32(trans, CSR_EEPROM_REG);
+                        e[addr / 2] = cpu_to_le16(r >> 16);
+                }
+        }
+
+        IWL_DEBUG_EEPROM(trans->dev, "NVM Type: %s\n",
+                         nvm_is_otp ? "OTP" : "EEPROM");
+
+        iwl_eeprom_release_semaphore(trans);
+
+        *eeprom_size = sz;
+        *eeprom = (u8 *)e;
+        return 0;
+
+ err_unlock:
+        iwl_eeprom_release_semaphore(trans);
+ err_free:
+        kfree(e);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(iwl_read_eeprom);
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
new file mode 100644
index 00000000000..1337c9d36fe
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
@@ -0,0 +1,70 @@
+/******************************************************************************
+ *
+ * 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) 2008 - 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) 2005 - 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_eeprom_h__
+#define __iwl_eeprom_h__
+
+#include "iwl-trans.h"
+
+int iwl_read_eeprom(struct iwl_trans *trans, u8 **eeprom, size_t *eeprom_size);
+
+#endif  /* __iwl_eeprom_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
deleted file mode 100644
index b8e2b223ac3..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/******************************************************************************
- *
- * 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) 2008 - 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) 2005 - 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/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/init.h>
-
-#include <net/mac80211.h>
-
-#include "iwl-dev.h"
-#include "iwl-debug.h"
-#include "iwl-agn.h"
-#include "iwl-eeprom.h"
-#include "iwl-io.h"
-#include "iwl-prph.h"
-
-/************************** EEPROM BANDS ****************************
- *
- * The iwl_eeprom_band definitions below provide the mapping from the
- * EEPROM contents to the specific channel number supported for each
- * band.
- *
- * For example, iwl_priv->eeprom.band_3_channels[4] from the band_3
- * definition below maps to physical channel 42 in the 5.2GHz spectrum.
- * The specific geography and calibration information for that channel
- * is contained in the eeprom map itself.
- *
- * During init, we copy the eeprom information and channel map
- * information into priv->channel_info_24/52 and priv->channel_map_24/52
- *
- * channel_map_24/52 provides the index in the channel_info array for a
- * given channel.  We have to have two separate maps as there is channel
- * overlap with the 2.4GHz and 5.2GHz spectrum as seen in band_1 and
- * band_2
- *
- * A value of 0xff stored in the channel_map indicates that the channel
- * is not supported by the hardware at all.
- *
- * A value of 0xfe in the channel_map indicates that the channel is not
- * valid for Tx with the current hardware.  This means that
- * while the system can tune and receive on a given channel, it may not
- * be able to associate or transmit any frames on that
- * channel.  There is no corresponding channel information for that
- * entry.
- *
- *********************************************************************/
-
-/* 2.4 GHz */
-const u8 iwl_eeprom_band_1[14] = {
-        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
-};
-
-/* 5.2 GHz bands */
-static const u8 iwl_eeprom_band_2[] = { /* 4915-5080MHz */
-        183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
-};
-
-static const u8 iwl_eeprom_band_3[] = { /* 5170-5320MHz */
-        34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
-};
-
-static const u8 iwl_eeprom_band_4[] = { /* 5500-5700MHz */
-        100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
-};
-
-static const u8 iwl_eeprom_band_5[] = { /* 5725-5825MHz */
-        145, 149, 153, 157, 161, 165
-};
-
-static const u8 iwl_eeprom_band_6[] = {       /* 2.4 ht40 channel */
-        1, 2, 3, 4, 5, 6, 7
-};
-
-static const u8 iwl_eeprom_band_7[] = {       /* 5.2 ht40 channel */
-        36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
-};
-
-/******************************************************************************
- *
- * generic NVM functions
- *
-******************************************************************************/
-
-/*
- * The device's EEPROM semaphore prevents conflicts between driver and uCode
- * when accessing the EEPROM; each access is a series of pulses to/from the
- * EEPROM chip, not a single event, so even reads could conflict if they
- * weren't arbitrated by the semaphore.
- */
-
-#define EEPROM_SEM_TIMEOUT 10           /* milliseconds */
-#define EEPROM_SEM_RETRY_LIMIT 1000     /* number of attempts (not time) */
-
-static int iwl_eeprom_acquire_semaphore(struct iwl_trans *trans)
-{
-        u16 count;
-        int ret;
-
-        for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
-                /* Request semaphore */
-                iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
-                            CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
-
-                /* See if we got it */
-                ret = iwl_poll_bit(trans, CSR_HW_IF_CONFIG_REG,
-                                CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
-                                CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
-                                EEPROM_SEM_TIMEOUT);
-                if (ret >= 0) {
-                        IWL_DEBUG_EEPROM(trans,
-                                "Acquired semaphore after %d tries.\n",
-                                count+1);
-                        return ret;
-                }
-        }
-
-        return ret;
-}
-
-static void iwl_eeprom_release_semaphore(struct iwl_trans *trans)
-{
-        iwl_clear_bit(trans, CSR_HW_IF_CONFIG_REG,
-                CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
-
-}
-
-static int iwl_eeprom_verify_signature(struct iwl_priv *priv)
-{
-        u32 gp = iwl_read32(priv->trans, CSR_EEPROM_GP) &
-                           CSR_EEPROM_GP_VALID_MSK;
-        int ret = 0;
-
-        IWL_DEBUG_EEPROM(priv, "EEPROM signature=0x%08x\n", gp);
-        switch (gp) {
-        case CSR_EEPROM_GP_BAD_SIG_EEP_GOOD_SIG_OTP:
-                if (priv->nvm_device_type != NVM_DEVICE_TYPE_OTP) {
-                        IWL_ERR(priv, "EEPROM with bad signature: 0x%08x\n",
-                                gp);
-                        ret = -ENOENT;
-                }
-                break;
-        case CSR_EEPROM_GP_GOOD_SIG_EEP_LESS_THAN_4K:
-        case CSR_EEPROM_GP_GOOD_SIG_EEP_MORE_THAN_4K:
-                if (priv->nvm_device_type != NVM_DEVICE_TYPE_EEPROM) {
-                        IWL_ERR(priv, "OTP with bad signature: 0x%08x\n", gp);
-                        ret = -ENOENT;
-                }
-                break;
-        case CSR_EEPROM_GP_BAD_SIGNATURE_BOTH_EEP_AND_OTP:
-        default:
-                IWL_ERR(priv, "bad EEPROM/OTP signature, type=%s, "
-                        "EEPROM_GP=0x%08x\n",
-                        (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-                        ? "OTP" : "EEPROM", gp);
-                ret = -ENOENT;
-                break;
-        }
-        return ret;
-}
-
-u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset)
-{
-        if (!priv->eeprom)
-                return 0;
-        return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
-}
-
-int iwl_eeprom_check_version(struct iwl_priv *priv)
-{
-        u16 eeprom_ver;
-        u16 calib_ver;
-
-        eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
-        calib_ver = iwl_eeprom_calib_version(priv);
-
-        if (eeprom_ver < priv->cfg->eeprom_ver ||
-            calib_ver < priv->cfg->eeprom_calib_ver)
-                goto err;
-
-        IWL_INFO(priv, "device EEPROM VER=0x%x, CALIB=0x%x\n",
-                 eeprom_ver, calib_ver);
-
-        return 0;
-err:
-        IWL_ERR(priv, "Unsupported (too old) EEPROM VER=0x%x < 0x%x "
-                  "CALIB=0x%x < 0x%x\n",
-                  eeprom_ver, priv->cfg->eeprom_ver,
-                  calib_ver,  priv->cfg->eeprom_calib_ver);
-        return -EINVAL;
-
-}
-
-int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
-{
-        u16 radio_cfg;
-
-        priv->hw_params.sku = iwl_eeprom_query16(priv, EEPROM_SKU_CAP);
-        if (priv->hw_params.sku & EEPROM_SKU_CAP_11N_ENABLE &&
-            !priv->cfg->ht_params) {
-                IWL_ERR(priv, "Invalid 11n configuration\n");
-                return -EINVAL;
-        }
-
-        if (!priv->hw_params.sku) {
-                IWL_ERR(priv, "Invalid device sku\n");
-                return -EINVAL;
-        }
-
-        IWL_INFO(priv, "Device SKU: 0x%X\n", priv->hw_params.sku);
-
-        radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
-
-        priv->hw_params.valid_tx_ant = EEPROM_RF_CFG_TX_ANT_MSK(radio_cfg);
-        priv->hw_params.valid_rx_ant = EEPROM_RF_CFG_RX_ANT_MSK(radio_cfg);
-
-        /* check overrides (some devices have wrong EEPROM) */
-        if (priv->cfg->valid_tx_ant)
-                priv->hw_params.valid_tx_ant = priv->cfg->valid_tx_ant;
-        if (priv->cfg->valid_rx_ant)
-                priv->hw_params.valid_rx_ant = priv->cfg->valid_rx_ant;
-
-        if (!priv->hw_params.valid_tx_ant || !priv->hw_params.valid_rx_ant) {
-                IWL_ERR(priv, "Invalid chain (0x%X, 0x%X)\n",
-                        priv->hw_params.valid_tx_ant,
-                        priv->hw_params.valid_rx_ant);
-                return -EINVAL;
-        }
-
-        IWL_INFO(priv, "Valid Tx ant: 0x%X, Valid Rx ant: 0x%X\n",
-                 priv->hw_params.valid_tx_ant, priv->hw_params.valid_rx_ant);
-
-        return 0;
-}
-
-u16 iwl_eeprom_calib_version(struct iwl_priv *priv)
-{
-        struct iwl_eeprom_calib_hdr *hdr;
-
-        hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
-                                                        EEPROM_CALIB_ALL);
-        return hdr->version;
-}
-
-static u32 eeprom_indirect_address(struct iwl_priv *priv, u32 address)
-{
-        u16 offset = 0;
-
-        if ((address & INDIRECT_ADDRESS) == 0)
-                return address;
-
-        switch (address & INDIRECT_TYPE_MSK) {
-        case INDIRECT_HOST:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_HOST);
-                break;
-        case INDIRECT_GENERAL:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_GENERAL);
-                break;
-        case INDIRECT_REGULATORY:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_REGULATORY);
-                break;
-        case INDIRECT_TXP_LIMIT:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT);
-                break;
-        case INDIRECT_TXP_LIMIT_SIZE:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_TXP_LIMIT_SIZE);
-                break;
-        case INDIRECT_CALIBRATION:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_CALIBRATION);
-                break;
-        case INDIRECT_PROCESS_ADJST:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_PROCESS_ADJST);
-                break;
-        case INDIRECT_OTHERS:
-                offset = iwl_eeprom_query16(priv, EEPROM_LINK_OTHERS);
-                break;
-        default:
-                IWL_ERR(priv, "illegal indirect type: 0x%X\n",
-                address & INDIRECT_TYPE_MSK);
-                break;
-        }
-
-        /* translate the offset from words to byte */
-        return (address & ADDRESS_MSK) + (offset << 1);
-}
-
-const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset)
-{
-        u32 address = eeprom_indirect_address(priv, offset);
-        BUG_ON(address >= priv->cfg->base_params->eeprom_size);
-        return &priv->eeprom[address];
-}
-
-void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac)
-{
-        const u8 *addr = iwl_eeprom_query_addr(priv,
-                                        EEPROM_MAC_ADDRESS);
-        memcpy(mac, addr, ETH_ALEN);
-}
-
-/******************************************************************************
- *
- * OTP related functions
- *
-******************************************************************************/
-
-static void iwl_set_otp_access(struct iwl_trans *trans,
-                               enum iwl_access_mode mode)
-{
-        iwl_read32(trans, CSR_OTP_GP_REG);
-
-        if (mode == IWL_OTP_ACCESS_ABSOLUTE)
-                iwl_clear_bit(trans, CSR_OTP_GP_REG,
-                              CSR_OTP_GP_REG_OTP_ACCESS_MODE);
-        else
-                iwl_set_bit(trans, CSR_OTP_GP_REG,
-                            CSR_OTP_GP_REG_OTP_ACCESS_MODE);
-}
-
-static int iwl_get_nvm_type(struct iwl_trans *trans, u32 hw_rev)
-{
-        u32 otpgp;
-        int nvm_type;
-
-        /* OTP only valid for CP/PP and after */
-        switch (hw_rev & CSR_HW_REV_TYPE_MSK) {
-        case CSR_HW_REV_TYPE_NONE:
-                IWL_ERR(trans, "Unknown hardware type\n");
-                return -ENOENT;
-        case CSR_HW_REV_TYPE_5300:
-        case CSR_HW_REV_TYPE_5350:
-        case CSR_HW_REV_TYPE_5100:
-        case CSR_HW_REV_TYPE_5150:
-                nvm_type = NVM_DEVICE_TYPE_EEPROM;
-                break;
-        default:
-                otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
-                if (otpgp & CSR_OTP_GP_REG_DEVICE_SELECT)
-                        nvm_type = NVM_DEVICE_TYPE_OTP;
-                else
-                        nvm_type = NVM_DEVICE_TYPE_EEPROM;
-                break;
-        }
-        return  nvm_type;
-}
-
-static int iwl_init_otp_access(struct iwl_trans *trans)
-{
-        int ret;
-
-        /* Enable 40MHz radio clock */
-        iwl_write32(trans, CSR_GP_CNTRL,
-                    iwl_read32(trans, CSR_GP_CNTRL) |
-                    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
-
-        /* wait for clock to be ready */
-        ret = iwl_poll_bit(trans, CSR_GP_CNTRL,
-                                 CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-                                 CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-                                 25000);
-        if (ret < 0)
-                IWL_ERR(trans, "Time out access OTP\n");
-        else {
-                iwl_set_bits_prph(trans, APMG_PS_CTRL_REG,
-                                  APMG_PS_CTRL_VAL_RESET_REQ);
-                udelay(5);
-                iwl_clear_bits_prph(trans, APMG_PS_CTRL_REG,
-                                    APMG_PS_CTRL_VAL_RESET_REQ);
-
-                /*
-                 * CSR auto clock gate disable bit -
-                 * this is only applicable for HW with OTP shadow RAM
-                 */
-                if (trans->cfg->base_params->shadow_ram_support)
-                        iwl_set_bit(trans, CSR_DBG_LINK_PWR_MGMT_REG,
-                                CSR_RESET_LINK_PWR_MGMT_DISABLED);
-        }
-        return ret;
-}
-
-static int iwl_read_otp_word(struct iwl_trans *trans, u16 addr,
-                             __le16 *eeprom_data)
-{
-        int ret = 0;
-        u32 r;
-        u32 otpgp;
-
-        iwl_write32(trans, CSR_EEPROM_REG,
-                    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
-        ret = iwl_poll_bit(trans, CSR_EEPROM_REG,
-                                 CSR_EEPROM_REG_READ_VALID_MSK,
-                                 CSR_EEPROM_REG_READ_VALID_MSK,
-                                 IWL_EEPROM_ACCESS_TIMEOUT);
-        if (ret < 0) {
-                IWL_ERR(trans, "Time out reading OTP[%d]\n", addr);
-                return ret;
-        }
-        r = iwl_read32(trans, CSR_EEPROM_REG);
-        /* check for ECC errors: */
-        otpgp = iwl_read32(trans, CSR_OTP_GP_REG);
-        if (otpgp & CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK) {
-                /* stop in this case */
-                /* set the uncorrectable OTP ECC bit for acknowledgement */
-                iwl_set_bit(trans, CSR_OTP_GP_REG,
-                        CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
-                IWL_ERR(trans, "Uncorrectable OTP ECC error, abort OTP read\n");
-                return -EINVAL;
-        }
-        if (otpgp & CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK) {
-                /* continue in this case */
-                /* set the correctable OTP ECC bit for acknowledgement */
-                iwl_set_bit(trans, CSR_OTP_GP_REG,
-                                CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK);
-                IWL_ERR(trans, "Correctable OTP ECC error, continue read\n");
-        }
-        *eeprom_data = cpu_to_le16(r >> 16);
-        return 0;
-}
-
-/*
- * iwl_is_otp_empty: check for empty OTP
- */
-static bool iwl_is_otp_empty(struct iwl_trans *trans)
-{
-        u16 next_link_addr = 0;
-        __le16 link_value;
-        bool is_empty = false;
-
-        /* locate the beginning of OTP link list */
-        if (!iwl_read_otp_word(trans, next_link_addr, &link_value)) {
-                if (!link_value) {
-                        IWL_ERR(trans, "OTP is empty\n");
-                        is_empty = true;
-                }
-        } else {
-                IWL_ERR(trans, "Unable to read first block of OTP list.\n");
-                is_empty = true;
-        }
-
-        return is_empty;
-}
-
-
-/*
- * iwl_find_otp_image: find EEPROM image in OTP
- *   finding the OTP block that contains the EEPROM image.
- *   the last valid block on the link list (the block _before_ the last block)
- *   is the block we should read and used to configure the device.
- *   If all the available OTP blocks are full, the last block will be the block
- *   we should read and used to configure the device.
- *   only perform this operation if shadow RAM is disabled
- */
-static int iwl_find_otp_image(struct iwl_trans *trans,
-                                        u16 *validblockaddr)
-{
-        u16 next_link_addr = 0, valid_addr;
-        __le16 link_value = 0;
-        int usedblocks = 0;
-
-        /* set addressing mode to absolute to traverse the link list */
-        iwl_set_otp_access(trans, IWL_OTP_ACCESS_ABSOLUTE);
-
-        /* checking for empty OTP or error */
-        if (iwl_is_otp_empty(trans))
-                return -EINVAL;
-
-        /*
-         * start traverse link list
-         * until reach the max number of OTP blocks
-         * different devices have different number of OTP blocks
-         */
-        do {
-                /* save current valid block address
-                 * check for more block on the link list
-                 */
-                valid_addr = next_link_addr;
-                next_link_addr = le16_to_cpu(link_value) * sizeof(u16);
-                IWL_DEBUG_EEPROM(trans, "OTP blocks %d addr 0x%x\n",
-                               usedblocks, next_link_addr);
-                if (iwl_read_otp_word(trans, next_link_addr, &link_value))
-                        return -EINVAL;
-                if (!link_value) {
-                        /*
-                         * reach the end of link list, return success and
-                         * set address point to the starting address
-                         * of the image
-                         */
-                        *validblockaddr = valid_addr;
-                        /* skip first 2 bytes (link list pointer) */
-                        *validblockaddr += 2;
-                        return 0;
-                }
-                /* more in the link list, continue */
-                usedblocks++;
-        } while (usedblocks <= trans->cfg->base_params->max_ll_items);
-
-        /* OTP has no valid blocks */
-        IWL_DEBUG_EEPROM(trans, "OTP has no valid blocks\n");
-        return -EINVAL;
-}
-
-/******************************************************************************
- *
- * Tx Power related functions
- *
-******************************************************************************/
-/**
- * iwl_get_max_txpower_avg - get the highest tx power from all chains.
- *     find the highest tx power from all chains for the channel
- */
-static s8 iwl_get_max_txpower_avg(struct iwl_priv *priv,
-                struct iwl_eeprom_enhanced_txpwr *enhanced_txpower,
-                int element, s8 *max_txpower_in_half_dbm)
-{
-        s8 max_txpower_avg = 0; /* (dBm) */
-
-        /* Take the highest tx power from any valid chains */
-        if ((priv->hw_params.valid_tx_ant & ANT_A) &&
-            (enhanced_txpower[element].chain_a_max > max_txpower_avg))
-                max_txpower_avg = enhanced_txpower[element].chain_a_max;
-        if ((priv->hw_params.valid_tx_ant & ANT_B) &&
-            (enhanced_txpower[element].chain_b_max > max_txpower_avg))
-                max_txpower_avg = enhanced_txpower[element].chain_b_max;
-        if ((priv->hw_params.valid_tx_ant & ANT_C) &&
-            (enhanced_txpower[element].chain_c_max > max_txpower_avg))
-                max_txpower_avg = enhanced_txpower[element].chain_c_max;
-        if (((priv->hw_params.valid_tx_ant == ANT_AB) |
-            (priv->hw_params.valid_tx_ant == ANT_BC) |
-            (priv->hw_params.valid_tx_ant == ANT_AC)) &&
-            (enhanced_txpower[element].mimo2_max > max_txpower_avg))
-                max_txpower_avg =  enhanced_txpower[element].mimo2_max;
-        if ((priv->hw_params.valid_tx_ant == ANT_ABC) &&
-            (enhanced_txpower[element].mimo3_max > max_txpower_avg))
-                max_txpower_avg = enhanced_txpower[element].mimo3_max;
-
-        /*
-         * max. tx power in EEPROM is in 1/2 dBm format
-         * convert from 1/2 dBm to dBm (round-up convert)
-         * but we also do not want to loss 1/2 dBm resolution which
-         * will impact performance
-         */
-        *max_txpower_in_half_dbm = max_txpower_avg;
-        return (max_txpower_avg & 0x01) + (max_txpower_avg >> 1);
-}
-
-static void
-iwl_eeprom_enh_txp_read_element(struct iwl_priv *priv,
-                                    struct iwl_eeprom_enhanced_txpwr *txp,
-                                    s8 max_txpower_avg)
-{
-        int ch_idx;
-        bool is_ht40 = txp->flags & IWL_EEPROM_ENH_TXP_FL_40MHZ;
-        enum ieee80211_band band;
-
-        band = txp->flags & IWL_EEPROM_ENH_TXP_FL_BAND_52G ?
-                IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
-
-        for (ch_idx = 0; ch_idx < priv->channel_count; ch_idx++) {
-                struct iwl_channel_info *ch_info = &priv->channel_info[ch_idx];
-
-                /* update matching channel or from common data only */
-                if (txp->channel != 0 && ch_info->channel != txp->channel)
-                        continue;
-
-                /* update matching band only */
-                if (band != ch_info->band)
-                        continue;
-
-                if (ch_info->max_power_avg < max_txpower_avg && !is_ht40) {
-                        ch_info->max_power_avg = max_txpower_avg;
-                        ch_info->curr_txpow = max_txpower_avg;
-                        ch_info->scan_power = max_txpower_avg;
-                }
-
-                if (is_ht40 && ch_info->ht40_max_power_avg < max_txpower_avg)
-                        ch_info->ht40_max_power_avg = max_txpower_avg;
-        }
-}
-
-#define EEPROM_TXP_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT)
-#define EEPROM_TXP_ENTRY_LEN sizeof(struct iwl_eeprom_enhanced_txpwr)
-#define EEPROM_TXP_SZ_OFFS (0x00 | INDIRECT_ADDRESS | INDIRECT_TXP_LIMIT_SIZE)
-
-#define TXP_CHECK_AND_PRINT(x) ((txp->flags & IWL_EEPROM_ENH_TXP_FL_##x) \
-                            ? # x " " : "")
-
-static void iwl_eeprom_enhanced_txpower(struct iwl_priv *priv)
-{
-        struct iwl_eeprom_enhanced_txpwr *txp_array, *txp;
-        int idx, entries;
-        __le16 *txp_len;
-        s8 max_txp_avg, max_txp_avg_halfdbm;
-
-        BUILD_BUG_ON(sizeof(struct iwl_eeprom_enhanced_txpwr) != 8);
-
-        /* the length is in 16-bit words, but we want entries */
-        txp_len = (__le16 *) iwl_eeprom_query_addr(priv, EEPROM_TXP_SZ_OFFS);
-        entries = le16_to_cpup(txp_len) * 2 / EEPROM_TXP_ENTRY_LEN;
-
-        txp_array = (void *) iwl_eeprom_query_addr(priv, EEPROM_TXP_OFFS);
-
-        for (idx = 0; idx < entries; idx++) {
-                txp = &txp_array[idx];
-                /* skip invalid entries */
-                if (!(txp->flags & IWL_EEPROM_ENH_TXP_FL_VALID))
-                        continue;
-
-                IWL_DEBUG_EEPROM(priv, "%s %d:\t %s%s%s%s%s%s%s%s (0x%02x)\n",
-                                 (txp->channel && (txp->flags &
-                                        IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE)) ?
-                                        "Common " : (txp->channel) ?
-                                        "Channel" : "Common",
-                                 (txp->channel),
-                                 TXP_CHECK_AND_PRINT(VALID),
-                                 TXP_CHECK_AND_PRINT(BAND_52G),
-                                 TXP_CHECK_AND_PRINT(OFDM),
-                                 TXP_CHECK_AND_PRINT(40MHZ),
-                                 TXP_CHECK_AND_PRINT(HT_AP),
-                                 TXP_CHECK_AND_PRINT(RES1),
-                                 TXP_CHECK_AND_PRINT(RES2),
-                                 TXP_CHECK_AND_PRINT(COMMON_TYPE),
-                                 txp->flags);
-                IWL_DEBUG_EEPROM(priv, "\t\t chain_A: 0x%02x "
-                                 "chain_B: 0X%02x chain_C: 0X%02x\n",
-                                 txp->chain_a_max, txp->chain_b_max,
-                                 txp->chain_c_max);
-                IWL_DEBUG_EEPROM(priv, "\t\t MIMO2: 0x%02x "
-                                 "MIMO3: 0x%02x High 20_on_40: 0x%02x "
-                                 "Low 20_on_40: 0x%02x\n",
-                                 txp->mimo2_max, txp->mimo3_max,
-                                 ((txp->delta_20_in_40 & 0xf0) >> 4),
-                                 (txp->delta_20_in_40 & 0x0f));
-
-                max_txp_avg = iwl_get_max_txpower_avg(priv, txp_array, idx,
-                                                      &max_txp_avg_halfdbm);
-
-                /*
-                 * Update the user limit values values to the highest
-                 * power supported by any channel
-                 */
-                if (max_txp_avg > priv->tx_power_user_lmt)
-                        priv->tx_power_user_lmt = max_txp_avg;
-                if (max_txp_avg_halfdbm > priv->tx_power_lmt_in_half_dbm)
-                        priv->tx_power_lmt_in_half_dbm = max_txp_avg_halfdbm;
-
-                iwl_eeprom_enh_txp_read_element(priv, txp, max_txp_avg);
-        }
-}
-
-/**
- * iwl_eeprom_init - read EEPROM contents
- *
- * Load the EEPROM contents from adapter into priv->eeprom
- *
- * NOTE:  This routine uses the non-debug IO access functions.
- */
-int iwl_eeprom_init(struct iwl_priv *priv, u32 hw_rev)
-{
-        __le16 *e;
-        u32 gp = iwl_read32(priv->trans, CSR_EEPROM_GP);
-        int sz;
-        int ret;
-        u16 addr;
-        u16 validblockaddr = 0;
-        u16 cache_addr = 0;
-
-        priv->nvm_device_type = iwl_get_nvm_type(priv->trans, hw_rev);
-        if (priv->nvm_device_type == -ENOENT)
-                return -ENOENT;
-        /* allocate eeprom */
-        sz = priv->cfg->base_params->eeprom_size;
-        IWL_DEBUG_EEPROM(priv, "NVM size = %d\n", sz);
-        priv->eeprom = kzalloc(sz, GFP_KERNEL);
-        if (!priv->eeprom) {
-                ret = -ENOMEM;
-                goto alloc_err;
-        }
-        e = (__le16 *)priv->eeprom;
-
-        ret = iwl_eeprom_verify_signature(priv);
-        if (ret < 0) {
-                IWL_ERR(priv, "EEPROM not found, EEPROM_GP=0x%08x\n", gp);
-                ret = -ENOENT;
-                goto err;
-        }
-
-        /* Make sure driver (instead of uCode) is allowed to read EEPROM */
-        ret = iwl_eeprom_acquire_semaphore(priv->trans);
-        if (ret < 0) {
-                IWL_ERR(priv, "Failed to acquire EEPROM semaphore.\n");
-                ret = -ENOENT;
-                goto err;
-        }
-
-        if (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP) {
-
-                ret = iwl_init_otp_access(priv->trans);
-                if (ret) {
-                        IWL_ERR(priv, "Failed to initialize OTP access.\n");
-                        ret = -ENOENT;
-                        goto done;
-                }
-                iwl_write32(priv->trans, CSR_EEPROM_GP,
-                            iwl_read32(priv->trans, CSR_EEPROM_GP) &
-                            ~CSR_EEPROM_GP_IF_OWNER_MSK);
-
-                iwl_set_bit(priv->trans, CSR_OTP_GP_REG,
-                             CSR_OTP_GP_REG_ECC_CORR_STATUS_MSK |
-                             CSR_OTP_GP_REG_ECC_UNCORR_STATUS_MSK);
-                /* traversing the linked list if no shadow ram supported */
-                if (!priv->cfg->base_params->shadow_ram_support) {
-                        if (iwl_find_otp_image(priv->trans, &validblockaddr)) {
-                                ret = -ENOENT;
-                                goto done;
-                        }
-                }
-                for (addr = validblockaddr; addr < validblockaddr + sz;
-                     addr += sizeof(u16)) {
-                        __le16 eeprom_data;
-
-                        ret = iwl_read_otp_word(priv->trans, addr,
-                                                &eeprom_data);
-                        if (ret)
-                                goto done;
-                        e[cache_addr / 2] = eeprom_data;
-                        cache_addr += sizeof(u16);
-                }
-        } else {
-                /* eeprom is an array of 16bit values */
-                for (addr = 0; addr < sz; addr += sizeof(u16)) {
-                        u32 r;
-
-                        iwl_write32(priv->trans, CSR_EEPROM_REG,
-                                    CSR_EEPROM_REG_MSK_ADDR & (addr << 1));
-
-                        ret = iwl_poll_bit(priv->trans, CSR_EEPROM_REG,
-                                                  CSR_EEPROM_REG_READ_VALID_MSK,
-                                                  CSR_EEPROM_REG_READ_VALID_MSK,
-                                                  IWL_EEPROM_ACCESS_TIMEOUT);
-                        if (ret < 0) {
-                                IWL_ERR(priv,
-                                        "Time out reading EEPROM[%d]\n", addr);
-                                goto done;
-                        }
-                        r = iwl_read32(priv->trans, CSR_EEPROM_REG);
-                        e[addr / 2] = cpu_to_le16(r >> 16);
-                }
-        }
-
-        IWL_DEBUG_EEPROM(priv, "NVM Type: %s, version: 0x%x\n",
-                       (priv->nvm_device_type == NVM_DEVICE_TYPE_OTP)
-                       ? "OTP" : "EEPROM",
-                       iwl_eeprom_query16(priv, EEPROM_VERSION));
-
-        ret = 0;
-done:
-        iwl_eeprom_release_semaphore(priv->trans);
-
-err:
-        if (ret)
-                iwl_eeprom_free(priv);
-alloc_err:
-        return ret;
-}
-
-void iwl_eeprom_free(struct iwl_priv *priv)
-{
-        kfree(priv->eeprom);
-        priv->eeprom = NULL;
-}
-
-static void iwl_init_band_reference(struct iwl_priv *priv,
-                        int eep_band, int *eeprom_ch_count,
-                        const struct iwl_eeprom_channel **eeprom_ch_info,
-                        const u8 **eeprom_ch_index)
-{
-        u32 offset = priv->lib->
-                        eeprom_ops.regulatory_bands[eep_band - 1];
-        switch (eep_band) {
-        case 1:         /* 2.4GHz band */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_1;
-                break;
-        case 2:         /* 4.9GHz band */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_2;
-                break;
-        case 3:         /* 5.2GHz band */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_3;
-                break;
-        case 4:         /* 5.5GHz band */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_4;
-                break;
-        case 5:         /* 5.7GHz band */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_5;
-                break;
-        case 6:         /* 2.4GHz ht40 channels */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_6;
-                break;
-        case 7:         /* 5 GHz ht40 channels */
-                *eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
-                *eeprom_ch_info = (struct iwl_eeprom_channel *)
-                                iwl_eeprom_query_addr(priv, offset);
-                *eeprom_ch_index = iwl_eeprom_band_7;
-                break;
-        default:
-                BUG();
-                return;
-        }
-}
-
-#define CHECK_AND_PRINT(x) ((eeprom_ch->flags & EEPROM_CHANNEL_##x) \
-                            ? # x " " : "")
-/**
- * iwl_mod_ht40_chan_info - Copy ht40 channel info into driver's priv.
- *
- * Does not set up a command, or touch hardware.
- */
-static int iwl_mod_ht40_chan_info(struct iwl_priv *priv,
-                              enum ieee80211_band band, u16 channel,
-                              const struct iwl_eeprom_channel *eeprom_ch,
-                              u8 clear_ht40_extension_channel)
-{
-        struct iwl_channel_info *ch_info;
-
-        ch_info = (struct iwl_channel_info *)
-                        iwl_get_channel_info(priv, band, channel);
-
-        if (!is_channel_valid(ch_info))
-                return -1;
-
-        IWL_DEBUG_EEPROM(priv, "HT40 Ch. %d [%sGHz] %s%s%s%s%s(0x%02x %ddBm):"
-                        " Ad-Hoc %ssupported\n",
-                        ch_info->channel,
-                        is_channel_a_band(ch_info) ?
-                        "5.2" : "2.4",
-                        CHECK_AND_PRINT(IBSS),
-                        CHECK_AND_PRINT(ACTIVE),
-                        CHECK_AND_PRINT(RADAR),
-                        CHECK_AND_PRINT(WIDE),
-                        CHECK_AND_PRINT(DFS),
-                        eeprom_ch->flags,
-                        eeprom_ch->max_power_avg,
-                        ((eeprom_ch->flags & EEPROM_CHANNEL_IBSS)
-                         && !(eeprom_ch->flags & EEPROM_CHANNEL_RADAR)) ?
-                        "" : "not ");
-
-        ch_info->ht40_eeprom = *eeprom_ch;
-        ch_info->ht40_max_power_avg = eeprom_ch->max_power_avg;
-        ch_info->ht40_flags = eeprom_ch->flags;
-        if (eeprom_ch->flags & EEPROM_CHANNEL_VALID)
-                ch_info->ht40_extension_channel &= ~clear_ht40_extension_channel;
-
-        return 0;
-}
-
-#define CHECK_AND_PRINT_I(x) ((eeprom_ch_info[ch].flags & EEPROM_CHANNEL_##x) \
-                            ? # x " " : "")
-
-/**
- * iwl_init_channel_map - Set up driver's info for all possible channels
- */
-int iwl_init_channel_map(struct iwl_priv *priv)
-{
-        int eeprom_ch_count = 0;
-        const u8 *eeprom_ch_index = NULL;
-        const struct iwl_eeprom_channel *eeprom_ch_info = NULL;
-        int band, ch;
-        struct iwl_channel_info *ch_info;
-
-        if (priv->channel_count) {
-                IWL_DEBUG_EEPROM(priv, "Channel map already initialized.\n");
-                return 0;
-        }
-
-        IWL_DEBUG_EEPROM(priv, "Initializing regulatory info from EEPROM\n");
-
-        priv->channel_count =
-            ARRAY_SIZE(iwl_eeprom_band_1) +
-            ARRAY_SIZE(iwl_eeprom_band_2) +
-            ARRAY_SIZE(iwl_eeprom_band_3) +
-            ARRAY_SIZE(iwl_eeprom_band_4) +
-            ARRAY_SIZE(iwl_eeprom_band_5);
-
-        IWL_DEBUG_EEPROM(priv, "Parsing data for %d channels.\n",
-                        priv->channel_count);
-
-        priv->channel_info = kcalloc(priv->channel_count,
-                                     sizeof(struct iwl_channel_info),
-                                     GFP_KERNEL);
-        if (!priv->channel_info) {
-                IWL_ERR(priv, "Could not allocate channel_info\n");
-                priv->channel_count = 0;
-                return -ENOMEM;
-        }
-
-        ch_info = priv->channel_info;
-
-        /* Loop through the 5 EEPROM bands adding them in order to the
-         * channel map we maintain (that contains additional information than
-         * what just in the EEPROM) */
-        for (band = 1; band <= 5; band++) {
-
-                iwl_init_band_reference(priv, band, &eeprom_ch_count,
-                                        &eeprom_ch_info, &eeprom_ch_index);
-
-                /* Loop through each band adding each of the channels */
-                for (ch = 0; ch < eeprom_ch_count; ch++) {
-                        ch_info->channel = eeprom_ch_index[ch];
-                        ch_info->band = (band == 1) ? IEEE80211_BAND_2GHZ :
-                            IEEE80211_BAND_5GHZ;
-
-                        /* permanently store EEPROM's channel regulatory flags
-                         *   and max power in channel info database. */
-                        ch_info->eeprom = eeprom_ch_info[ch];
-
-                        /* Copy the run-time flags so they are there even on
-                         * invalid channels */
-                        ch_info->flags = eeprom_ch_info[ch].flags;
-                        /* First write that ht40 is not enabled, and then enable
-                         * one by one */
-                        ch_info->ht40_extension_channel =
-                                        IEEE80211_CHAN_NO_HT40;
-
-                        if (!(is_channel_valid(ch_info))) {
-                                IWL_DEBUG_EEPROM(priv,
-                                               "Ch. %d Flags %x [%sGHz] - "
-                                               "No traffic\n",
-                                               ch_info->channel,
-                                               ch_info->flags,
-                                               is_channel_a_band(ch_info) ?
-                                               "5.2" : "2.4");
-                                ch_info++;
-                                continue;
-                        }
-
-                        /* Initialize regulatory-based run-time data */
-                        ch_info->max_power_avg = ch_info->curr_txpow =
-                            eeprom_ch_info[ch].max_power_avg;
-                        ch_info->scan_power = eeprom_ch_info[ch].max_power_avg;
-                        ch_info->min_power = 0;
-
-                        IWL_DEBUG_EEPROM(priv, "Ch. %d [%sGHz] "
-                                       "%s%s%s%s%s%s(0x%02x %ddBm):"
-                                       " Ad-Hoc %ssupported\n",
-                                       ch_info->channel,
-                                       is_channel_a_band(ch_info) ?
-                                       "5.2" : "2.4",
-                                       CHECK_AND_PRINT_I(VALID),
-                                       CHECK_AND_PRINT_I(IBSS),
-                                       CHECK_AND_PRINT_I(ACTIVE),
-                                       CHECK_AND_PRINT_I(RADAR),
-                                       CHECK_AND_PRINT_I(WIDE),
-                                       CHECK_AND_PRINT_I(DFS),
-                                       eeprom_ch_info[ch].flags,
-                                       eeprom_ch_info[ch].max_power_avg,
-                                       ((eeprom_ch_info[ch].
-                                         flags & EEPROM_CHANNEL_IBSS)
-                                        && !(eeprom_ch_info[ch].
-                                             flags & EEPROM_CHANNEL_RADAR))
-                                       ? "" : "not ");
-
-                        ch_info++;
-                }
-        }
-
-        /* Check if we do have HT40 channels */
-        if (priv->lib->eeprom_ops.regulatory_bands[5] ==
-            EEPROM_REGULATORY_BAND_NO_HT40 &&
-            priv->lib->eeprom_ops.regulatory_bands[6] ==
-            EEPROM_REGULATORY_BAND_NO_HT40)
-                return 0;
-
-        /* Two additional EEPROM bands for 2.4 and 5 GHz HT40 channels */
-        for (band = 6; band <= 7; band++) {
-                enum ieee80211_band ieeeband;
-
-                iwl_init_band_reference(priv, band, &eeprom_ch_count,
-                                        &eeprom_ch_info, &eeprom_ch_index);
-
-                /* EEPROM band 6 is 2.4, band 7 is 5 GHz */
-                ieeeband =
-                        (band == 6) ? IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
-
-                /* Loop through each band adding each of the channels */
-                for (ch = 0; ch < eeprom_ch_count; ch++) {
-                        /* Set up driver's info for lower half */
-                        iwl_mod_ht40_chan_info(priv, ieeeband,
-                                                eeprom_ch_index[ch],
-                                                &eeprom_ch_info[ch],
-                                                IEEE80211_CHAN_NO_HT40PLUS);
-
-                        /* Set up driver's info for upper half */
-                        iwl_mod_ht40_chan_info(priv, ieeeband,
-                                                eeprom_ch_index[ch] + 4,
-                                                &eeprom_ch_info[ch],
-                                                IEEE80211_CHAN_NO_HT40MINUS);
-                }
-        }
-
-        /* for newer device (6000 series and up)
-         * EEPROM contain enhanced tx power information
-         * driver need to process addition information
-         * to determine the max channel tx power limits
-         */
-        if (priv->lib->eeprom_ops.enhanced_txpower)
-                iwl_eeprom_enhanced_txpower(priv);
-
-        return 0;
-}
-
-/*
- * iwl_free_channel_map - undo allocations in iwl_init_channel_map
- */
-void iwl_free_channel_map(struct iwl_priv *priv)
-{
-        kfree(priv->channel_info);
-        priv->channel_count = 0;
-}
-
-/**
- * iwl_get_channel_info - Find driver's private channel info
- *
- * Based on band and channel number.
- */
-const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
-                                        enum ieee80211_band band, u16 channel)
-{
-        int i;
-
-        switch (band) {
-        case IEEE80211_BAND_5GHZ:
-                for (i = 14; i < priv->channel_count; i++) {
-                        if (priv->channel_info[i].channel == channel)
-                                return &priv->channel_info[i];
-                }
-                break;
-        case IEEE80211_BAND_2GHZ:
-                if (channel >= 1 && channel <= 14)
-                        return &priv->channel_info[channel - 1];
-                break;
-        default:
-                BUG();
-        }
-
-        return NULL;
-}
-
-void iwl_rf_config(struct iwl_priv *priv)
-{
-        u16 radio_cfg;
-
-        radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
-
-        /* write radio config values to register */
-        if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) <= EEPROM_RF_CONFIG_TYPE_MAX) {
-                iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
-                            EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
-                            EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
-                            EEPROM_RF_CFG_DASH_MSK(radio_cfg));
-                IWL_INFO(priv, "Radio type=0x%x-0x%x-0x%x\n",
-                         EEPROM_RF_CFG_TYPE_MSK(radio_cfg),
-                         EEPROM_RF_CFG_STEP_MSK(radio_cfg),
-                         EEPROM_RF_CFG_DASH_MSK(radio_cfg));
-        } else
-                WARN_ON(1);
-
-        /* set CSR_HW_CONFIG_REG for uCode use */
-        iwl_set_bit(priv->trans, CSR_HW_IF_CONFIG_REG,
-                    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-                    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
-}
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
deleted file mode 100644
index 64bfd947cae..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ /dev/null
@@ -1,269 +0,0 @@
-/******************************************************************************
- *
- * 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) 2008 - 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) 2005 - 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_eeprom_h__
-#define __iwl_eeprom_h__
-
-#include <net/mac80211.h>
-
-struct iwl_priv;
-
-/*
- * EEPROM access time values:
- *
- * Driver initiates EEPROM read by writing byte address << 1 to CSR_EEPROM_REG.
- * Driver then polls CSR_EEPROM_REG for CSR_EEPROM_REG_READ_VALID_MSK (0x1).
- * When polling, wait 10 uSec between polling loops, up to a maximum 5000 uSec.
- * Driver reads 16-bit value from bits 31-16 of CSR_EEPROM_REG.
- */
-#define IWL_EEPROM_ACCESS_TIMEOUT       5000 /* uSec */
-
-#define IWL_EEPROM_SEM_TIMEOUT          10   /* microseconds */
-#define IWL_EEPROM_SEM_RETRY_LIMIT      1000 /* number of attempts (not time) */
-
-
-/*
- * Regulatory channel usage flags in EEPROM struct iwl4965_eeprom_channel.flags.
- *
- * IBSS and/or AP operation is allowed *only* on those channels with
- * (VALID && IBSS && ACTIVE && !RADAR).  This restriction is in place because
- * RADAR detection is not supported by the 4965 driver, but is a
- * requirement for establishing a new network for legal operation on channels
- * requiring RADAR detection or restricting ACTIVE scanning.
- *
- * NOTE:  "WIDE" flag does not indicate anything about "HT40" 40 MHz channels.
- *        It only indicates that 20 MHz channel use is supported; HT40 channel
- *        usage is indicated by a separate set of regulatory flags for each
- *        HT40 channel pair.
- *
- * NOTE:  Using a channel inappropriately will result in a uCode error!
- */
-#define IWL_NUM_TX_CALIB_GROUPS 5
-enum {
-        EEPROM_CHANNEL_VALID = (1 << 0),        /* usable for this SKU/geo */
-        EEPROM_CHANNEL_IBSS = (1 << 1),         /* usable as an IBSS channel */
-        /* Bit 2 Reserved */
-        EEPROM_CHANNEL_ACTIVE = (1 << 3),       /* active scanning allowed */
-        EEPROM_CHANNEL_RADAR = (1 << 4),        /* radar detection required */
-        EEPROM_CHANNEL_WIDE = (1 << 5),         /* 20 MHz channel okay */
-        /* Bit 6 Reserved (was Narrow Channel) */
-        EEPROM_CHANNEL_DFS = (1 << 7),  /* dynamic freq selection candidate */
-};
-
-/* SKU Capabilities */
-#define EEPROM_SKU_CAP_BAND_24GHZ                       (1 << 4)
-#define EEPROM_SKU_CAP_BAND_52GHZ                       (1 << 5)
-#define EEPROM_SKU_CAP_11N_ENABLE                       (1 << 6)
-#define EEPROM_SKU_CAP_AMT_ENABLE                       (1 << 7)
-#define EEPROM_SKU_CAP_IPAN_ENABLE                      (1 << 8)
-
-/* *regulatory* channel data format in eeprom, one for each channel.
- * There are separate entries for HT40 (40 MHz) vs. normal (20 MHz) channels. */
-struct iwl_eeprom_channel {
-        u8 flags;               /* EEPROM_CHANNEL_* flags copied from EEPROM */
-        s8 max_power_avg;       /* max power (dBm) on this chnl, limit 31 */
-} __packed;
-
-enum iwl_eeprom_enhanced_txpwr_flags {
-        IWL_EEPROM_ENH_TXP_FL_VALID             = BIT(0),
-        IWL_EEPROM_ENH_TXP_FL_BAND_52G          = BIT(1),
-        IWL_EEPROM_ENH_TXP_FL_OFDM              = BIT(2),
-        IWL_EEPROM_ENH_TXP_FL_40MHZ             = BIT(3),
-        IWL_EEPROM_ENH_TXP_FL_HT_AP             = BIT(4),
-        IWL_EEPROM_ENH_TXP_FL_RES1              = BIT(5),
-        IWL_EEPROM_ENH_TXP_FL_RES2              = BIT(6),
-        IWL_EEPROM_ENH_TXP_FL_COMMON_TYPE       = BIT(7),
-};
-
-/**
- * iwl_eeprom_enhanced_txpwr structure
- *    This structure presents the enhanced regulatory tx power limit layout
- *    in eeprom image
- *    Enhanced regulatory tx power portion of eeprom image can be broken down
- *    into individual structures; each one is 8 bytes in size and contain the
- *    following information
- * @flags: entry flags
- * @channel: channel number
- * @chain_a_max_pwr: chain a max power in 1/2 dBm
- * @chain_b_max_pwr: chain b max power in 1/2 dBm
- * @chain_c_max_pwr: chain c max power in 1/2 dBm
- * @delta_20_in_40: 20-in-40 deltas (hi/lo)
- * @mimo2_max_pwr: mimo2 max power in 1/2 dBm
- * @mimo3_max_pwr: mimo3 max power in 1/2 dBm
- *
- */
-struct iwl_eeprom_enhanced_txpwr {
-        u8 flags;
-        u8 channel;
-        s8 chain_a_max;
-        s8 chain_b_max;
-        s8 chain_c_max;
-        u8 delta_20_in_40;
-        s8 mimo2_max;
-        s8 mimo3_max;
-} __packed;
-
-/* calibration */
-struct iwl_eeprom_calib_hdr {
-        u8 version;
-        u8 pa_type;
-        __le16 voltage;
-} __packed;
-
-#define EEPROM_CALIB_ALL        (INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
-#define EEPROM_XTAL             ((2*0x128) | EEPROM_CALIB_ALL)
-
-/* temperature */
-#define EEPROM_KELVIN_TEMPERATURE       ((2*0x12A) | EEPROM_CALIB_ALL)
-#define EEPROM_RAW_TEMPERATURE          ((2*0x12B) | EEPROM_CALIB_ALL)
-
-
-/* agn links */
-#define EEPROM_LINK_HOST             (2*0x64)
-#define EEPROM_LINK_GENERAL          (2*0x65)
-#define EEPROM_LINK_REGULATORY       (2*0x66)
-#define EEPROM_LINK_CALIBRATION      (2*0x67)
-#define EEPROM_LINK_PROCESS_ADJST    (2*0x68)
-#define EEPROM_LINK_OTHERS           (2*0x69)
-#define EEPROM_LINK_TXP_LIMIT        (2*0x6a)
-#define EEPROM_LINK_TXP_LIMIT_SIZE   (2*0x6b)
-
-/* agn regulatory - indirect access */
-#define EEPROM_REG_BAND_1_CHANNELS       ((0x08)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 28 bytes */
-#define EEPROM_REG_BAND_2_CHANNELS       ((0x26)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 26 bytes */
-#define EEPROM_REG_BAND_3_CHANNELS       ((0x42)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 24 bytes */
-#define EEPROM_REG_BAND_4_CHANNELS       ((0x5C)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22 bytes */
-#define EEPROM_REG_BAND_5_CHANNELS       ((0x74)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 12 bytes */
-#define EEPROM_REG_BAND_24_HT40_CHANNELS  ((0x82)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
-#define EEPROM_REG_BAND_52_HT40_CHANNELS  ((0x92)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22  bytes */
-
-/* 6000 regulatory - indirect access */
-#define EEPROM_6000_REG_BAND_24_HT40_CHANNELS  ((0x80)\
-                | INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
-/* 2.4 GHz */
-extern const u8 iwl_eeprom_band_1[14];
-
-#define ADDRESS_MSK                 0x0000FFFF
-#define INDIRECT_TYPE_MSK           0x000F0000
-#define INDIRECT_HOST               0x00010000
-#define INDIRECT_GENERAL            0x00020000
-#define INDIRECT_REGULATORY         0x00030000
-#define INDIRECT_CALIBRATION        0x00040000
-#define INDIRECT_PROCESS_ADJST      0x00050000
-#define INDIRECT_OTHERS             0x00060000
-#define INDIRECT_TXP_LIMIT          0x00070000
-#define INDIRECT_TXP_LIMIT_SIZE     0x00080000
-#define INDIRECT_ADDRESS            0x00100000
-
-/* General */
-#define EEPROM_DEVICE_ID                    (2*0x08)    /* 2 bytes */
-#define EEPROM_SUBSYSTEM_ID                 (2*0x0A)    /* 2 bytes */
-#define EEPROM_MAC_ADDRESS                  (2*0x15)    /* 6  bytes */
-#define EEPROM_BOARD_REVISION               (2*0x35)    /* 2  bytes */
-#define EEPROM_BOARD_PBA_NUMBER             (2*0x3B+1)  /* 9  bytes */
-#define EEPROM_VERSION                      (2*0x44)    /* 2  bytes */
-#define EEPROM_SKU_CAP                      (2*0x45)    /* 2  bytes */
-#define EEPROM_OEM_MODE                     (2*0x46)    /* 2  bytes */
-#define EEPROM_RADIO_CONFIG                 (2*0x48)    /* 2  bytes */
-#define EEPROM_NUM_MAC_ADDRESS              (2*0x4C)    /* 2  bytes */
-
-/* The following masks are to be applied on EEPROM_RADIO_CONFIG */
-#define EEPROM_RF_CFG_TYPE_MSK(x)   (x & 0x3)         /* bits 0-1   */
-#define EEPROM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
-#define EEPROM_RF_CFG_DASH_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
-#define EEPROM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
-#define EEPROM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
-#define EEPROM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
-
-#define EEPROM_RF_CONFIG_TYPE_MAX       0x3
-
-#define EEPROM_REGULATORY_BAND_NO_HT40                  (0)
-
-struct iwl_eeprom_ops {
-        const u32 regulatory_bands[7];
-        bool enhanced_txpower;
-};
-
-
-int iwl_eeprom_init(struct iwl_priv *priv, u32 hw_rev);
-void iwl_eeprom_free(struct iwl_priv *priv);
-int iwl_eeprom_check_version(struct iwl_priv *priv);
-int iwl_eeprom_init_hw_params(struct iwl_priv *priv);
-u16 iwl_eeprom_calib_version(struct iwl_priv *priv);
-const u8 *iwl_eeprom_query_addr(struct iwl_priv *priv, size_t offset);
-u16 iwl_eeprom_query16(struct iwl_priv *priv, size_t offset);
-void iwl_eeprom_get_mac(struct iwl_priv *priv, u8 *mac);
-int iwl_init_channel_map(struct iwl_priv *priv);
-void iwl_free_channel_map(struct iwl_priv *priv);
-const struct iwl_channel_info *iwl_get_channel_info(
-                const struct iwl_priv *priv,
-                enum ieee80211_band band, u16 channel);
-void iwl_rf_config(struct iwl_priv *priv);
-
-#endif  /* __iwl_eeprom_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
index 74bce97a860..80604664174 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 ee93274214d..66c873399ab 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -65,6 +65,24 @@ void iwl_clear_bit(struct iwl_trans *trans, u32 reg, u32 mask)
 }
 EXPORT_SYMBOL_GPL(iwl_clear_bit);
 
+void iwl_set_bits_mask(struct iwl_trans *trans, u32 reg, u32 mask, u32 value)
+{
+        unsigned long flags;
+        u32 v;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+        WARN_ON_ONCE(value & ~mask);
+#endif
+
+        spin_lock_irqsave(&trans->reg_lock, flags);
+        v = iwl_read32(trans, reg);
+        v &= ~mask;
+        v |= value;
+        iwl_write32(trans, reg, v);
+        spin_unlock_irqrestore(&trans->reg_lock, flags);
+}
+EXPORT_SYMBOL_GPL(iwl_set_bits_mask);
+
 int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
                  u32 bits, u32 mask, int timeout)
 {
@@ -280,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;
@@ -290,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;
@@ -318,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
@@ -327,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 abb3250164b..50d3819739d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -54,6 +54,8 @@ static inline u32 iwl_read32(struct iwl_trans *trans, u32 ofs)
 void iwl_set_bit(struct iwl_trans *trans, u32 reg, u32 mask);
 void iwl_clear_bit(struct iwl_trans *trans, u32 reg, u32 mask);
 
+void iwl_set_bits_mask(struct iwl_trans *trans, u32 reg, u32 mask, u32 value);
+
 int iwl_poll_bit(struct iwl_trans *trans, u32 addr,
                  u32 bits, u32 mask, int timeout);
 int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
@@ -74,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-notif-wait.c b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
index 5cfed29b1b1..c61f2070f15 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
@@ -121,13 +121,12 @@ EXPORT_SYMBOL_GPL(iwl_notification_wait_notify);
 
 void iwl_abort_notification_waits(struct iwl_notif_wait_data *notif_wait)
 {
-        unsigned long flags;
         struct iwl_notification_wait *wait_entry;
 
-        spin_lock_irqsave(&notif_wait->notif_wait_lock, flags);
+        spin_lock(&notif_wait->notif_wait_lock);
         list_for_each_entry(wait_entry, &notif_wait->notif_waits, list)
                 wait_entry->aborted = true;
-        spin_unlock_irqrestore(&notif_wait->notif_wait_lock, flags);
+        spin_unlock(&notif_wait->notif_wait_lock);
 
         wake_up_all(&notif_wait->notif_waitq);
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index cec133c87ad..64886f95664 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -111,22 +111,25 @@ struct iwl_cfg;
  *      May sleep
  * @rx: Rx notification to the op_mode. rxb is the Rx buffer itself. Cmd is the
  *      HCMD the this Rx responds to.
- *      Must be atomic.
+ *      Must be atomic and called with BH disabled.
  * @queue_full: notifies that a HW queue is full.
- *      Must be atomic
+ *      Must be atomic and called with BH disabled.
  * @queue_not_full: notifies that a HW queue is not full any more.
- *      Must be atomic
+ *      Must be atomic and called with BH disabled.
  * @hw_rf_kill:notifies of a change in the HW rf kill switch. True means that
  *      the radio is killed. Must be atomic.
  * @free_skb: allows the transport layer to free skbs that haven't been
  *      reclaimed by the op_mode. This can happen when the driver is freed and
  *      there are Tx packets pending in the transport layer.
  *      Must be atomic
- * @nic_error: error notification. Must be atomic
- * @cmd_queue_full: Called when the command queue gets full. Must be atomic.
+ * @nic_error: error notification. Must be atomic and must be called with BH
+ *      disabled.
+ * @cmd_queue_full: Called when the command queue gets full. Must be atomic and
+ *      called with BH disabled.
  * @nic_config: configure NIC, called before firmware is started.
  *      May sleep
- * @wimax_active: invoked when WiMax becomes active.  Must be atomic.
+ * @wimax_active: invoked when WiMax becomes active.  Must be atomic and called
+ *      with BH disabled.
  */
 struct iwl_op_mode_ops {
         struct iwl_op_mode *(*start)(struct iwl_trans *trans,
@@ -165,7 +168,6 @@ struct iwl_op_mode {
 static inline void iwl_op_mode_stop(struct iwl_op_mode *op_mode)
 {
         might_sleep();
-
         op_mode->ops->stop(op_mode);
 }
 
@@ -221,9 +223,4 @@ static inline void iwl_op_mode_wimax_active(struct iwl_op_mode *op_mode)
         op_mode->ops->wimax_active(op_mode);
 }
 
-/*****************************************************
-* Op mode layers implementations
-******************************************************/
-extern const struct iwl_op_mode_ops iwl_dvm_ops;
-
 #endif /* __iwl_op_mode_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 3b1069290fa..9253ef1dba7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -187,7 +187,7 @@
 #define SCD_QUEUE_STTS_REG_POS_ACTIVE   (3)
 #define SCD_QUEUE_STTS_REG_POS_WSL      (4)
 #define SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN (19)
-#define SCD_QUEUE_STTS_REG_MSK          (0x00FF0000)
+#define SCD_QUEUE_STTS_REG_MSK          (0x017F0000)
 
 #define SCD_QUEUE_CTX_REG1_CREDIT_POS           (8)
 #define SCD_QUEUE_CTX_REG1_CREDIT_MSK           (0x00FFFF00)
@@ -224,6 +224,7 @@
 #define SCD_TXFACT              (SCD_BASE + 0x10)
 #define SCD_ACTIVE              (SCD_BASE + 0x14)
 #define SCD_QUEUECHAIN_SEL      (SCD_BASE + 0xe8)
+#define SCD_CHAINEXT_EN         (SCD_BASE + 0x244)
 #define SCD_AGGR_SEL            (SCD_BASE + 0x248)
 #define SCD_INTERRUPT_MASK      (SCD_BASE + 0x108)
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-test.c b/drivers/net/wireless/iwlwifi/iwl-test.c
new file mode 100644
index 00000000000..81e8c7126d7
--- /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 00000000000..e13ffa8acc0
--- /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/iwl-testmode.c b/drivers/net/wireless/iwlwifi/iwl-testmode.c
deleted file mode 100644
index 060aac3e22f..00000000000
--- a/drivers/net/wireless/iwlwifi/iwl-testmode.c
+++ /dev/null
@@ -1,1114 +0,0 @@
-/******************************************************************************
- *
- * 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/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/dma-mapping.h>
-#include <net/net_namespace.h>
-#include <linux/netdevice.h>
-#include <net/cfg80211.h>
-#include <net/mac80211.h>
-#include <net/netlink.h>
-
-#include "iwl-dev.h"
-#include "iwl-debug.h"
-#include "iwl-io.h"
-#include "iwl-agn.h"
-#include "iwl-testmode.h"
-#include "iwl-trans.h"
-#include "iwl-fh.h"
-#include "iwl-prph.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 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, },
-};
-
-/*
- * See the struct iwl_rx_packet in iwl-commands.h for the format of the
- * received events from the device
- */
-static inline int get_event_length(struct iwl_rx_cmd_buffer *rxb)
-{
-        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;
-}
-
-
-/*
- * 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)
-{
-        struct ieee80211_hw *hw = priv->hw;
-        struct sk_buff *skb;
-        void *data;
-        int length;
-
-        data = (void *)rxb_addr(rxb);
-        length = get_event_length(rxb);
-
-        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");
-}
-
-void iwl_testmode_init(struct iwl_priv *priv)
-{
-        priv->pre_rx_handler = NULL;
-        priv->testmode_trace.trace_enabled = false;
-        priv->testmode_mem.read_in_progress = false;
-}
-
-static void iwl_mem_cleanup(struct iwl_priv *priv)
-{
-        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;
-        }
-}
-
-static void iwl_trace_cleanup(struct iwl_priv *priv)
-{
-        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;
-        }
-}
-
-
-void iwl_testmode_cleanup(struct iwl_priv *priv)
-{
-        iwl_trace_cleanup(priv);
-        iwl_mem_cleanup(priv);
-}
-
-
-/*
- * 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)
-{
-        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;
-}
-
-
-/*
- * 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)
-{
-        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;
-}
-
-
-static int iwl_testmode_cfg_init_calib(struct iwl_priv *priv)
-{
-        struct iwl_notification_wait calib_wait;
-        static const u8 calib_complete[] = {
-                CALIBRATION_COMPLETE_NOTIFICATION
-        };
-        int ret;
-
-        iwl_init_notification_wait(&priv->notif_wait, &calib_wait,
-                                   calib_complete, ARRAY_SIZE(calib_complete),
-                                   NULL, NULL);
-        ret = iwl_init_alive_start(priv);
-        if (ret) {
-                IWL_ERR(priv, "Fail init calibration: %d\n", ret);
-                goto cfg_init_calib_error;
-        }
-
-        ret = iwl_wait_notification(&priv->notif_wait, &calib_wait, 2 * HZ);
-        if (ret)
-                IWL_ERR(priv, "Error detecting"
-                        " CALIBRATION_COMPLETE_NOTIFICATION: %d\n", ret);
-        return ret;
-
-cfg_init_calib_error:
-        iwl_remove_notification(&priv->notif_wait, &calib_wait);
-        return ret;
-}
-
-/*
- * This function handles the user application commands for driver.
- *
- * 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.
- *
- * If there's any message responding to the user space, IWL_TM_ATTR_SYNC_RSP
- * is used for carry the message while IWL_TM_ATTR_COMMAND must set to
- * IWL_TM_CMD_DEV2APP_SYNC_RSP.
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_driver(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        struct iwl_trans *trans = priv->trans;
-        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;
-        const struct fw_img *img;
-
-        switch (nla_get_u32(tb[IWL_TM_ATTR_COMMAND])) {
-        case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
-                rsp_data_ptr = (unsigned char *)priv->cfg->name;
-                rsp_data_len = strlen(priv->cfg->name);
-                skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-                                                        rsp_data_len + 20);
-                if (!skb) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        return -ENOMEM;
-                }
-                if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
-                                IWL_TM_CMD_DEV2APP_SYNC_RSP) ||
-                    nla_put(skb, IWL_TM_ATTR_SYNC_RSP,
-                            rsp_data_len, rsp_data_ptr))
-                        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_LOAD_INIT_FW:
-                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_INIT);
-                if (status)
-                        IWL_ERR(priv, "Error loading init ucode: %d\n", status);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
-                iwl_testmode_cfg_init_calib(priv);
-                priv->ucode_loaded = false;
-                iwl_trans_stop_device(trans);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
-                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_REGULAR);
-                if (status) {
-                        IWL_ERR(priv,
-                                "Error loading runtime ucode: %d\n", status);
-                        break;
-                }
-                status = iwl_alive_start(priv);
-                if (status)
-                        IWL_ERR(priv,
-                                "Error starting the device: %d\n", status);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_LOAD_WOWLAN_FW:
-                iwl_scan_cancel_timeout(priv, 200);
-                priv->ucode_loaded = false;
-                iwl_trans_stop_device(trans);
-                status = iwl_load_ucode_wait_alive(priv, IWL_UCODE_WOWLAN);
-                if (status) {
-                        IWL_ERR(priv,
-                                "Error loading WOWLAN ucode: %d\n", status);
-                        break;
-                }
-                status = iwl_alive_start(priv);
-                if (status)
-                        IWL_ERR(priv,
-                                "Error starting the device: %d\n", status);
-                break;
-
-        case IWL_TM_CMD_APP2DEV_GET_EEPROM:
-                if (priv->eeprom) {
-                        skb = cfg80211_testmode_alloc_reply_skb(hw->wiphy,
-                                priv->cfg->base_params->eeprom_size + 20);
-                        if (!skb) {
-                                IWL_ERR(priv, "Memory allocation fail\n");
-                                return -ENOMEM;
-                        }
-                        if (nla_put_u32(skb, IWL_TM_ATTR_COMMAND,
-                                        IWL_TM_CMD_DEV2APP_EEPROM_RSP) ||
-                            nla_put(skb, IWL_TM_ATTR_EEPROM,
-                                    priv->cfg->base_params->eeprom_size,
-                                    priv->eeprom))
-                                goto nla_put_failure;
-                        status = cfg80211_testmode_reply(skb);
-                        if (status < 0)
-                                IWL_ERR(priv, "Error sending msg : %d\n",
-                                        status);
-                } else
-                        return -EFAULT;
-                break;
-
-        case IWL_TM_CMD_APP2DEV_FIXRATE_REQ:
-                if (!tb[IWL_TM_ATTR_FIXRATE]) {
-                        IWL_ERR(priv, "Missing fixrate setting\n");
-                        return -ENOMSG;
-                }
-                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) {
-                        IWL_ERR(priv, "Memory allocation fail\n");
-                        return -ENOMEM;
-                }
-                if (!priv->ucode_loaded) {
-                        IWL_ERR(priv, "No uCode has not been loaded\n");
-                        return -EINVAL;
-                } else {
-                        img = &priv->fw->img[priv->cur_ucode];
-                        inst_size = img->sec[IWL_UCODE_SECTION_INST].len;
-                        data_size = img->sec[IWL_UCODE_SECTION_DATA].len;
-                }
-                if (nla_put_u32(skb, IWL_TM_ATTR_FW_TYPE, priv->cur_ucode) ||
-                    nla_put_u32(skb, IWL_TM_ATTR_FW_INST_SIZE, inst_size) ||
-                    nla_put_u32(skb, IWL_TM_ATTR_FW_DATA_SIZE, data_size))
-                        goto nla_put_failure;
-                status = cfg80211_testmode_reply(skb);
-                if (status < 0)
-                        IWL_ERR(priv, "Error sending msg : %d\n", status);
-                break;
-
-        default:
-                IWL_ERR(priv, "Unknown testmode driver command ID\n");
-                return -ENOSYS;
-        }
-        return status;
-
-nla_put_failure:
-        kfree_skb(skb);
-        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.
- *
- * It retrieves the mandatory fields IWL_TM_ATTR_UCODE_OWNER and
- * decide who the current owner of the uCode
- *
- * If the current owner is OWNERSHIP_TM, then the only host command
- * can deliver to uCode is from testmode, all the other host commands
- * will dropped.
- *
- * default driver is the owner of uCode in normal operational mode
- *
- * @hw: ieee80211_hw object that represents the device
- * @tb: gnl message fields from the user space
- */
-static int iwl_testmode_ownership(struct ieee80211_hw *hw, struct nlattr **tb)
-{
-        struct iwl_priv *priv = IWL_MAC80211_GET_DVM(hw);
-        u8 owner;
-
-        if (!tb[IWL_TM_ATTR_UCODE_OWNER]) {
-                IWL_ERR(priv, "Missing ucode owner\n");
-                return -ENOMSG;
-        }
-
-        owner = nla_get_u8(tb[IWL_TM_ATTR_UCODE_OWNER]);
-        if (owner == IWL_OWNERSHIP_DRIVER) {
-                priv->ucode_owner = owner;
-                priv->pre_rx_handler = NULL;
-        } else if (owner == IWL_OWNERSHIP_TM) {
-                priv->pre_rx_handler = iwl_testmode_ucode_rx_pkt;
-                priv->ucode_owner = owner;
-        } else {
-                IWL_ERR(priv, "Invalid owner\n");
-                return -EINVAL;
-        }
-        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.
- *
- * This function is invoked when there is user space application sending
- * gnl message through the testmode tunnel NL80211_CMD_TESTMODE regulated
- * by nl80211.
- *
- * It retrieves the mandatory field, IWL_TM_ATTR_COMMAND, before
- * dispatching it to the corresponding handler.
- *
- * If IWL_TM_ATTR_COMMAND is missing, -ENOMSG is replied to user application;
- * -ENOSYS is replied to the user application if the command is unknown;
- * Otherwise, the command is dispatched to the respective handler.
- *
- * @hw: ieee80211_hw object that represents the device
- * @data: pointer to user space message
- * @len: length in byte of @data
- */
-int iwlagn_mac_testmode_cmd(struct ieee80211_hw *hw, void *data, int len)
-{
-        struct nlattr *tb[IWL_TM_ATTR_MAX];
-        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);
-                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);
-                break;
-        case IWL_TM_CMD_APP2DEV_GET_DEVICENAME:
-        case IWL_TM_CMD_APP2DEV_LOAD_INIT_FW:
-        case IWL_TM_CMD_APP2DEV_CFG_INIT_CALIB:
-        case IWL_TM_CMD_APP2DEV_LOAD_RUNTIME_FW:
-        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);
-        return result;
-}
-
-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;
-
-        if (cb->args[3]) {
-                /* 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);
-                        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;
-        }
-
-        mutex_unlock(&priv->mutex);
-        return result;
-}
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 0aeeb7ce91c..00efde8e553 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -355,10 +355,10 @@ struct iwl_trans;
  *      Must be atomic
  * @reclaim: free packet until ssn. Returns a list of freed packets.
  *      Must be atomic
- * @tx_agg_setup: setup a tx queue for AMPDU - will be called once the HW is
+ * @txq_enable: setup a tx queue for AMPDU - will be called once the HW is
  *      ready and a successful ADDBA response has been received.
  *      May sleep
- * @tx_agg_disable: de-configure a Tx queue to send AMPDUs
+ * @txq_disable: de-configure a Tx queue to send AMPDUs
  *      Must be atomic
  * @wait_tx_queue_empty: wait until all tx queues are empty
  *      May sleep
@@ -391,9 +391,9 @@ struct iwl_trans_ops {
         void (*reclaim)(struct iwl_trans *trans, int queue, int ssn,
                         struct sk_buff_head *skbs);
 
-        void (*tx_agg_setup)(struct iwl_trans *trans, int queue, int fifo,
-                             int sta_id, int tid, int frame_limit, u16 ssn);
-        void (*tx_agg_disable)(struct iwl_trans *trans, int queue);
+        void (*txq_enable)(struct iwl_trans *trans, int queue, int fifo,
+                           int sta_id, int tid, int frame_limit, u16 ssn);
+        void (*txq_disable)(struct iwl_trans *trans, int queue);
 
         int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
         int (*wait_tx_queue_empty)(struct iwl_trans *trans);
@@ -433,6 +433,11 @@ enum iwl_trans_state {
  * @hw_id_str: a string with info about HW ID. Set during transport allocation.
  * @pm_support: set to true in start_hw if link pm is supported
  * @wait_command_queue: the wait_queue for SYNC host commands
+ * @dev_cmd_pool: pool for Tx cmd allocation - for internal use only.
+ *      The user should use iwl_trans_{alloc,free}_tx_cmd.
+ * @dev_cmd_headroom: room needed for the transport's private use before the
+ *      device_cmd for Tx - for internal use only
+ *      The user should use iwl_trans_{alloc,free}_tx_cmd.
  */
 struct iwl_trans {
         const struct iwl_trans_ops *ops;
@@ -450,6 +455,10 @@ struct iwl_trans {
 
         wait_queue_head_t wait_command_queue;
 
+        /* The following fields are internal only */
+        struct kmem_cache *dev_cmd_pool;
+        size_t dev_cmd_headroom;
+
         /* pointer to trans specific struct */
         /*Ensure that this pointer will always be aligned to sizeof pointer */
         char trans_specific[0] __aligned(sizeof(void *));
@@ -525,6 +534,26 @@ static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
         return trans->ops->send_cmd(trans, cmd);
 }
 
+static inline struct iwl_device_cmd *
+iwl_trans_alloc_tx_cmd(struct iwl_trans *trans)
+{
+        u8 *dev_cmd_ptr = kmem_cache_alloc(trans->dev_cmd_pool, GFP_ATOMIC);
+
+        if (unlikely(dev_cmd_ptr == NULL))
+                return NULL;
+
+        return (struct iwl_device_cmd *)
+                        (dev_cmd_ptr + trans->dev_cmd_headroom);
+}
+
+static inline void iwl_trans_free_tx_cmd(struct iwl_trans *trans,
+                                         struct iwl_device_cmd *dev_cmd)
+{
+        u8 *dev_cmd_ptr = (u8 *)dev_cmd - trans->dev_cmd_headroom;
+
+        kmem_cache_free(trans->dev_cmd_pool, dev_cmd_ptr);
+}
+
 static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
                                struct iwl_device_cmd *dev_cmd, int queue)
 {
@@ -543,24 +572,24 @@ static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
         trans->ops->reclaim(trans, queue, ssn, skbs);
 }
 
-static inline void iwl_trans_tx_agg_disable(struct iwl_trans *trans, int queue)
+static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue)
 {
         WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
                   "%s bad state = %d", __func__, trans->state);
 
-        trans->ops->tx_agg_disable(trans, queue);
+        trans->ops->txq_disable(trans, queue);
 }
 
-static inline void iwl_trans_tx_agg_setup(struct iwl_trans *trans, int queue,
-                                          int fifo, int sta_id, int tid,
-                                          int frame_limit, u16 ssn)
+static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
+                                        int fifo, int sta_id, int tid,
+                                        int frame_limit, u16 ssn)
 {
         might_sleep();
 
         WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
                   "%s bad state = %d", __func__, trans->state);
 
-        trans->ops->tx_agg_setup(trans, queue, fifo, sta_id, tid,
+        trans->ops->txq_enable(trans, queue, fifo, sta_id, tid,
                                  frame_limit, ssn);
 }
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/pcie/1000.c
index 2629a6602df..81b83f484f0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/1000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-csr.h"
 #include "iwl-agn-hw.h"
+#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL1000_UCODE_API_MAX 5
@@ -64,13 +64,26 @@ static const struct iwl_base_params iwl1000_base_params = {
         .support_ct_kill_exit = true,
         .plcp_delta_threshold = IWL_MAX_PLCP_ERR_EXT_LONG_THRESHOLD_DEF,
         .chain_noise_scale = 1000,
-        .wd_timeout = IWL_WATCHHDOG_DISABLED,
+        .wd_timeout = IWL_WATCHDOG_DISABLED,
         .max_event_log_size = 128,
 };
 
 static const struct iwl_ht_params iwl1000_ht_params = {
         .ht_greenfield_support = true,
         .use_rts_for_aggregation = true, /* use rts/cts protection */
+        .ht40_bands = BIT(IEEE80211_BAND_2GHZ),
+};
+
+static const struct iwl_eeprom_params iwl1000_eeprom_params = {
+        .regulatory_bands = {
+                EEPROM_REG_BAND_1_CHANNELS,
+                EEPROM_REG_BAND_2_CHANNELS,
+                EEPROM_REG_BAND_3_CHANNELS,
+                EEPROM_REG_BAND_4_CHANNELS,
+                EEPROM_REG_BAND_5_CHANNELS,
+                EEPROM_REG_BAND_24_HT40_CHANNELS,
+                EEPROM_REGULATORY_BAND_NO_HT40,
+        }
 };
 
 #define IWL_DEVICE_1000                                         \
@@ -84,6 +97,7 @@ static const struct iwl_ht_params iwl1000_ht_params = {
         .eeprom_ver = EEPROM_1000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,       \
         .base_params = &iwl1000_base_params,                    \
+        .eeprom_params = &iwl1000_eeprom_params,                \
         .led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl1000_bgn_cfg = {
@@ -108,6 +122,7 @@ const struct iwl_cfg iwl1000_bg_cfg = {
         .eeprom_ver = EEPROM_1000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_1000_TX_POWER_VERSION,       \
         .base_params = &iwl1000_base_params,                    \
+        .eeprom_params = &iwl1000_eeprom_params,                \
         .led_mode = IWL_LED_RF_STATE,                           \
         .rx_with_siso_diversity = true
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/pcie/2000.c
index 8133105ac64..fd4e78f56fa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/2000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
-#include "iwl-commands.h" /* needed for BT for now */
+#include "cfg.h"
+#include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL2030_UCODE_API_MAX 6
@@ -104,6 +104,7 @@ static const struct iwl_base_params iwl2030_base_params = {
 static const struct iwl_ht_params iwl2000_ht_params = {
         .ht_greenfield_support = true,
         .use_rts_for_aggregation = true, /* use rts/cts protection */
+        .ht40_bands = BIT(IEEE80211_BAND_2GHZ),
 };
 
 static const struct iwl_bt_params iwl2030_bt_params = {
@@ -116,6 +117,19 @@ static const struct iwl_bt_params iwl2030_bt_params = {
         .bt_session_2 = true,
 };
 
+static const struct iwl_eeprom_params iwl20x0_eeprom_params = {
+        .regulatory_bands = {
+                EEPROM_REG_BAND_1_CHANNELS,
+                EEPROM_REG_BAND_2_CHANNELS,
+                EEPROM_REG_BAND_3_CHANNELS,
+                EEPROM_REG_BAND_4_CHANNELS,
+                EEPROM_REG_BAND_5_CHANNELS,
+                EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+                EEPROM_REGULATORY_BAND_NO_HT40,
+        },
+        .enhanced_txpower = true,
+};
+
 #define IWL_DEVICE_2000                                         \
         .fw_name_pre = IWL2000_FW_PRE,                          \
         .ucode_api_max = IWL2000_UCODE_API_MAX,                 \
@@ -127,6 +141,7 @@ static const struct iwl_bt_params iwl2030_bt_params = {
         .eeprom_ver = EEPROM_2000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,       \
         .base_params = &iwl2000_base_params,                    \
+        .eeprom_params = &iwl20x0_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .temp_offset_v2 = true,                                 \
         .led_mode = IWL_LED_RF_STATE
@@ -155,6 +170,7 @@ const struct iwl_cfg iwl2000_2bgn_d_cfg = {
         .eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,       \
         .base_params = &iwl2030_base_params,                    \
         .bt_params = &iwl2030_bt_params,                        \
+        .eeprom_params = &iwl20x0_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .temp_offset_v2 = true,                                 \
         .led_mode = IWL_LED_RF_STATE,                           \
@@ -177,6 +193,7 @@ const struct iwl_cfg iwl2030_2bgn_cfg = {
         .eeprom_ver = EEPROM_2000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,       \
         .base_params = &iwl2000_base_params,                    \
+        .eeprom_params = &iwl20x0_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .temp_offset_v2 = true,                                 \
         .led_mode = IWL_LED_RF_STATE,                           \
@@ -207,6 +224,7 @@ const struct iwl_cfg iwl105_bgn_d_cfg = {
         .eeprom_calib_ver = EEPROM_2000_TX_POWER_VERSION,       \
         .base_params = &iwl2030_base_params,                    \
         .bt_params = &iwl2030_bt_params,                        \
+        .eeprom_params = &iwl20x0_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .temp_offset_v2 = true,                                 \
         .led_mode = IWL_LED_RF_STATE,                           \
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/pcie/5000.c
index 8e26bc825f2..d1665fa6d15 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/5000.c
@@ -27,9 +27,9 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
 #include "iwl-csr.h"
+#include "cfg.h"
 
 /* Highest firmware API version supported */
 #define IWL5000_UCODE_API_MAX 5
@@ -62,13 +62,26 @@ static const struct iwl_base_params iwl5000_base_params = {
         .led_compensation = 51,
         .plcp_delta_threshold = IWL_MAX_PLCP_ERR_LONG_THRESHOLD_DEF,
         .chain_noise_scale = 1000,
-        .wd_timeout = IWL_WATCHHDOG_DISABLED,
+        .wd_timeout = IWL_WATCHDOG_DISABLED,
         .max_event_log_size = 512,
         .no_idle_support = true,
 };
 
 static const struct iwl_ht_params iwl5000_ht_params = {
         .ht_greenfield_support = true,
+        .ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
+};
+
+static const struct iwl_eeprom_params iwl5000_eeprom_params = {
+        .regulatory_bands = {
+                EEPROM_REG_BAND_1_CHANNELS,
+                EEPROM_REG_BAND_2_CHANNELS,
+                EEPROM_REG_BAND_3_CHANNELS,
+                EEPROM_REG_BAND_4_CHANNELS,
+                EEPROM_REG_BAND_5_CHANNELS,
+                EEPROM_REG_BAND_24_HT40_CHANNELS,
+                EEPROM_REG_BAND_52_HT40_CHANNELS
+        },
 };
 
 #define IWL_DEVICE_5000                                         \
@@ -82,6 +95,7 @@ static const struct iwl_ht_params iwl5000_ht_params = {
         .eeprom_ver = EEPROM_5000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_5000_TX_POWER_VERSION,       \
         .base_params = &iwl5000_base_params,                    \
+        .eeprom_params = &iwl5000_eeprom_params,                \
         .led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl5300_agn_cfg = {
@@ -128,6 +142,7 @@ const struct iwl_cfg iwl5350_agn_cfg = {
         .eeprom_ver = EEPROM_5050_EEPROM_VERSION,
         .eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,
         .base_params = &iwl5000_base_params,
+        .eeprom_params = &iwl5000_eeprom_params,
         .ht_params = &iwl5000_ht_params,
         .led_mode = IWL_LED_BLINK,
         .internal_wimax_coex = true,
@@ -144,6 +159,7 @@ const struct iwl_cfg iwl5350_agn_cfg = {
         .eeprom_ver = EEPROM_5050_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_5050_TX_POWER_VERSION,       \
         .base_params = &iwl5000_base_params,                    \
+        .eeprom_params = &iwl5000_eeprom_params,                \
         .no_xtal_calib = true,                                  \
         .led_mode = IWL_LED_BLINK,                              \
         .internal_wimax_coex = true
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/pcie/6000.c
index 19f7ee84ae8..4a57624afc4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/pcie/6000.c
@@ -27,25 +27,28 @@
 #include <linux/module.h>
 #include <linux/stringify.h>
 #include "iwl-config.h"
-#include "iwl-cfg.h"
 #include "iwl-agn-hw.h"
-#include "iwl-commands.h" /* needed for BT for now */
+#include "cfg.h"
+#include "dvm/commands.h" /* needed for BT for now */
 
 /* Highest firmware API version supported */
 #define IWL6000_UCODE_API_MAX 6
 #define IWL6050_UCODE_API_MAX 5
 #define IWL6000G2_UCODE_API_MAX 6
+#define IWL6035_UCODE_API_MAX 6
 
 /* Oldest version we won't warn about */
 #define IWL6000_UCODE_API_OK 4
 #define IWL6000G2_UCODE_API_OK 5
 #define IWL6050_UCODE_API_OK 5
 #define IWL6000G2B_UCODE_API_OK 6
+#define IWL6035_UCODE_API_OK 6
 
 /* Lowest firmware API version supported */
 #define IWL6000_UCODE_API_MIN 4
 #define IWL6050_UCODE_API_MIN 4
-#define IWL6000G2_UCODE_API_MIN 4
+#define IWL6000G2_UCODE_API_MIN 5
+#define IWL6035_UCODE_API_MIN 6
 
 /* EEPROM versions */
 #define EEPROM_6000_TX_POWER_VERSION    (4)
@@ -124,6 +127,7 @@ static const struct iwl_base_params iwl6000_g2_base_params = {
 static const struct iwl_ht_params iwl6000_ht_params = {
         .ht_greenfield_support = true,
         .use_rts_for_aggregation = true, /* use rts/cts protection */
+        .ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
 };
 
 static const struct iwl_bt_params iwl6000_bt_params = {
@@ -135,6 +139,19 @@ static const struct iwl_bt_params iwl6000_bt_params = {
         .bt_sco_disable = true,
 };
 
+static const struct iwl_eeprom_params iwl6000_eeprom_params = {
+        .regulatory_bands = {
+                EEPROM_REG_BAND_1_CHANNELS,
+                EEPROM_REG_BAND_2_CHANNELS,
+                EEPROM_REG_BAND_3_CHANNELS,
+                EEPROM_REG_BAND_4_CHANNELS,
+                EEPROM_REG_BAND_5_CHANNELS,
+                EEPROM_6000_REG_BAND_24_HT40_CHANNELS,
+                EEPROM_REG_BAND_52_HT40_CHANNELS
+        },
+        .enhanced_txpower = true,
+};
+
 #define IWL_DEVICE_6005                                         \
         .fw_name_pre = IWL6005_FW_PRE,                          \
         .ucode_api_max = IWL6000G2_UCODE_API_MAX,               \
@@ -146,6 +163,7 @@ static const struct iwl_bt_params iwl6000_bt_params = {
         .eeprom_ver = EEPROM_6005_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_6005_TX_POWER_VERSION,       \
         .base_params = &iwl6000_g2_base_params,                 \
+        .eeprom_params = &iwl6000_eeprom_params,                \
         .need_temp_offset_calib = true,                         \
         .led_mode = IWL_LED_RF_STATE
 
@@ -201,6 +219,7 @@ const struct iwl_cfg iwl6005_2agn_mow2_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                                          \
@@ -227,9 +246,26 @@ const struct iwl_cfg iwl6030_2bg_cfg = {
         IWL_DEVICE_6030,
 };
 
+#define IWL_DEVICE_6035                                         \
+        .fw_name_pre = IWL6030_FW_PRE,                          \
+        .ucode_api_max = IWL6035_UCODE_API_MAX,                 \
+        .ucode_api_ok = IWL6035_UCODE_API_OK,                   \
+        .ucode_api_min = IWL6035_UCODE_API_MIN,                 \
+        .device_family = IWL_DEVICE_FAMILY_6030,                \
+        .max_inst_size = IWL60_RTC_INST_SIZE,                   \
+        .max_data_size = IWL60_RTC_DATA_SIZE,                   \
+        .eeprom_ver = EEPROM_6030_EEPROM_VERSION,               \
+        .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
+
 const struct iwl_cfg iwl6035_2agn_cfg = {
         .name = "Intel(R) Centrino(R) Advanced-N 6235 AGN",
-        IWL_DEVICE_6030,
+        IWL_DEVICE_6035,
         .ht_params = &iwl6000_ht_params,
 };
 
@@ -273,6 +309,7 @@ const struct iwl_cfg iwl130_bg_cfg = {
         .eeprom_ver = EEPROM_6000_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,       \
         .base_params = &iwl6000_base_params,                    \
+        .eeprom_params = &iwl6000_eeprom_params,                \
         .led_mode = IWL_LED_BLINK
 
 const struct iwl_cfg iwl6000i_2agn_cfg = {
@@ -303,6 +340,7 @@ const struct iwl_cfg iwl6000i_2bg_cfg = {
         .eeprom_ver = EEPROM_6050_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_6050_TX_POWER_VERSION,       \
         .base_params = &iwl6050_base_params,                    \
+        .eeprom_params = &iwl6000_eeprom_params,                \
         .led_mode = IWL_LED_BLINK,                              \
         .internal_wimax_coex = true
 
@@ -327,6 +365,7 @@ const struct iwl_cfg iwl6050_2abg_cfg = {
         .eeprom_ver = EEPROM_6150_EEPROM_VERSION,               \
         .eeprom_calib_ver = EEPROM_6150_TX_POWER_VERSION,       \
         .base_params = &iwl6050_base_params,                    \
+        .eeprom_params = &iwl6000_eeprom_params,                \
         .led_mode = IWL_LED_BLINK,                              \
         .internal_wimax_coex = true
 
@@ -353,6 +392,7 @@ const struct iwl_cfg iwl6000_3agn_cfg = {
         .eeprom_ver = EEPROM_6000_EEPROM_VERSION,
         .eeprom_calib_ver = EEPROM_6000_TX_POWER_VERSION,
         .base_params = &iwl6000_base_params,
+        .eeprom_params = &iwl6000_eeprom_params,
         .ht_params = &iwl6000_ht_params,
         .led_mode = IWL_LED_BLINK,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-cfg.h b/drivers/net/wireless/iwlwifi/pcie/cfg.h
index 82152311d73..82152311d73 100644
--- a/drivers/net/wireless/iwlwifi/iwl-cfg.h
+++ b/drivers/net/wireless/iwlwifi/pcie/cfg.h
diff --git a/drivers/net/wireless/iwlwifi/iwl-pci.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 0c8a1c2d884..f4c3500b68c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-pci.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -68,10 +68,11 @@
 #include <linux/pci-aspm.h>
 
 #include "iwl-trans.h"
-#include "iwl-cfg.h"
 #include "iwl-drv.h"
 #include "iwl-trans.h"
-#include "iwl-trans-pcie-int.h"
+
+#include "cfg.h"
+#include "internal.h"
 
 #define IWL_PCI_DEVICE(dev, subdev, cfg) \
         .vendor = PCI_VENDOR_ID_INTEL,  .device = (dev), \
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index f027769933d..5024fb662bf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-int.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -339,13 +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_tx_agg_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_tx_agg_setup(struct iwl_trans *trans, int queue, 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/iwl-trans-pcie-rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 98605fc7ad3..be143eb4aa4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -32,7 +32,7 @@
 
 #include "iwl-prph.h"
 #include "iwl-io.h"
-#include "iwl-trans-pcie-int.h"
+#include "internal.h"
 #include "iwl-op-mode.h"
 
 #ifdef CONFIG_IWLWIFI_IDI
@@ -867,24 +867,23 @@ void iwl_disable_ict(struct iwl_trans *trans)
         spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
 }
 
+/* legacy (non-ICT) ISR. Assumes that trans_pcie->irq_lock is held */
 static irqreturn_t iwl_isr(int irq, void *data)
 {
         struct iwl_trans *trans = data;
-        struct iwl_trans_pcie *trans_pcie;
+        struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
         u32 inta, inta_mask;
-        unsigned long flags;
 #ifdef CONFIG_IWLWIFI_DEBUG
         u32 inta_fh;
 #endif
+
+        lockdep_assert_held(&trans_pcie->irq_lock);
+
         if (!trans)
                 return IRQ_NONE;
 
         trace_iwlwifi_dev_irq(trans->dev);
 
-        trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-
-        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
-
         /* Disable (but don't clear!) interrupts here to avoid
          *    back-to-back ISRs and sporadic interrupts from our NIC.
          * If we have something to service, the tasklet will re-enable ints.
@@ -907,7 +906,7 @@ static irqreturn_t iwl_isr(int irq, void *data)
                 /* Hardware disappeared. It might have already raised
                  * an interrupt */
                 IWL_WARN(trans, "HARDWARE GONE?? INTA == 0x%08x\n", inta);
-                goto unplugged;
+                return IRQ_HANDLED;
         }
 
 #ifdef CONFIG_IWLWIFI_DEBUG
@@ -926,18 +925,13 @@ static irqreturn_t iwl_isr(int irq, void *data)
                  !trans_pcie->inta)
                 iwl_enable_interrupts(trans);
 
- unplugged:
-        spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
-        return IRQ_HANDLED;
-
- none:
+none:
         /* re-enable interrupts here since we don't have anything to service. */
         /* only Re-enable if disabled by irq  and no schedules tasklet. */
         if (test_bit(STATUS_INT_ENABLED, &trans_pcie->status) &&
             !trans_pcie->inta)
                 iwl_enable_interrupts(trans);
 
-        spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
         return IRQ_NONE;
 }
 
@@ -963,15 +957,19 @@ irqreturn_t iwl_isr_ict(int irq, void *data)
 
         trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
+        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+
         /* dram interrupt table not set yet,
          * use legacy interrupt.
          */
-        if (!trans_pcie->use_ict)
-                return iwl_isr(irq, data);
+        if (unlikely(!trans_pcie->use_ict)) {
+                irqreturn_t ret = iwl_isr(irq, data);
+                spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+                return ret;
+        }
 
         trace_iwlwifi_dev_irq(trans->dev);
 
-        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
 
         /* Disable (but don't clear!) interrupts here to avoid
          * back-to-back ISRs and sporadic interrupts from our NIC.
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 1e50401023e..42f369d15f4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -70,13 +70,12 @@
 
 #include "iwl-drv.h"
 #include "iwl-trans.h"
-#include "iwl-trans-pcie-int.h"
 #include "iwl-csr.h"
 #include "iwl-prph.h"
-#include "iwl-eeprom.h"
 #include "iwl-agn-hw.h"
+#include "internal.h"
 /* FIXME: need to abstract out TX command (once we know what it looks like) */
-#include "iwl-commands.h"
+#include "dvm/commands.h"
 
 #define SCD_QUEUECHAIN_SEL_ALL(trans, trans_pcie)       \
         (((1<<trans->cfg->base_params->num_of_queues) - 1) &\
@@ -297,8 +296,13 @@ static void iwlagn_free_dma_ptr(struct iwl_trans *trans,
 static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
 {
         struct iwl_tx_queue *txq = (void *)data;
+        struct iwl_queue *q = &txq->q;
         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 */
@@ -308,15 +312,48 @@ 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)));
+        }
+
+        for (i = q->read_ptr; i != q->write_ptr;
+             i = iwl_queue_inc_wrap(i, q->n_bd)) {
+                struct iwl_tx_cmd *tx_cmd =
+                        (struct iwl_tx_cmd *)txq->entries[i].cmd->payload;
+                IWL_ERR(trans, "scratch %d = 0x%08x\n", i,
+                        get_unaligned_le32(&tx_cmd->scratch));
+        }
 
         iwl_op_mode_nic_error(trans->op_mode);
 }
@@ -1009,15 +1046,12 @@ static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
 
 /*
  * Activate/Deactivate Tx DMA/FIFO channels according tx fifos mask
- * must be called under the irq lock and with MAC access
  */
 static void iwl_trans_txq_set_sched(struct iwl_trans *trans, u32 mask)
 {
         struct iwl_trans_pcie __maybe_unused *trans_pcie =
                 IWL_TRANS_GET_PCIE_TRANS(trans);
 
-        lockdep_assert_held(&trans_pcie->irq_lock);
-
         iwl_write_prph(trans, SCD_TXFACT, mask);
 }
 
@@ -1025,11 +1059,12 @@ static void iwl_tx_start(struct iwl_trans *trans)
 {
         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
         u32 a;
-        unsigned long flags;
         int i, chan;
         u32 reg_val;
 
-        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
+        /* make sure all queue are not stopped/used */
+        memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
+        memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
 
         trans_pcie->scd_base_addr =
                 iwl_read_prph(trans, SCD_SRAM_BASE_ADDR);
@@ -1051,60 +1086,31 @@ static void iwl_tx_start(struct iwl_trans *trans)
         iwl_write_prph(trans, SCD_DRAM_BASE_ADDR,
                        trans_pcie->scd_bc_tbls.dma >> 10);
 
-        /* Enable DMA channel */
-        for (chan = 0; chan < FH_TCSR_CHNL_NUM ; chan++)
-                iwl_write_direct32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(chan),
-                                FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
-                                FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE);
-
-        /* Update FH chicken bits */
-        reg_val = iwl_read_direct32(trans, FH_TX_CHICKEN_BITS_REG);
-        iwl_write_direct32(trans, FH_TX_CHICKEN_BITS_REG,
-                           reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
+        /* 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);
 
-        iwl_write_prph(trans, SCD_QUEUECHAIN_SEL,
-                       SCD_QUEUECHAIN_SEL_ALL(trans, trans_pcie));
-        iwl_write_prph(trans, SCD_AGGR_SEL, 0);
+        for (i = 0; i < trans_pcie->n_q_to_fifo; i++) {
+                int fifo = trans_pcie->setup_q_to_fifo[i];
 
-        /* initiate the queues */
-        for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
-                iwl_write_prph(trans, SCD_QUEUE_RDPTR(i), 0);
-                iwl_write_direct32(trans, HBUS_TARG_WRPTR, 0 | (i << 8));
-                iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
-                                SCD_CONTEXT_QUEUE_OFFSET(i), 0);
-                iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
-                                SCD_CONTEXT_QUEUE_OFFSET(i) +
-                                sizeof(u32),
-                                ((SCD_WIN_SIZE <<
-                                SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
-                                SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
-                                ((SCD_FRAME_LIMIT <<
-                                SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
-                                SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+                iwl_trans_pcie_txq_enable(trans, i, fifo, IWL_INVALID_STATION,
+                                          IWL_TID_NON_QOS, SCD_FRAME_LIMIT, 0);
         }
 
-        iwl_write_prph(trans, SCD_INTERRUPT_MASK,
-                       IWL_MASK(0, trans->cfg->base_params->num_of_queues));
-
         /* Activate all Tx DMA/FIFO channels */
         iwl_trans_txq_set_sched(trans, IWL_MASK(0, 7));
 
-        iwl_trans_set_wr_ptrs(trans, trans_pcie->cmd_queue, 0);
-
-        /* make sure all queue are not stopped/used */
-        memset(trans_pcie->queue_stopped, 0, sizeof(trans_pcie->queue_stopped));
-        memset(trans_pcie->queue_used, 0, sizeof(trans_pcie->queue_used));
-
-        for (i = 0; i < trans_pcie->n_q_to_fifo; i++) {
-                int fifo = trans_pcie->setup_q_to_fifo[i];
-
-                set_bit(i, trans_pcie->queue_used);
-
-                iwl_trans_tx_queue_set_status(trans, &trans_pcie->txq[i],
-                                              fifo, true);
-        }
+        /* Enable DMA channel */
+        for (chan = 0; chan < FH_TCSR_CHNL_NUM ; chan++)
+                iwl_write_direct32(trans, FH_TCSR_CHNL_TX_CONFIG_REG(chan),
+                                   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+                                   FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE);
 
-        spin_unlock_irqrestore(&trans_pcie->irq_lock, flags);
+        /* Update FH chicken bits */
+        reg_val = iwl_read_direct32(trans, FH_TX_CHICKEN_BITS_REG);
+        iwl_write_direct32(trans, FH_TX_CHICKEN_BITS_REG,
+                           reg_val | FH_TX_CHICKEN_BITS_SCD_AUTO_RETRY_EN);
 
         /* Enable L1-Active */
         iwl_clear_bits_prph(trans, APMG_PCIDEV_STT_REG,
@@ -1261,6 +1267,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;
@@ -1354,7 +1373,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 */
@@ -1556,6 +1576,7 @@ void iwl_trans_pcie_free(struct iwl_trans *trans)
         iounmap(trans_pcie->hw_base);
         pci_release_regions(trans_pcie->pci_dev);
         pci_disable_device(trans_pcie->pci_dev);
+        kmem_cache_destroy(trans->dev_cmd_pool);
 
         kfree(trans);
 }
@@ -1997,7 +2018,9 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
         if (!trans->op_mode)
                 return -EAGAIN;
 
+        local_bh_disable();
         iwl_op_mode_nic_error(trans->op_mode);
+        local_bh_enable();
 
         return count;
 }
@@ -2046,8 +2069,8 @@ static const struct iwl_trans_ops trans_ops_pcie = {
         .tx = iwl_trans_pcie_tx,
         .reclaim = iwl_trans_pcie_reclaim,
 
-        .tx_agg_disable = iwl_trans_pcie_tx_agg_disable,
-        .tx_agg_setup = iwl_trans_pcie_tx_agg_setup,
+        .txq_disable = iwl_trans_pcie_txq_disable,
+        .txq_enable = iwl_trans_pcie_txq_enable,
 
         .dbgfs_register = iwl_trans_pcie_dbgfs_register,
 
@@ -2070,6 +2093,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 {
         struct iwl_trans_pcie *trans_pcie;
         struct iwl_trans *trans;
+        char cmd_pool_name[100];
         u16 pci_cmd;
         int err;
 
@@ -2166,8 +2190,25 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
         init_waitqueue_head(&trans->wait_command_queue);
         spin_lock_init(&trans->reg_lock);
 
+        snprintf(cmd_pool_name, sizeof(cmd_pool_name), "iwl_cmd_pool:%s",
+                 dev_name(trans->dev));
+
+        trans->dev_cmd_headroom = 0;
+        trans->dev_cmd_pool =
+                kmem_cache_create(cmd_pool_name,
+                                  sizeof(struct iwl_device_cmd)
+                                  + trans->dev_cmd_headroom,
+                                  sizeof(void *),
+                                  SLAB_HWCACHE_ALIGN,
+                                  NULL);
+
+        if (!trans->dev_cmd_pool)
+                goto out_pci_disable_msi;
+
         return trans;
 
+out_pci_disable_msi:
+        pci_disable_msi(pdev);
 out_pci_release_regions:
         pci_release_regions(pdev);
 out_pci_disable_device:
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 6f601cd05a9..6baf8deef51 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans-pcie-tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -34,11 +34,10 @@
 #include "iwl-csr.h"
 #include "iwl-prph.h"
 #include "iwl-io.h"
-#include "iwl-agn-hw.h"
 #include "iwl-op-mode.h"
-#include "iwl-trans-pcie-int.h"
+#include "internal.h"
 /* FIXME: need to abstract out TX command (once we know what it looks like) */
-#include "iwl-commands.h"
+#include "dvm/commands.h"
 
 #define IWL_TX_CRC_SIZE 4
 #define IWL_TX_DELIMITER_SIZE 4
@@ -381,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;
@@ -406,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. */
@@ -416,101 +415,87 @@ 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_tx_agg_setup(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);
-        unsigned long flags;
-        u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
         if (test_and_set_bit(txq_id, trans_pcie->queue_used))
                 WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
 
-        spin_lock_irqsave(&trans_pcie->irq_lock, flags);
-
         /* 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)
+                iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
 
-        /* Map receiver-address / traffic-ID to this queue */
-        iwlagn_tx_queue_set_q2ratid(trans, ra_tid, txq_id);
+        /* If this queue is mapped to a certain station: it is an AGG queue */
+        if (sta_id != IWL_INVALID_STATION) {
+                u16 ra_tid = BUILD_RAxTID(sta_id, tid);
 
-        /* Set this queue as a chain-building queue */
-        iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
+                /* Map receiver-address / traffic-ID to this queue */
+                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));
+                /* 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 +
+                        SCD_CONTEXT_QUEUE_OFFSET(txq_id), 0);
+        iwl_write_targ_mem(trans, trans_pcie->scd_base_addr +
                         SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
                         ((frame_limit << SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
                                 SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
                         ((frame_limit << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
                                 SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
 
-        iwl_set_bits_prph(trans, SCD_INTERRUPT_MASK, (1 << txq_id));
-
         /* 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);
-
-        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_tx_agg_disable(struct iwl_trans *trans, int txq_id)
+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));
-
-        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);
+        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));
 
-        iwl_clear_bits_prph(trans, SCD_INTERRUPT_MASK, BIT(txq_id));
+        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/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index eacfe020d0b..5bddf53ece1 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -1553,6 +1553,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
                         hdr = (struct ieee80211_hdr *) skb->data;
                         mac80211_hwsim_monitor_ack(data2->hw, hdr->addr2);
                 }
+                txi->flags |= IEEE80211_TX_STAT_ACK;
         }
         ieee80211_tx_status_irqsafe(data2->hw, skb);
         return 0;
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 015fec3371a..9c2e08e4b09 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -170,7 +170,9 @@ mwifiex_cfg80211_set_default_key(struct wiphy *wiphy, struct net_device *netdev,
         if (!priv->sec_info.wep_enabled)
                 return 0;
 
-        if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
+        if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP) {
+                priv->wep_key_curr_index = key_index;
+        } else if (mwifiex_set_encode(priv, NULL, 0, key_index, NULL, 0)) {
                 wiphy_err(wiphy, "set default Tx key index\n");
                 return -EFAULT;
         }
@@ -187,9 +189,25 @@ mwifiex_cfg80211_add_key(struct wiphy *wiphy, struct net_device *netdev,
                          struct key_params *params)
 {
         struct mwifiex_private *priv = mwifiex_netdev_get_priv(netdev);
+        struct mwifiex_wep_key *wep_key;
         const u8 bc_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
         const u8 *peer_mac = pairwise ? mac_addr : bc_mac;
 
+        if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP &&
+            (params->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+             params->cipher == WLAN_CIPHER_SUITE_WEP104)) {
+                if (params->key && params->key_len) {
+                        wep_key = &priv->wep_key[key_index];
+                        memset(wep_key, 0, sizeof(struct mwifiex_wep_key));
+                        memcpy(wep_key->key_material, params->key,
+                               params->key_len);
+                        wep_key->key_index = key_index;
+                        wep_key->key_length = params->key_len;
+                        priv->sec_info.wep_enabled = 1;
+                }
+                return 0;
+        }
+
         if (mwifiex_set_encode(priv, params->key, params->key_len,
                                key_index, peer_mac, 0)) {
                 wiphy_err(wiphy, "crypto keys added\n");
@@ -384,13 +402,13 @@ mwifiex_set_rf_channel(struct mwifiex_private *priv,
         cfp.freq = chan->center_freq;
         cfp.channel = ieee80211_frequency_to_channel(chan->center_freq);
 
-        if (mwifiex_bss_set_channel(priv, &cfp))
-                return -EFAULT;
-
-        if (priv->bss_type == MWIFIEX_BSS_TYPE_STA)
+        if (priv->bss_type == MWIFIEX_BSS_TYPE_STA) {
+                if (mwifiex_bss_set_channel(priv, &cfp))
+                        return -EFAULT;
                 return mwifiex_drv_change_adhoc_chan(priv, cfp.channel);
-        else
-                return mwifiex_uap_set_channel(priv, cfp.channel);
+        }
+
+        return 0;
 }
 
 /*
@@ -961,12 +979,25 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
                 return -EINVAL;
         }
 
+        bss_cfg->channel =
+            (u8)ieee80211_frequency_to_channel(params->channel->center_freq);
+        bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
+
+        if (mwifiex_set_rf_channel(priv, params->channel,
+                                   params->channel_type)) {
+                kfree(bss_cfg);
+                wiphy_err(wiphy, "Failed to set band config information!\n");
+                return -1;
+        }
+
         if (mwifiex_set_secure_params(priv, bss_cfg, params)) {
                 kfree(bss_cfg);
                 wiphy_err(wiphy, "Failed to parse secuirty parameters!\n");
                 return -1;
         }
 
+        mwifiex_set_ht_params(priv, bss_cfg, params);
+
         if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_UAP_BSS_STOP,
                                   HostCmd_ACT_GEN_SET, 0, NULL)) {
                 wiphy_err(wiphy, "Failed to stop the BSS\n");
@@ -990,6 +1021,16 @@ static int mwifiex_cfg80211_start_ap(struct wiphy *wiphy,
                 return -1;
         }
 
+        if (priv->sec_info.wep_enabled)
+                priv->curr_pkt_filter |= HostCmd_ACT_MAC_WEP_ENABLE;
+        else
+                priv->curr_pkt_filter &= ~HostCmd_ACT_MAC_WEP_ENABLE;
+
+        if (mwifiex_send_cmd_sync(priv, HostCmd_CMD_MAC_CONTROL,
+                                  HostCmd_ACT_GEN_SET, 0,
+                                  &priv->curr_pkt_filter))
+                return -1;
+
         return 0;
 }
 
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 51e023ec1de..ea37b887a87 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -1102,7 +1102,8 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
                 &resp->params.opt_hs_cfg;
         uint32_t conditions = le32_to_cpu(phs_cfg->params.hs_config.conditions);
 
-        if (phs_cfg->action == cpu_to_le16(HS_ACTIVATE)) {
+        if (phs_cfg->action == cpu_to_le16(HS_ACTIVATE) &&
+            adapter->iface_type == MWIFIEX_SDIO) {
                 mwifiex_hs_activated_event(priv, true);
                 return 0;
         } else {
@@ -1114,6 +1115,9 @@ int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
         }
         if (conditions != HOST_SLEEP_CFG_CANCEL) {
                 adapter->is_hs_configured = true;
+                if (adapter->iface_type == MWIFIEX_USB ||
+                    adapter->iface_type == MWIFIEX_PCIE)
+                        mwifiex_hs_activated_event(priv, true);
         } else {
                 adapter->is_hs_configured = false;
                 if (adapter->hs_activated)
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 561452a5c81..1184141839a 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -124,6 +124,7 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_UAP_DTIM_PERIOD    (PROPRIETARY_TLV_BASE_ID + 45)
 #define TLV_TYPE_UAP_BCAST_SSID     (PROPRIETARY_TLV_BASE_ID + 48)
 #define TLV_TYPE_UAP_RTS_THRESHOLD  (PROPRIETARY_TLV_BASE_ID + 51)
+#define TLV_TYPE_UAP_WEP_KEY        (PROPRIETARY_TLV_BASE_ID + 59)
 #define TLV_TYPE_UAP_WPA_PASSPHRASE (PROPRIETARY_TLV_BASE_ID + 60)
 #define TLV_TYPE_UAP_ENCRY_PROTOCOL (PROPRIETARY_TLV_BASE_ID + 64)
 #define TLV_TYPE_UAP_AKMP           (PROPRIETARY_TLV_BASE_ID + 65)
@@ -162,6 +163,12 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 
 #define ISSUPP_11NENABLED(FwCapInfo) (FwCapInfo & BIT(11))
 
+#define MWIFIEX_DEF_HT_CAP      (IEEE80211_HT_CAP_DSSSCCK40 | \
+                                 (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT) | \
+                                 IEEE80211_HT_CAP_SM_PS)
+
+#define MWIFIEX_DEF_AMPDU       IEEE80211_HT_AMPDU_PARM_FACTOR
+
 /* dev_cap bitmap
  * BIT
  * 0-16         reserved
@@ -1195,6 +1202,13 @@ struct host_cmd_tlv_passphrase {
         u8 passphrase[0];
 } __packed;
 
+struct host_cmd_tlv_wep_key {
+        struct host_cmd_tlv tlv;
+        u8 key_index;
+        u8 is_default;
+        u8 key[1];
+};
+
 struct host_cmd_tlv_auth_type {
         struct host_cmd_tlv tlv;
         u8 auth_type;
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/mwifiex/ie.c
index ceb82cd749c..328fb14d9e1 100644
--- a/drivers/net/wireless/mwifiex/ie.c
+++ b/drivers/net/wireless/mwifiex/ie.c
@@ -224,29 +224,46 @@ int mwifiex_set_mgmt_ies(struct mwifiex_private *priv,
                          struct cfg80211_ap_settings *params)
 {
         struct mwifiex_ie *beacon_ie = NULL, *pr_ie = NULL;
-        struct mwifiex_ie *ar_ie = NULL, *rsn_ie = NULL;
-        struct ieee_types_header *ie = NULL;
+        struct mwifiex_ie *ar_ie = NULL, *gen_ie = NULL;
+        struct ieee_types_header *rsn_ie = NULL, *wpa_ie = NULL;
         u16 beacon_idx = MWIFIEX_AUTO_IDX_MASK, pr_idx = MWIFIEX_AUTO_IDX_MASK;
         u16 ar_idx = MWIFIEX_AUTO_IDX_MASK, rsn_idx = MWIFIEX_AUTO_IDX_MASK;
-        u16 mask;
+        u16 mask, ie_len = 0;
+        const u8 *vendor_ie;
         int ret = 0;
 
         if (params->beacon.tail && params->beacon.tail_len) {
-                ie = (void *)cfg80211_find_ie(WLAN_EID_RSN, params->beacon.tail,
-                                              params->beacon.tail_len);
-                if (ie) {
-                        rsn_ie = kmalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
-                        if (!rsn_ie)
-                                return -ENOMEM;
-
-                        rsn_ie->ie_index = cpu_to_le16(rsn_idx);
-                        mask = MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
-                               MGMT_MASK_ASSOC_RESP;
-                        rsn_ie->mgmt_subtype_mask = cpu_to_le16(mask);
-                        rsn_ie->ie_length = cpu_to_le16(ie->len + 2);
-                        memcpy(rsn_ie->ie_buffer, ie, ie->len + 2);
-
-                        if (mwifiex_update_uap_custom_ie(priv, rsn_ie, &rsn_idx,
+                gen_ie = kzalloc(sizeof(struct mwifiex_ie), GFP_KERNEL);
+                if (!gen_ie)
+                        return -ENOMEM;
+                gen_ie->ie_index = cpu_to_le16(rsn_idx);
+                mask = MGMT_MASK_BEACON | MGMT_MASK_PROBE_RESP |
+                       MGMT_MASK_ASSOC_RESP;
+                gen_ie->mgmt_subtype_mask = cpu_to_le16(mask);
+
+                rsn_ie = (void *)cfg80211_find_ie(WLAN_EID_RSN,
+                                                  params->beacon.tail,
+                                                  params->beacon.tail_len);
+                if (rsn_ie) {
+                        memcpy(gen_ie->ie_buffer, rsn_ie, rsn_ie->len + 2);
+                        ie_len = rsn_ie->len + 2;
+                        gen_ie->ie_length = cpu_to_le16(ie_len);
+                }
+
+                vendor_ie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
+                                                    WLAN_OUI_TYPE_MICROSOFT_WPA,
+                                                    params->beacon.tail,
+                                                    params->beacon.tail_len);
+                if (vendor_ie) {
+                        wpa_ie = (struct ieee_types_header *)vendor_ie;
+                        memcpy(gen_ie->ie_buffer + ie_len,
+                               wpa_ie, wpa_ie->len + 2);
+                        ie_len += wpa_ie->len + 2;
+                        gen_ie->ie_length = cpu_to_le16(ie_len);
+                }
+
+                if (rsn_ie || wpa_ie) {
+                        if (mwifiex_update_uap_custom_ie(priv, gen_ie, &rsn_idx,
                                                          NULL, NULL,
                                                          NULL, NULL)) {
                                 ret = -1;
@@ -319,7 +336,7 @@ done:
         kfree(beacon_ie);
         kfree(pr_ie);
         kfree(ar_ie);
-        kfree(rsn_ie);
+        kfree(gen_ie);
 
         return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index c1cb004db91..b543a4d82ff 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -57,6 +57,69 @@ static int mwifiex_add_bss_prio_tbl(struct mwifiex_private *priv)
         return 0;
 }
 
+static void scan_delay_timer_fn(unsigned long data)
+{
+        struct mwifiex_private *priv = (struct mwifiex_private *)data;
+        struct mwifiex_adapter *adapter = priv->adapter;
+        struct cmd_ctrl_node *cmd_node, *tmp_node;
+        unsigned long flags;
+
+        if (!mwifiex_wmm_lists_empty(adapter)) {
+                if (adapter->scan_delay_cnt == MWIFIEX_MAX_SCAN_DELAY_CNT) {
+                        /*
+                         * Abort scan operation by cancelling all pending scan
+                         * command
+                         */
+                        spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
+                        list_for_each_entry_safe(cmd_node, tmp_node,
+                                                 &adapter->scan_pending_q,
+                                                 list) {
+                                list_del(&cmd_node->list);
+                                cmd_node->wait_q_enabled = false;
+                                mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+                        }
+                        spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+                                               flags);
+
+                        spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+                        adapter->scan_processing = false;
+                        spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock,
+                                               flags);
+
+                        if (priv->user_scan_cfg) {
+                                dev_dbg(priv->adapter->dev,
+                                        "info: %s: scan aborted\n", __func__);
+                                cfg80211_scan_done(priv->scan_request, 1);
+                                priv->scan_request = NULL;
+                                kfree(priv->user_scan_cfg);
+                                priv->user_scan_cfg = NULL;
+                        }
+                } else {
+                        /*
+                         * Tx data queue is still not empty, delay scan
+                         * operation further by 20msec.
+                         */
+                        mod_timer(&priv->scan_delay_timer, jiffies +
+                                  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
+                        adapter->scan_delay_cnt++;
+                }
+                queue_work(priv->adapter->workqueue, &priv->adapter->main_work);
+        } else {
+                /*
+                 * Tx data queue is empty. Get scan command from scan_pending_q
+                 * and put to cmd_pending_q to resume scan operation
+                 */
+                adapter->scan_delay_cnt = 0;
+                spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
+                cmd_node = list_first_entry(&adapter->scan_pending_q,
+                                            struct cmd_ctrl_node, list);
+                list_del(&cmd_node->list);
+                spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
+
+                mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
+        }
+}
+
 /*
  * This function initializes the private structure and sets default
  * values to the members.
@@ -136,6 +199,9 @@ static int mwifiex_init_priv(struct mwifiex_private *priv)
 
         priv->scan_block = false;
 
+        setup_timer(&priv->scan_delay_timer, scan_delay_timer_fn,
+                    (unsigned long)priv);
+
         return mwifiex_add_bss_prio_tbl(priv);
 }
 
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/mwifiex/ioctl.h
index e6be6ee7595..9f088fb88cb 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/mwifiex/ioctl.h
@@ -21,6 +21,7 @@
 #define _MWIFIEX_IOCTL_H_
 
 #include <net/mac80211.h>
+#include <net/lib80211.h>
 
 enum {
         MWIFIEX_SCAN_TYPE_UNCHANGED = 0,
@@ -71,6 +72,13 @@ struct wpa_param {
         u8 passphrase[MWIFIEX_WPA_PASSHPHRASE_LEN];
 };
 
+struct wep_key {
+        u8 key_index;
+        u8 is_default;
+        u16 length;
+        u8 key[WLAN_KEY_LEN_WEP104];
+};
+
 #define KEY_MGMT_ON_HOST        0x03
 #define MWIFIEX_AUTH_MODE_AUTO  0xFF
 #define BAND_CONFIG_MANUAL      0x00
@@ -90,6 +98,8 @@ struct mwifiex_uap_bss_param {
         u16 key_mgmt;
         u16 key_mgmt_operation;
         struct wpa_param wpa_cfg;
+        struct wep_key wep_cfg[NUM_WEP_KEYS];
+        struct ieee80211_ht_cap ht_cap;
 };
 
 enum {
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index 3192855c31c..f0219efc895 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -190,7 +190,8 @@ process_start:
                             adapter->tx_lock_flag)
                                 break;
 
-                        if (adapter->scan_processing || adapter->data_sent ||
+                        if ((adapter->scan_processing &&
+                             !adapter->scan_delay_cnt) || adapter->data_sent ||
                             mwifiex_wmm_lists_empty(adapter)) {
                                 if (adapter->cmd_sent || adapter->curr_cmd ||
                                     (!is_command_pending(adapter)))
@@ -244,8 +245,8 @@ process_start:
                         }
                 }
 
-                if (!adapter->scan_processing && !adapter->data_sent &&
-                    !mwifiex_wmm_lists_empty(adapter)) {
+                if ((!adapter->scan_processing || adapter->scan_delay_cnt) &&
+                    !adapter->data_sent && !mwifiex_wmm_lists_empty(adapter)) {
                         mwifiex_wmm_process_tx(adapter);
                         if (adapter->hs_activated) {
                                 adapter->is_hs_configured = false;
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index bd3b0bf94b9..0b3b5aa9830 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -79,14 +79,17 @@ enum {
 
 #define SCAN_BEACON_ENTRY_PAD                   6
 
-#define MWIFIEX_PASSIVE_SCAN_CHAN_TIME  200
-#define MWIFIEX_ACTIVE_SCAN_CHAN_TIME   200
-#define MWIFIEX_SPECIFIC_SCAN_CHAN_TIME 110
+#define MWIFIEX_PASSIVE_SCAN_CHAN_TIME  110
+#define MWIFIEX_ACTIVE_SCAN_CHAN_TIME   30
+#define MWIFIEX_SPECIFIC_SCAN_CHAN_TIME 30
 
 #define SCAN_RSSI(RSSI)                                 (0x100 - ((u8)(RSSI)))
 
 #define MWIFIEX_MAX_TOTAL_SCAN_TIME     (MWIFIEX_TIMER_10S - MWIFIEX_TIMER_1S)
 
+#define MWIFIEX_MAX_SCAN_DELAY_CNT                      50
+#define MWIFIEX_SCAN_DELAY_MSEC                         20
+
 #define RSN_GTK_OUI_OFFSET                              2
 
 #define MWIFIEX_OUI_NOT_PRESENT                 0
@@ -482,6 +485,7 @@ struct mwifiex_private {
         u16 proberesp_idx;
         u16 assocresp_idx;
         u16 rsn_idx;
+        struct timer_list scan_delay_timer;
 };
 
 enum mwifiex_ba_status {
@@ -686,6 +690,7 @@ struct mwifiex_adapter {
         struct completion fw_load;
         u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
         u16 max_mgmt_ie_index;
+        u8 scan_delay_cnt;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -835,6 +840,9 @@ void mwifiex_init_priv_params(struct mwifiex_private *priv,
 int mwifiex_set_secure_params(struct mwifiex_private *priv,
                               struct mwifiex_uap_bss_param *bss_config,
                               struct cfg80211_ap_settings *params);
+void mwifiex_set_ht_params(struct mwifiex_private *priv,
+                           struct mwifiex_uap_bss_param *bss_cfg,
+                           struct cfg80211_ap_settings *params);
 
 /*
  * This function checks if the queuing is RA based or not.
@@ -985,7 +993,6 @@ int mwifiex_set_tx_power(struct mwifiex_private *priv,
 
 int mwifiex_main_process(struct mwifiex_adapter *);
 
-int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel);
 int mwifiex_bss_set_channel(struct mwifiex_private *,
                             struct mwifiex_chan_freq_power *cfp);
 int mwifiex_get_bss_info(struct mwifiex_private *,
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index 74f04571572..efaf26ccd6b 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -28,7 +28,10 @@
 /* The maximum number of channels the firmware can scan per command */
 #define MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN   14
 
-#define MWIFIEX_CHANNELS_PER_SCAN_CMD            4
+#define MWIFIEX_DEF_CHANNELS_PER_SCAN_CMD       4
+#define MWIFIEX_LIMIT_1_CHANNEL_PER_SCAN_CMD    15
+#define MWIFIEX_LIMIT_2_CHANNELS_PER_SCAN_CMD   27
+#define MWIFIEX_LIMIT_3_CHANNELS_PER_SCAN_CMD   35
 
 /* Memory needed to store a max sized Channel List TLV for a firmware scan */
 #define CHAN_TLV_MAX_SIZE  (sizeof(struct mwifiex_ie_types_header)         \
@@ -471,7 +474,7 @@ mwifiex_is_network_compatible(struct mwifiex_private *priv,
  * This routine is used for any scan that is not provided with a
  * specific channel list to scan.
  */
-static void
+static int
 mwifiex_scan_create_channel_list(struct mwifiex_private *priv,
                                  const struct mwifiex_user_scan_cfg
                                                         *user_scan_in,
@@ -528,6 +531,7 @@ mwifiex_scan_create_channel_list(struct mwifiex_private *priv,
                 }
 
         }
+        return chan_idx;
 }
 
 /*
@@ -727,6 +731,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
         u32 num_probes;
         u32 ssid_len;
         u32 chan_idx;
+        u32 chan_num;
         u32 scan_type;
         u16 scan_dur;
         u8 channel;
@@ -850,7 +855,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
         if (*filtered_scan)
                 *max_chan_per_scan = MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN;
         else
-                *max_chan_per_scan = MWIFIEX_CHANNELS_PER_SCAN_CMD;
+                *max_chan_per_scan = MWIFIEX_DEF_CHANNELS_PER_SCAN_CMD;
 
         /* If the input config or adapter has the number of Probes set,
            add tlv */
@@ -962,13 +967,28 @@ mwifiex_config_scan(struct mwifiex_private *priv,
                         dev_dbg(adapter->dev,
                                 "info: Scan: Scanning current channel only\n");
                 }
-
+                chan_num = chan_idx;
         } else {
                 dev_dbg(adapter->dev,
                         "info: Scan: Creating full region channel list\n");
-                mwifiex_scan_create_channel_list(priv, user_scan_in,
-                                                 scan_chan_list,
-                                                 *filtered_scan);
+                chan_num = mwifiex_scan_create_channel_list(priv, user_scan_in,
+                                                            scan_chan_list,
+                                                            *filtered_scan);
+        }
+
+        /*
+         * In associated state we will reduce the number of channels scanned per
+         * scan command to avoid any traffic delay/loss. This number is decided
+         * based on total number of channels to be scanned due to constraints
+         * of command buffers.
+         */
+        if (priv->media_connected) {
+                if (chan_num < MWIFIEX_LIMIT_1_CHANNEL_PER_SCAN_CMD)
+                        *max_chan_per_scan = 1;
+                else if (chan_num < MWIFIEX_LIMIT_2_CHANNELS_PER_SCAN_CMD)
+                        *max_chan_per_scan = 2;
+                else if (chan_num < MWIFIEX_LIMIT_3_CHANNELS_PER_SCAN_CMD)
+                        *max_chan_per_scan = 3;
         }
 }
 
@@ -1772,14 +1792,23 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
                         priv->user_scan_cfg = NULL;
                 }
         } else {
-                /* Get scan command from scan_pending_q and put to
-                   cmd_pending_q */
-                cmd_node = list_first_entry(&adapter->scan_pending_q,
-                                            struct cmd_ctrl_node, list);
-                list_del(&cmd_node->list);
-                spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
-
-                mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
+                if (!mwifiex_wmm_lists_empty(adapter)) {
+                        spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+                                               flags);
+                        adapter->scan_delay_cnt = 1;
+                        mod_timer(&priv->scan_delay_timer, jiffies +
+                                  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
+                } else {
+                        /* Get scan command from scan_pending_q and put to
+                           cmd_pending_q */
+                        cmd_node = list_first_entry(&adapter->scan_pending_q,
+                                                    struct cmd_ctrl_node, list);
+                        list_del(&cmd_node->list);
+                        spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+                                               flags);
+                        mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
+                                                        true);
+                }
         }
 
 done:
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index 8173ab66066..f40e93fe894 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -26,6 +26,18 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
                               struct mwifiex_uap_bss_param *bss_config,
                               struct cfg80211_ap_settings *params) {
         int i;
+        struct mwifiex_wep_key wep_key;
+
+        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:
@@ -54,7 +66,7 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
                         }
                         if (params->crypto.wpa_versions &
                             NL80211_WPA_VERSION_2) {
-                                bss_config->protocol = PROTOCOL_WPA2;
+                                bss_config->protocol |= PROTOCOL_WPA2;
                                 bss_config->key_mgmt = KEY_MGMT_EAP;
                         }
                         break;
@@ -66,7 +78,7 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
                         }
                         if (params->crypto.wpa_versions &
                             NL80211_WPA_VERSION_2) {
-                                bss_config->protocol = PROTOCOL_WPA2;
+                                bss_config->protocol |= PROTOCOL_WPA2;
                                 bss_config->key_mgmt = KEY_MGMT_PSK;
                         }
                         break;
@@ -80,10 +92,19 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
                 case WLAN_CIPHER_SUITE_WEP104:
                         break;
                 case WLAN_CIPHER_SUITE_TKIP:
-                        bss_config->wpa_cfg.pairwise_cipher_wpa = CIPHER_TKIP;
+                        if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                                bss_config->wpa_cfg.pairwise_cipher_wpa |=
+                                                                CIPHER_TKIP;
+                        if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                                bss_config->wpa_cfg.pairwise_cipher_wpa2 |=
+                                                                CIPHER_TKIP;
                         break;
                 case WLAN_CIPHER_SUITE_CCMP:
-                        bss_config->wpa_cfg.pairwise_cipher_wpa2 =
+                        if (params->crypto.wpa_versions & NL80211_WPA_VERSION_1)
+                                bss_config->wpa_cfg.pairwise_cipher_wpa |=
+                                                                CIPHER_AES_CCMP;
+                        if (params->crypto.wpa_versions & NL80211_WPA_VERSION_2)
+                                bss_config->wpa_cfg.pairwise_cipher_wpa2 |=
                                                                 CIPHER_AES_CCMP;
                 default:
                         break;
@@ -93,6 +114,27 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
         switch (params->crypto.cipher_group) {
         case WLAN_CIPHER_SUITE_WEP40:
         case WLAN_CIPHER_SUITE_WEP104:
+                if (priv->sec_info.wep_enabled) {
+                        bss_config->protocol = PROTOCOL_STATIC_WEP;
+                        bss_config->key_mgmt = KEY_MGMT_NONE;
+                        bss_config->wpa_cfg.length = 0;
+
+                        for (i = 0; i < NUM_WEP_KEYS; i++) {
+                                wep_key = priv->wep_key[i];
+                                bss_config->wep_cfg[i].key_index = i;
+
+                                if (priv->wep_key_curr_index == i)
+                                        bss_config->wep_cfg[i].is_default = 1;
+                                else
+                                        bss_config->wep_cfg[i].is_default = 0;
+
+                                bss_config->wep_cfg[i].length =
+                                                             wep_key.key_length;
+                                memcpy(&bss_config->wep_cfg[i].key,
+                                       &wep_key.key_material,
+                                       wep_key.key_length);
+                        }
+                }
                 break;
         case WLAN_CIPHER_SUITE_TKIP:
                 bss_config->wpa_cfg.group_cipher = CIPHER_TKIP;
@@ -107,6 +149,33 @@ int mwifiex_set_secure_params(struct mwifiex_private *priv,
         return 0;
 }
 
+/* This function updates 11n related parameters from IE and sets them into
+ * bss_config structure.
+ */
+void
+mwifiex_set_ht_params(struct mwifiex_private *priv,
+                      struct mwifiex_uap_bss_param *bss_cfg,
+                      struct cfg80211_ap_settings *params)
+{
+        const u8 *ht_ie;
+
+        if (!ISSUPP_11NENABLED(priv->adapter->fw_cap_info))
+                return;
+
+        ht_ie = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY, params->beacon.tail,
+                                 params->beacon.tail_len);
+        if (ht_ie) {
+                memcpy(&bss_cfg->ht_cap, ht_ie + 2,
+                       sizeof(struct ieee80211_ht_cap));
+        } else {
+                memset(&bss_cfg->ht_cap , 0, sizeof(struct ieee80211_ht_cap));
+                bss_cfg->ht_cap.cap_info = cpu_to_le16(MWIFIEX_DEF_HT_CAP);
+                bss_cfg->ht_cap.ampdu_params_info = MWIFIEX_DEF_AMPDU;
+        }
+
+        return;
+}
+
 /* This function initializes some of mwifiex_uap_bss_param variables.
  * This helps FW in ignoring invalid values. These values may or may not
  * be get updated to valid ones at later stage.
@@ -124,6 +193,120 @@ void mwifiex_set_sys_config_invalid_data(struct mwifiex_uap_bss_param *config)
 }
 
 /* This function parses BSS related parameters from structure
+ * and prepares TLVs specific to WPA/WPA2 security.
+ * These TLVs are appended to command buffer.
+ */
+static void
+mwifiex_uap_bss_wpa(u8 **tlv_buf, void *cmd_buf, u16 *param_size)
+{
+        struct host_cmd_tlv_pwk_cipher *pwk_cipher;
+        struct host_cmd_tlv_gwk_cipher *gwk_cipher;
+        struct host_cmd_tlv_passphrase *passphrase;
+        struct host_cmd_tlv_akmp *tlv_akmp;
+        struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+        u16 cmd_size = *param_size;
+        u8 *tlv = *tlv_buf;
+
+        tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
+        tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
+        tlv_akmp->tlv.len = cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
+                                        sizeof(struct host_cmd_tlv));
+        tlv_akmp->key_mgmt_operation = cpu_to_le16(bss_cfg->key_mgmt_operation);
+        tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
+        cmd_size += sizeof(struct host_cmd_tlv_akmp);
+        tlv += sizeof(struct host_cmd_tlv_akmp);
+
+        if (bss_cfg->wpa_cfg.pairwise_cipher_wpa & VALID_CIPHER_BITMAP) {
+                pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+                pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+                pwk_cipher->tlv.len =
+                        cpu_to_le16(sizeof(struct host_cmd_tlv_pwk_cipher) -
+                                    sizeof(struct host_cmd_tlv));
+                pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
+                pwk_cipher->cipher = bss_cfg->wpa_cfg.pairwise_cipher_wpa;
+                cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+                tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+        }
+
+        if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 & VALID_CIPHER_BITMAP) {
+                pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
+                pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
+                pwk_cipher->tlv.len =
+                        cpu_to_le16(sizeof(struct host_cmd_tlv_pwk_cipher) -
+                                    sizeof(struct host_cmd_tlv));
+                pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
+                pwk_cipher->cipher = bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
+                cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
+                tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
+        }
+
+        if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
+                gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
+                gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
+                gwk_cipher->tlv.len =
+                        cpu_to_le16(sizeof(struct host_cmd_tlv_gwk_cipher) -
+                                    sizeof(struct host_cmd_tlv));
+                gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
+                cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
+                tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
+        }
+
+        if (bss_cfg->wpa_cfg.length) {
+                passphrase = (struct host_cmd_tlv_passphrase *)tlv;
+                passphrase->tlv.type = cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
+                passphrase->tlv.len = cpu_to_le16(bss_cfg->wpa_cfg.length);
+                memcpy(passphrase->passphrase, bss_cfg->wpa_cfg.passphrase,
+                       bss_cfg->wpa_cfg.length);
+                cmd_size += sizeof(struct host_cmd_tlv) +
+                            bss_cfg->wpa_cfg.length;
+                tlv += sizeof(struct host_cmd_tlv) + bss_cfg->wpa_cfg.length;
+        }
+
+        *param_size = cmd_size;
+        *tlv_buf = tlv;
+
+        return;
+}
+
+/* This function parses BSS related parameters from structure
+ * and prepares TLVs specific to WEP encryption.
+ * These TLVs are appended to command buffer.
+ */
+static void
+mwifiex_uap_bss_wep(u8 **tlv_buf, void *cmd_buf, u16 *param_size)
+{
+        struct host_cmd_tlv_wep_key *wep_key;
+        u16 cmd_size = *param_size;
+        int i;
+        u8 *tlv = *tlv_buf;
+        struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
+
+        for (i = 0; i < NUM_WEP_KEYS; i++) {
+                if (bss_cfg->wep_cfg[i].length &&
+                    (bss_cfg->wep_cfg[i].length == WLAN_KEY_LEN_WEP40 ||
+                     bss_cfg->wep_cfg[i].length == WLAN_KEY_LEN_WEP104)) {
+                        wep_key = (struct host_cmd_tlv_wep_key *)tlv;
+                        wep_key->tlv.type = cpu_to_le16(TLV_TYPE_UAP_WEP_KEY);
+                        wep_key->tlv.len =
+                                cpu_to_le16(bss_cfg->wep_cfg[i].length + 2);
+                        wep_key->key_index = bss_cfg->wep_cfg[i].key_index;
+                        wep_key->is_default = bss_cfg->wep_cfg[i].is_default;
+                        memcpy(wep_key->key, bss_cfg->wep_cfg[i].key,
+                               bss_cfg->wep_cfg[i].length);
+                        cmd_size += sizeof(struct host_cmd_tlv) + 2 +
+                                    bss_cfg->wep_cfg[i].length;
+                        tlv += sizeof(struct host_cmd_tlv) + 2 +
+                                    bss_cfg->wep_cfg[i].length;
+                }
+        }
+
+        *param_size = cmd_size;
+        *tlv_buf = tlv;
+
+        return;
+}
+
+/* This function parses BSS related parameters from structure
  * and prepares TLVs. These TLVs are appended to command buffer.
 */
 static int
@@ -137,12 +320,9 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
         struct host_cmd_tlv_frag_threshold *frag_threshold;
         struct host_cmd_tlv_rts_threshold *rts_threshold;
         struct host_cmd_tlv_retry_limit *retry_limit;
-        struct host_cmd_tlv_pwk_cipher *pwk_cipher;
-        struct host_cmd_tlv_gwk_cipher *gwk_cipher;
         struct host_cmd_tlv_encrypt_protocol *encrypt_protocol;
         struct host_cmd_tlv_auth_type *auth_type;
-        struct host_cmd_tlv_passphrase *passphrase;
-        struct host_cmd_tlv_akmp *tlv_akmp;
+        struct mwifiex_ie_types_htcap *htcap;
         struct mwifiex_uap_bss_param *bss_cfg = cmd_buf;
         u16 cmd_size = *param_size;
 
@@ -232,70 +412,11 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
         }
         if ((bss_cfg->protocol & PROTOCOL_WPA) ||
             (bss_cfg->protocol & PROTOCOL_WPA2) ||
-            (bss_cfg->protocol & PROTOCOL_EAP)) {
-                tlv_akmp = (struct host_cmd_tlv_akmp *)tlv;
-                tlv_akmp->tlv.type = cpu_to_le16(TLV_TYPE_UAP_AKMP);
-                tlv_akmp->tlv.len =
-                    cpu_to_le16(sizeof(struct host_cmd_tlv_akmp) -
-                                sizeof(struct host_cmd_tlv));
-                tlv_akmp->key_mgmt_operation =
-                        cpu_to_le16(bss_cfg->key_mgmt_operation);
-                tlv_akmp->key_mgmt = cpu_to_le16(bss_cfg->key_mgmt);
-                cmd_size += sizeof(struct host_cmd_tlv_akmp);
-                tlv += sizeof(struct host_cmd_tlv_akmp);
-
-                if (bss_cfg->wpa_cfg.pairwise_cipher_wpa &
-                                VALID_CIPHER_BITMAP) {
-                        pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
-                        pwk_cipher->tlv.type =
-                                cpu_to_le16(TLV_TYPE_PWK_CIPHER);
-                        pwk_cipher->tlv.len = cpu_to_le16(
-                                sizeof(struct host_cmd_tlv_pwk_cipher) -
-                                sizeof(struct host_cmd_tlv));
-                        pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA);
-                        pwk_cipher->cipher =
-                                bss_cfg->wpa_cfg.pairwise_cipher_wpa;
-                        cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
-                        tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
-                }
-                if (bss_cfg->wpa_cfg.pairwise_cipher_wpa2 &
-                                VALID_CIPHER_BITMAP) {
-                        pwk_cipher = (struct host_cmd_tlv_pwk_cipher *)tlv;
-                        pwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_PWK_CIPHER);
-                        pwk_cipher->tlv.len = cpu_to_le16(
-                                sizeof(struct host_cmd_tlv_pwk_cipher) -
-                                sizeof(struct host_cmd_tlv));
-                        pwk_cipher->proto = cpu_to_le16(PROTOCOL_WPA2);
-                        pwk_cipher->cipher =
-                                bss_cfg->wpa_cfg.pairwise_cipher_wpa2;
-                        cmd_size += sizeof(struct host_cmd_tlv_pwk_cipher);
-                        tlv += sizeof(struct host_cmd_tlv_pwk_cipher);
-                }
-                if (bss_cfg->wpa_cfg.group_cipher & VALID_CIPHER_BITMAP) {
-                        gwk_cipher = (struct host_cmd_tlv_gwk_cipher *)tlv;
-                        gwk_cipher->tlv.type = cpu_to_le16(TLV_TYPE_GWK_CIPHER);
-                        gwk_cipher->tlv.len = cpu_to_le16(
-                                sizeof(struct host_cmd_tlv_gwk_cipher) -
-                                sizeof(struct host_cmd_tlv));
-                        gwk_cipher->cipher = bss_cfg->wpa_cfg.group_cipher;
-                        cmd_size += sizeof(struct host_cmd_tlv_gwk_cipher);
-                        tlv += sizeof(struct host_cmd_tlv_gwk_cipher);
-                }
-                if (bss_cfg->wpa_cfg.length) {
-                        passphrase = (struct host_cmd_tlv_passphrase *)tlv;
-                        passphrase->tlv.type =
-                                cpu_to_le16(TLV_TYPE_UAP_WPA_PASSPHRASE);
-                        passphrase->tlv.len =
-                                cpu_to_le16(bss_cfg->wpa_cfg.length);
-                        memcpy(passphrase->passphrase,
-                               bss_cfg->wpa_cfg.passphrase,
-                               bss_cfg->wpa_cfg.length);
-                        cmd_size += sizeof(struct host_cmd_tlv) +
-                                    bss_cfg->wpa_cfg.length;
-                        tlv += sizeof(struct host_cmd_tlv) +
-                               bss_cfg->wpa_cfg.length;
-                }
-        }
+            (bss_cfg->protocol & PROTOCOL_EAP))
+                mwifiex_uap_bss_wpa(&tlv, cmd_buf, &cmd_size);
+        else
+                mwifiex_uap_bss_wep(&tlv, cmd_buf, &cmd_size);
+
         if ((bss_cfg->auth_mode <= WLAN_AUTH_SHARED_KEY) ||
             (bss_cfg->auth_mode == MWIFIEX_AUTH_MODE_AUTO)) {
                 auth_type = (struct host_cmd_tlv_auth_type *)tlv;
@@ -319,6 +440,25 @@ mwifiex_uap_bss_param_prepare(u8 *tlv, void *cmd_buf, u16 *param_size)
                 tlv += sizeof(struct host_cmd_tlv_encrypt_protocol);
         }
 
+        if (bss_cfg->ht_cap.cap_info) {
+                htcap = (struct mwifiex_ie_types_htcap *)tlv;
+                htcap->header.type = cpu_to_le16(WLAN_EID_HT_CAPABILITY);
+                htcap->header.len =
+                                cpu_to_le16(sizeof(struct ieee80211_ht_cap));
+                htcap->ht_cap.cap_info = bss_cfg->ht_cap.cap_info;
+                htcap->ht_cap.ampdu_params_info =
+                                             bss_cfg->ht_cap.ampdu_params_info;
+                memcpy(&htcap->ht_cap.mcs, &bss_cfg->ht_cap.mcs,
+                       sizeof(struct ieee80211_mcs_info));
+                htcap->ht_cap.extended_ht_cap_info =
+                                        bss_cfg->ht_cap.extended_ht_cap_info;
+                htcap->ht_cap.tx_BF_cap_info = bss_cfg->ht_cap.tx_BF_cap_info;
+                htcap->ht_cap.antenna_selection_info =
+                                        bss_cfg->ht_cap.antenna_selection_info;
+                cmd_size += sizeof(struct mwifiex_ie_types_htcap);
+                tlv += sizeof(struct mwifiex_ie_types_htcap);
+        }
+
         *param_size = cmd_size;
 
         return 0;
@@ -410,33 +550,3 @@ int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, u16 cmd_no,
 
         return 0;
 }
-
-/* This function sets the RF channel for AP.
- *
- * This function populates channel information in AP config structure
- * and sends command to configure channel information in AP.
- */
-int mwifiex_uap_set_channel(struct mwifiex_private *priv, int channel)
-{
-        struct mwifiex_uap_bss_param *bss_cfg;
-        struct wiphy *wiphy = priv->wdev->wiphy;
-
-        bss_cfg = kzalloc(sizeof(struct mwifiex_uap_bss_param), GFP_KERNEL);
-        if (!bss_cfg)
-                return -ENOMEM;
-
-        mwifiex_set_sys_config_invalid_data(bss_cfg);
-        bss_cfg->band_cfg = BAND_CONFIG_MANUAL;
-        bss_cfg->channel = channel;
-
-        if (mwifiex_send_cmd_async(priv, HostCmd_CMD_UAP_SYS_CONFIG,
-                                   HostCmd_ACT_GEN_SET,
-                                   UAP_BSS_PARAMS_I, bss_cfg)) {
-                wiphy_err(wiphy, "Failed to set the uAP channel\n");
-                kfree(bss_cfg);
-                return -1;
-        }
-
-        kfree(bss_cfg);
-        return 0;
-}
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 2e9e6af2136..dfcd02ab6ca 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;
                 }
 
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 299c3879582..c7548da6573 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -99,6 +99,14 @@ config RT2800PCI_RT53XX
          rt2800pci driver.
          Supported chips: RT5390
 
+config RT2800PCI_RT3290
+       bool "rt2800pci - Include support for rt3290 devices (EXPERIMENTAL)"
+       depends on EXPERIMENTAL
+       default y
+       ---help---
+         This adds support for rt3290 wireless chipset family to the
+         rt2800pci driver.
+         Supported chips: RT3290
 endif
 
 config RT2500USB
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index 1ca88cdc6ec..e252e9bafd0 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -68,6 +68,7 @@
 #define RF3320                          0x000b
 #define RF3322                          0x000c
 #define RF3053                          0x000d
+#define RF3290                          0x3290
 #define RF5360                          0x5360
 #define RF5370                          0x5370
 #define RF5372                          0x5372
@@ -117,6 +118,12 @@
  * Registers.
  */
 
+
+/*
+ * MAC_CSR0_3290: MAC_CSR0 for RT3290 to identity MAC version number.
+ */
+#define MAC_CSR0_3290                           0x0000
+
 /*
  * E2PROM_CSR: PCI EEPROM control register.
  * RELOAD: Write 1 to reload eeprom content.
@@ -133,6 +140,150 @@
 #define E2PROM_CSR_RELOAD               FIELD32(0x00000080)
 
 /*
+ * CMB_CTRL_CFG
+ */
+#define CMB_CTRL                0x0020
+#define AUX_OPT_BIT0            FIELD32(0x00000001)
+#define AUX_OPT_BIT1            FIELD32(0x00000002)
+#define AUX_OPT_BIT2            FIELD32(0x00000004)
+#define AUX_OPT_BIT3            FIELD32(0x00000008)
+#define AUX_OPT_BIT4            FIELD32(0x00000010)
+#define AUX_OPT_BIT5            FIELD32(0x00000020)
+#define AUX_OPT_BIT6            FIELD32(0x00000040)
+#define AUX_OPT_BIT7            FIELD32(0x00000080)
+#define AUX_OPT_BIT8            FIELD32(0x00000100)
+#define AUX_OPT_BIT9            FIELD32(0x00000200)
+#define AUX_OPT_BIT10           FIELD32(0x00000400)
+#define AUX_OPT_BIT11           FIELD32(0x00000800)
+#define AUX_OPT_BIT12           FIELD32(0x00001000)
+#define AUX_OPT_BIT13           FIELD32(0x00002000)
+#define AUX_OPT_BIT14           FIELD32(0x00004000)
+#define AUX_OPT_BIT15           FIELD32(0x00008000)
+#define LDO25_LEVEL             FIELD32(0x00030000)
+#define LDO25_LARGEA            FIELD32(0x00040000)
+#define LDO25_FRC_ON            FIELD32(0x00080000)
+#define CMB_RSV                 FIELD32(0x00300000)
+#define XTAL_RDY                FIELD32(0x00400000)
+#define PLL_LD                  FIELD32(0x00800000)
+#define LDO_CORE_LEVEL          FIELD32(0x0F000000)
+#define LDO_BGSEL               FIELD32(0x30000000)
+#define LDO3_EN                 FIELD32(0x40000000)
+#define LDO0_EN                 FIELD32(0x80000000)
+
+/*
+ * EFUSE_CSR_3290: RT3290 EEPROM
+ */
+#define EFUSE_CTRL_3290                 0x0024
+
+/*
+ * EFUSE_DATA3 of 3290
+ */
+#define EFUSE_DATA3_3290                0x0028
+
+/*
+ * EFUSE_DATA2 of 3290
+ */
+#define EFUSE_DATA2_3290                0x002c
+
+/*
+ * EFUSE_DATA1 of 3290
+ */
+#define EFUSE_DATA1_3290                0x0030
+
+/*
+ * EFUSE_DATA0 of 3290
+ */
+#define EFUSE_DATA0_3290                0x0034
+
+/*
+ * OSC_CTRL_CFG
+ * Ring oscillator configuration
+ */
+#define OSC_CTRL                0x0038
+#define OSC_REF_CYCLE           FIELD32(0x00001fff)
+#define OSC_RSV                 FIELD32(0x0000e000)
+#define OSC_CAL_CNT             FIELD32(0x0fff0000)
+#define OSC_CAL_ACK             FIELD32(0x10000000)
+#define OSC_CLK_32K_VLD         FIELD32(0x20000000)
+#define OSC_CAL_REQ             FIELD32(0x40000000)
+#define OSC_ROSC_EN             FIELD32(0x80000000)
+
+/*
+ * COEX_CFG_0
+ */
+#define COEX_CFG0                       0x0040
+#define COEX_CFG_ANT            FIELD32(0xff000000)
+/*
+ * COEX_CFG_1
+ */
+#define COEX_CFG1                       0x0044
+
+/*
+ * COEX_CFG_2
+ */
+#define COEX_CFG2                       0x0048
+#define BT_COEX_CFG1            FIELD32(0xff000000)
+#define BT_COEX_CFG0            FIELD32(0x00ff0000)
+#define WL_COEX_CFG1            FIELD32(0x0000ff00)
+#define WL_COEX_CFG0            FIELD32(0x000000ff)
+/*
+ * PLL_CTRL_CFG
+ * PLL configuration register
+ */
+#define PLL_CTRL                0x0050
+#define PLL_RESERVED_INPUT1     FIELD32(0x000000ff)
+#define PLL_RESERVED_INPUT2     FIELD32(0x0000ff00)
+#define PLL_CONTROL             FIELD32(0x00070000)
+#define PLL_LPF_R1              FIELD32(0x00080000)
+#define PLL_LPF_C1_CTRL FIELD32(0x00300000)
+#define PLL_LPF_C2_CTRL FIELD32(0x00c00000)
+#define PLL_CP_CURRENT_CTRL     FIELD32(0x03000000)
+#define PLL_PFD_DELAY_CTRL      FIELD32(0x0c000000)
+#define PLL_LOCK_CTRL           FIELD32(0x70000000)
+#define PLL_VBGBK_EN            FIELD32(0x80000000)
+
+
+/*
+ * WLAN_CTRL_CFG
+ * RT3290 wlan configuration
+ */
+#define WLAN_FUN_CTRL                   0x0080
+#define WLAN_EN                         FIELD32(0x00000001)
+#define WLAN_CLK_EN                     FIELD32(0x00000002)
+#define WLAN_RSV1                       FIELD32(0x00000004)
+#define WLAN_RESET                      FIELD32(0x00000008)
+#define PCIE_APP0_CLK_REQ               FIELD32(0x00000010)
+#define FRC_WL_ANT_SET                  FIELD32(0x00000020)
+#define INV_TR_SW0                      FIELD32(0x00000040)
+#define WLAN_GPIO_IN_BIT0               FIELD32(0x00000100)
+#define WLAN_GPIO_IN_BIT1               FIELD32(0x00000200)
+#define WLAN_GPIO_IN_BIT2               FIELD32(0x00000400)
+#define WLAN_GPIO_IN_BIT3               FIELD32(0x00000800)
+#define WLAN_GPIO_IN_BIT4               FIELD32(0x00001000)
+#define WLAN_GPIO_IN_BIT5               FIELD32(0x00002000)
+#define WLAN_GPIO_IN_BIT6               FIELD32(0x00004000)
+#define WLAN_GPIO_IN_BIT7               FIELD32(0x00008000)
+#define WLAN_GPIO_IN_BIT_ALL            FIELD32(0x0000ff00)
+#define WLAN_GPIO_OUT_BIT0              FIELD32(0x00010000)
+#define WLAN_GPIO_OUT_BIT1              FIELD32(0x00020000)
+#define WLAN_GPIO_OUT_BIT2              FIELD32(0x00040000)
+#define WLAN_GPIO_OUT_BIT3              FIELD32(0x00050000)
+#define WLAN_GPIO_OUT_BIT4              FIELD32(0x00100000)
+#define WLAN_GPIO_OUT_BIT5              FIELD32(0x00200000)
+#define WLAN_GPIO_OUT_BIT6              FIELD32(0x00400000)
+#define WLAN_GPIO_OUT_BIT7              FIELD32(0x00800000)
+#define WLAN_GPIO_OUT_BIT_ALL           FIELD32(0x00ff0000)
+#define WLAN_GPIO_OUT_OE_BIT0           FIELD32(0x01000000)
+#define WLAN_GPIO_OUT_OE_BIT1           FIELD32(0x02000000)
+#define WLAN_GPIO_OUT_OE_BIT2           FIELD32(0x04000000)
+#define WLAN_GPIO_OUT_OE_BIT3           FIELD32(0x08000000)
+#define WLAN_GPIO_OUT_OE_BIT4           FIELD32(0x10000000)
+#define WLAN_GPIO_OUT_OE_BIT5           FIELD32(0x20000000)
+#define WLAN_GPIO_OUT_OE_BIT6           FIELD32(0x40000000)
+#define WLAN_GPIO_OUT_OE_BIT7           FIELD32(0x80000000)
+#define WLAN_GPIO_OUT_OE_BIT_ALL        FIELD32(0xff000000)
+
+/*
  * AUX_CTRL: Aux/PCI-E related configuration
  */
 #define AUX_CTRL                        0x10c
@@ -1763,9 +1914,11 @@ struct mac_iveiv_entry {
 /*
  * BBP 3: RX Antenna
  */
-#define BBP3_RX_ADC                             FIELD8(0x03)
+#define BBP3_RX_ADC                     FIELD8(0x03)
 #define BBP3_RX_ANTENNA                 FIELD8(0x18)
 #define BBP3_HT40_MINUS                 FIELD8(0x20)
+#define BBP3_ADC_MODE_SWITCH            FIELD8(0x40)
+#define BBP3_ADC_INIT_MODE              FIELD8(0x80)
 
 /*
  * BBP 4: Bandwidth
@@ -1775,6 +1928,14 @@ struct mac_iveiv_entry {
 #define BBP4_MAC_IF_CTRL                FIELD8(0x40)
 
 /*
+ * BBP 47: Bandwidth
+ */
+#define BBP47_TSSI_REPORT_SEL           FIELD8(0x03)
+#define BBP47_TSSI_UPDATE_REQ           FIELD8(0x04)
+#define BBP47_TSSI_TSSI_MODE            FIELD8(0x18)
+#define BBP47_TSSI_ADC6                 FIELD8(0x80)
+
+/*
  * BBP 109
  */
 #define BBP109_TX0_POWER                FIELD8(0x0f)
@@ -1917,6 +2078,16 @@ struct mac_iveiv_entry {
 #define RFCSR27_R4                      FIELD8(0x40)
 
 /*
+ * RFCSR 29:
+ */
+#define RFCSR29_ADC6_TEST               FIELD8(0x01)
+#define RFCSR29_ADC6_INT_TEST           FIELD8(0x02)
+#define RFCSR29_RSSI_RESET              FIELD8(0x04)
+#define RFCSR29_RSSI_ON                 FIELD8(0x08)
+#define RFCSR29_RSSI_RIP_CTRL           FIELD8(0x30)
+#define RFCSR29_RSSI_GAIN               FIELD8(0xc0)
+
+/*
  * RFCSR 30:
  */
 #define RFCSR30_TX_H20M                 FIELD8(0x02)
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 4d3747c3010..068276ee8af 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -354,16 +354,15 @@ int rt2800_check_firmware(struct rt2x00_dev *rt2x00dev,
          * of 4kb. Certain USB chipsets however require different firmware,
          * which Ralink only provides attached to the original firmware
          * file. Thus for USB devices, firmware files have a length
-         * which is a multiple of 4kb.
+         * which is a multiple of 4kb. The firmware for rt3290 chip also
+         * have a length which is a multiple of 4kb.
          */
-        if (rt2x00_is_usb(rt2x00dev)) {
+        if (rt2x00_is_usb(rt2x00dev) || rt2x00_rt(rt2x00dev, RT3290))
                 fw_len = 4096;
-                multiple = true;
-        } else {
+        else
                 fw_len = 8192;
-                multiple = true;
-        }
 
+        multiple = true;
         /*
          * Validate the firmware length
          */
@@ -415,7 +414,8 @@ int rt2800_load_firmware(struct rt2x00_dev *rt2x00dev,
                 return -EBUSY;
 
         if (rt2x00_is_pci(rt2x00dev)) {
-                if (rt2x00_rt(rt2x00dev, RT3572) ||
+                if (rt2x00_rt(rt2x00dev, RT3290) ||
+                    rt2x00_rt(rt2x00dev, RT3572) ||
                     rt2x00_rt(rt2x00dev, RT5390) ||
                     rt2x00_rt(rt2x00dev, RT5392)) {
                         rt2800_register_read(rt2x00dev, AUX_CTRL, &reg);
@@ -851,8 +851,13 @@ int rt2800_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 {
         u32 reg;
 
-        rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
-        return rt2x00_get_field32(reg, GPIO_CTRL_CFG_BIT2);
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+                return rt2x00_get_field32(reg, WLAN_GPIO_IN_BIT0);
+        } else {
+                rt2800_register_read(rt2x00dev, GPIO_CTRL_CFG, &reg);
+                return rt2x00_get_field32(reg, GPIO_CTRL_CFG_BIT2);
+        }
 }
 EXPORT_SYMBOL_GPL(rt2800_rfkill_poll);
 
@@ -1935,9 +1940,54 @@ static void rt2800_config_channel_rf3052(struct rt2x00_dev *rt2x00dev,
         rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
 }
 
+#define RT3290_POWER_BOUND     0x27
+#define RT3290_FREQ_OFFSET_BOUND       0x5f
 #define RT5390_POWER_BOUND     0x27
 #define RT5390_FREQ_OFFSET_BOUND       0x5f
 
+static void rt2800_config_channel_rf3290(struct rt2x00_dev *rt2x00dev,
+                                         struct ieee80211_conf *conf,
+                                         struct rf_channel *rf,
+                                         struct channel_info *info)
+{
+        u8 rfcsr;
+
+        rt2800_rfcsr_write(rt2x00dev, 8, rf->rf1);
+        rt2800_rfcsr_write(rt2x00dev, 9, rf->rf3);
+        rt2800_rfcsr_read(rt2x00dev, 11, &rfcsr);
+        rt2x00_set_field8(&rfcsr, RFCSR11_R, rf->rf2);
+        rt2800_rfcsr_write(rt2x00dev, 11, rfcsr);
+
+        rt2800_rfcsr_read(rt2x00dev, 49, &rfcsr);
+        if (info->default_power1 > RT3290_POWER_BOUND)
+                rt2x00_set_field8(&rfcsr, RFCSR49_TX, RT3290_POWER_BOUND);
+        else
+                rt2x00_set_field8(&rfcsr, RFCSR49_TX, info->default_power1);
+        rt2800_rfcsr_write(rt2x00dev, 49, rfcsr);
+
+        rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+        if (rt2x00dev->freq_offset > RT3290_FREQ_OFFSET_BOUND)
+                rt2x00_set_field8(&rfcsr, RFCSR17_CODE,
+                                  RT3290_FREQ_OFFSET_BOUND);
+        else
+                rt2x00_set_field8(&rfcsr, RFCSR17_CODE, rt2x00dev->freq_offset);
+        rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+        if (rf->channel <= 14) {
+                if (rf->channel == 6)
+                        rt2800_bbp_write(rt2x00dev, 68, 0x0c);
+                else
+                        rt2800_bbp_write(rt2x00dev, 68, 0x0b);
+
+                if (rf->channel >= 1 && rf->channel <= 6)
+                        rt2800_bbp_write(rt2x00dev, 59, 0x0f);
+                else if (rf->channel >= 7 && rf->channel <= 11)
+                        rt2800_bbp_write(rt2x00dev, 59, 0x0e);
+                else if (rf->channel >= 12 && rf->channel <= 14)
+                        rt2800_bbp_write(rt2x00dev, 59, 0x0d);
+        }
+}
+
 static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
                                          struct ieee80211_conf *conf,
                                          struct rf_channel *rf,
@@ -2036,15 +2086,6 @@ static void rt2800_config_channel_rf53xx(struct rt2x00_dev *rt2x00dev,
                         }
                 }
         }
-
-        rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
-        rt2x00_set_field8(&rfcsr, RFCSR30_TX_H20M, 0);
-        rt2x00_set_field8(&rfcsr, RFCSR30_RX_H20M, 0);
-        rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
-
-        rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
-        rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
-        rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
 }
 
 static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
@@ -2054,7 +2095,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 {
         u32 reg;
         unsigned int tx_pin;
-        u8 bbp;
+        u8 bbp, rfcsr;
 
         if (rf->channel <= 14) {
                 info->default_power1 = TXPOWER_G_TO_DEV(info->default_power1);
@@ -2075,6 +2116,9 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
         case RF3052:
                 rt2800_config_channel_rf3052(rt2x00dev, conf, rf, info);
                 break;
+        case RF3290:
+                rt2800_config_channel_rf3290(rt2x00dev, conf, rf, info);
+                break;
         case RF5360:
         case RF5370:
         case RF5372:
@@ -2086,6 +2130,22 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
                 rt2800_config_channel_rf2xxx(rt2x00dev, conf, rf, info);
         }
 
+        if (rt2x00_rf(rt2x00dev, RF3290) ||
+            rt2x00_rf(rt2x00dev, RF5360) ||
+            rt2x00_rf(rt2x00dev, RF5370) ||
+            rt2x00_rf(rt2x00dev, RF5372) ||
+            rt2x00_rf(rt2x00dev, RF5390) ||
+            rt2x00_rf(rt2x00dev, RF5392)) {
+                rt2800_rfcsr_read(rt2x00dev, 30, &rfcsr);
+                rt2x00_set_field8(&rfcsr, RFCSR30_TX_H20M, 0);
+                rt2x00_set_field8(&rfcsr, RFCSR30_RX_H20M, 0);
+                rt2800_rfcsr_write(rt2x00dev, 30, rfcsr);
+
+                rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+                rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
+                rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
+        }
+
         /*
          * Change BBP settings
          */
@@ -2566,6 +2626,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
                 rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
                 rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
                 break;
+        case RF3290:
         case RF5360:
         case RF5370:
         case RF5372:
@@ -2701,6 +2762,7 @@ static u8 rt2800_get_default_vgc(struct rt2x00_dev *rt2x00dev)
                 if (rt2x00_rt(rt2x00dev, RT3070) ||
                     rt2x00_rt(rt2x00dev, RT3071) ||
                     rt2x00_rt(rt2x00dev, RT3090) ||
+                    rt2x00_rt(rt2x00dev, RT3290) ||
                     rt2x00_rt(rt2x00dev, RT3390) ||
                     rt2x00_rt(rt2x00dev, RT5390) ||
                     rt2x00_rt(rt2x00dev, RT5392))
@@ -2797,10 +2859,54 @@ static int rt2800_init_registers(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, BKOFF_SLOT_CFG_CC_DELAY_TIME, 2);
         rt2800_register_write(rt2x00dev, BKOFF_SLOT_CFG, reg);
 
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+                if (rt2x00_get_field32(reg, WLAN_EN) == 1) {
+                        rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 1);
+                        rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+                }
+
+                rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+                if (!(rt2x00_get_field32(reg, LDO0_EN) == 1)) {
+                        rt2x00_set_field32(&reg, LDO0_EN, 1);
+                        rt2x00_set_field32(&reg, LDO_BGSEL, 3);
+                        rt2800_register_write(rt2x00dev, CMB_CTRL, reg);
+                }
+
+                rt2800_register_read(rt2x00dev, OSC_CTRL, &reg);
+                rt2x00_set_field32(&reg, OSC_ROSC_EN, 1);
+                rt2x00_set_field32(&reg, OSC_CAL_REQ, 1);
+                rt2x00_set_field32(&reg, OSC_REF_CYCLE, 0x27);
+                rt2800_register_write(rt2x00dev, OSC_CTRL, reg);
+
+                rt2800_register_read(rt2x00dev, COEX_CFG0, &reg);
+                rt2x00_set_field32(&reg, COEX_CFG_ANT, 0x5e);
+                rt2800_register_write(rt2x00dev, COEX_CFG0, reg);
+
+                rt2800_register_read(rt2x00dev, COEX_CFG2, &reg);
+                rt2x00_set_field32(&reg, BT_COEX_CFG1, 0x00);
+                rt2x00_set_field32(&reg, BT_COEX_CFG0, 0x17);
+                rt2x00_set_field32(&reg, WL_COEX_CFG1, 0x93);
+                rt2x00_set_field32(&reg, WL_COEX_CFG0, 0x7f);
+                rt2800_register_write(rt2x00dev, COEX_CFG2, reg);
+
+                rt2800_register_read(rt2x00dev, PLL_CTRL, &reg);
+                rt2x00_set_field32(&reg, PLL_CONTROL, 1);
+                rt2800_register_write(rt2x00dev, PLL_CTRL, reg);
+        }
+
         if (rt2x00_rt(rt2x00dev, RT3071) ||
             rt2x00_rt(rt2x00dev, RT3090) ||
+            rt2x00_rt(rt2x00dev, RT3290) ||
             rt2x00_rt(rt2x00dev, RT3390)) {
-                rt2800_register_write(rt2x00dev, TX_SW_CFG0, 0x00000400);
+
+                if (rt2x00_rt(rt2x00dev, RT3290))
+                        rt2800_register_write(rt2x00dev, TX_SW_CFG0,
+                                              0x00000404);
+                else
+                        rt2800_register_write(rt2x00dev, TX_SW_CFG0,
+                                              0x00000400);
+
                 rt2800_register_write(rt2x00dev, TX_SW_CFG1, 0x00000000);
                 if (rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
                     rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
@@ -3209,14 +3315,16 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
                      rt2800_wait_bbp_ready(rt2x00dev)))
                 return -EACCES;
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392)) {
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392)) {
                 rt2800_bbp_read(rt2x00dev, 4, &value);
                 rt2x00_set_field8(&value, BBP4_MAC_IF_CTRL, 1);
                 rt2800_bbp_write(rt2x00dev, 4, value);
         }
 
         if (rt2800_is_305x_soc(rt2x00dev) ||
+            rt2x00_rt(rt2x00dev, RT3290) ||
             rt2x00_rt(rt2x00dev, RT3572) ||
             rt2x00_rt(rt2x00dev, RT5390) ||
             rt2x00_rt(rt2x00dev, RT5392))
@@ -3225,20 +3333,26 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
         rt2800_bbp_write(rt2x00dev, 65, 0x2c);
         rt2800_bbp_write(rt2x00dev, 66, 0x38);
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 68, 0x0b);
 
         if (rt2x00_rt_rev(rt2x00dev, RT2860, REV_RT2860C)) {
                 rt2800_bbp_write(rt2x00dev, 69, 0x16);
                 rt2800_bbp_write(rt2x00dev, 73, 0x12);
-        } else if (rt2x00_rt(rt2x00dev, RT5390) ||
-                           rt2x00_rt(rt2x00dev, RT5392)) {
+        } else if (rt2x00_rt(rt2x00dev, RT3290) ||
+                   rt2x00_rt(rt2x00dev, RT5390) ||
+                   rt2x00_rt(rt2x00dev, RT5392)) {
                 rt2800_bbp_write(rt2x00dev, 69, 0x12);
                 rt2800_bbp_write(rt2x00dev, 73, 0x13);
                 rt2800_bbp_write(rt2x00dev, 75, 0x46);
                 rt2800_bbp_write(rt2x00dev, 76, 0x28);
-                rt2800_bbp_write(rt2x00dev, 77, 0x59);
+
+                if (rt2x00_rt(rt2x00dev, RT3290))
+                        rt2800_bbp_write(rt2x00dev, 77, 0x58);
+                else
+                        rt2800_bbp_write(rt2x00dev, 77, 0x59);
         } else {
                 rt2800_bbp_write(rt2x00dev, 69, 0x12);
                 rt2800_bbp_write(rt2x00dev, 73, 0x10);
@@ -3263,23 +3377,33 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
                 rt2800_bbp_write(rt2x00dev, 81, 0x37);
         }
 
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_bbp_write(rt2x00dev, 74, 0x0b);
+                rt2800_bbp_write(rt2x00dev, 79, 0x18);
+                rt2800_bbp_write(rt2x00dev, 80, 0x09);
+                rt2800_bbp_write(rt2x00dev, 81, 0x33);
+        }
+
         rt2800_bbp_write(rt2x00dev, 82, 0x62);
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 83, 0x7a);
         else
                 rt2800_bbp_write(rt2x00dev, 83, 0x6a);
 
         if (rt2x00_rt_rev(rt2x00dev, RT2860, REV_RT2860D))
                 rt2800_bbp_write(rt2x00dev, 84, 0x19);
-        else if (rt2x00_rt(rt2x00dev, RT5390) ||
-                         rt2x00_rt(rt2x00dev, RT5392))
+        else if (rt2x00_rt(rt2x00dev, RT3290) ||
+                     rt2x00_rt(rt2x00dev, RT5390) ||
+                     rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 84, 0x9a);
         else
                 rt2800_bbp_write(rt2x00dev, 84, 0x99);
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 86, 0x38);
         else
                 rt2800_bbp_write(rt2x00dev, 86, 0x00);
@@ -3289,8 +3413,9 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
 
         rt2800_bbp_write(rt2x00dev, 91, 0x04);
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 92, 0x02);
         else
                 rt2800_bbp_write(rt2x00dev, 92, 0x00);
@@ -3304,6 +3429,7 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
             rt2x00_rt_rev_gte(rt2x00dev, RT3071, REV_RT3071E) ||
             rt2x00_rt_rev_gte(rt2x00dev, RT3090, REV_RT3090E) ||
             rt2x00_rt_rev_gte(rt2x00dev, RT3390, REV_RT3390E) ||
+            rt2x00_rt(rt2x00dev, RT3290) ||
             rt2x00_rt(rt2x00dev, RT3572) ||
             rt2x00_rt(rt2x00dev, RT5390) ||
             rt2x00_rt(rt2x00dev, RT5392) ||
@@ -3312,27 +3438,32 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
         else
                 rt2800_bbp_write(rt2x00dev, 103, 0x00);
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 104, 0x92);
 
         if (rt2800_is_305x_soc(rt2x00dev))
                 rt2800_bbp_write(rt2x00dev, 105, 0x01);
+        else if (rt2x00_rt(rt2x00dev, RT3290))
+                rt2800_bbp_write(rt2x00dev, 105, 0x1c);
         else if (rt2x00_rt(rt2x00dev, RT5390) ||
                          rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 105, 0x3c);
         else
                 rt2800_bbp_write(rt2x00dev, 105, 0x05);
 
-        if (rt2x00_rt(rt2x00dev, RT5390))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390))
                 rt2800_bbp_write(rt2x00dev, 106, 0x03);
         else if (rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 106, 0x12);
         else
                 rt2800_bbp_write(rt2x00dev, 106, 0x35);
 
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392))
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392))
                 rt2800_bbp_write(rt2x00dev, 128, 0x12);
 
         if (rt2x00_rt(rt2x00dev, RT5392)) {
@@ -3357,6 +3488,29 @@ static int rt2800_init_bbp(struct rt2x00_dev *rt2x00dev)
                 rt2800_bbp_write(rt2x00dev, 138, value);
         }
 
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_bbp_write(rt2x00dev, 67, 0x24);
+                rt2800_bbp_write(rt2x00dev, 143, 0x04);
+                rt2800_bbp_write(rt2x00dev, 142, 0x99);
+                rt2800_bbp_write(rt2x00dev, 150, 0x30);
+                rt2800_bbp_write(rt2x00dev, 151, 0x2e);
+                rt2800_bbp_write(rt2x00dev, 152, 0x20);
+                rt2800_bbp_write(rt2x00dev, 153, 0x34);
+                rt2800_bbp_write(rt2x00dev, 154, 0x40);
+                rt2800_bbp_write(rt2x00dev, 155, 0x3b);
+                rt2800_bbp_write(rt2x00dev, 253, 0x04);
+
+                rt2800_bbp_read(rt2x00dev, 47, &value);
+                rt2x00_set_field8(&value, BBP47_TSSI_ADC6, 1);
+                rt2800_bbp_write(rt2x00dev, 47, value);
+
+                /* Use 5-bit ADC for Acquisition and 8-bit ADC for data */
+                rt2800_bbp_read(rt2x00dev, 3, &value);
+                rt2x00_set_field8(&value, BBP3_ADC_MODE_SWITCH, 1);
+                rt2x00_set_field8(&value, BBP3_ADC_INIT_MODE, 1);
+                rt2800_bbp_write(rt2x00dev, 3, value);
+        }
+
         if (rt2x00_rt(rt2x00dev, RT5390) ||
                 rt2x00_rt(rt2x00dev, RT5392)) {
                 int ant, div_mode;
@@ -3489,6 +3643,7 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
         if (!rt2x00_rt(rt2x00dev, RT3070) &&
             !rt2x00_rt(rt2x00dev, RT3071) &&
             !rt2x00_rt(rt2x00dev, RT3090) &&
+            !rt2x00_rt(rt2x00dev, RT3290) &&
             !rt2x00_rt(rt2x00dev, RT3390) &&
             !rt2x00_rt(rt2x00dev, RT3572) &&
             !rt2x00_rt(rt2x00dev, RT5390) &&
@@ -3499,8 +3654,9 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
         /*
          * Init RF calibration.
          */
-        if (rt2x00_rt(rt2x00dev, RT5390) ||
-                rt2x00_rt(rt2x00dev, RT5392)) {
+        if (rt2x00_rt(rt2x00dev, RT3290) ||
+            rt2x00_rt(rt2x00dev, RT5390) ||
+            rt2x00_rt(rt2x00dev, RT5392)) {
                 rt2800_rfcsr_read(rt2x00dev, 2, &rfcsr);
                 rt2x00_set_field8(&rfcsr, RFCSR2_RESCAL_EN, 1);
                 rt2800_rfcsr_write(rt2x00dev, 2, rfcsr);
@@ -3538,6 +3694,53 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
                 rt2800_rfcsr_write(rt2x00dev, 24, 0x16);
                 rt2800_rfcsr_write(rt2x00dev, 25, 0x01);
                 rt2800_rfcsr_write(rt2x00dev, 29, 0x1f);
+        } else if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_rfcsr_write(rt2x00dev, 1, 0x0f);
+                rt2800_rfcsr_write(rt2x00dev, 2, 0x80);
+                rt2800_rfcsr_write(rt2x00dev, 3, 0x08);
+                rt2800_rfcsr_write(rt2x00dev, 4, 0x00);
+                rt2800_rfcsr_write(rt2x00dev, 6, 0xa0);
+                rt2800_rfcsr_write(rt2x00dev, 8, 0xf3);
+                rt2800_rfcsr_write(rt2x00dev, 9, 0x02);
+                rt2800_rfcsr_write(rt2x00dev, 10, 0x53);
+                rt2800_rfcsr_write(rt2x00dev, 11, 0x4a);
+                rt2800_rfcsr_write(rt2x00dev, 12, 0x46);
+                rt2800_rfcsr_write(rt2x00dev, 13, 0x9f);
+                rt2800_rfcsr_write(rt2x00dev, 18, 0x02);
+                rt2800_rfcsr_write(rt2x00dev, 22, 0x20);
+                rt2800_rfcsr_write(rt2x00dev, 25, 0x83);
+                rt2800_rfcsr_write(rt2x00dev, 26, 0x82);
+                rt2800_rfcsr_write(rt2x00dev, 27, 0x09);
+                rt2800_rfcsr_write(rt2x00dev, 29, 0x10);
+                rt2800_rfcsr_write(rt2x00dev, 30, 0x10);
+                rt2800_rfcsr_write(rt2x00dev, 31, 0x80);
+                rt2800_rfcsr_write(rt2x00dev, 32, 0x80);
+                rt2800_rfcsr_write(rt2x00dev, 33, 0x00);
+                rt2800_rfcsr_write(rt2x00dev, 34, 0x05);
+                rt2800_rfcsr_write(rt2x00dev, 35, 0x12);
+                rt2800_rfcsr_write(rt2x00dev, 36, 0x00);
+                rt2800_rfcsr_write(rt2x00dev, 38, 0x85);
+                rt2800_rfcsr_write(rt2x00dev, 39, 0x1b);
+                rt2800_rfcsr_write(rt2x00dev, 40, 0x0b);
+                rt2800_rfcsr_write(rt2x00dev, 41, 0xbb);
+                rt2800_rfcsr_write(rt2x00dev, 42, 0xd5);
+                rt2800_rfcsr_write(rt2x00dev, 43, 0x7b);
+                rt2800_rfcsr_write(rt2x00dev, 44, 0x0e);
+                rt2800_rfcsr_write(rt2x00dev, 45, 0xa2);
+                rt2800_rfcsr_write(rt2x00dev, 46, 0x73);
+                rt2800_rfcsr_write(rt2x00dev, 47, 0x00);
+                rt2800_rfcsr_write(rt2x00dev, 48, 0x10);
+                rt2800_rfcsr_write(rt2x00dev, 49, 0x98);
+                rt2800_rfcsr_write(rt2x00dev, 52, 0x38);
+                rt2800_rfcsr_write(rt2x00dev, 53, 0x00);
+                rt2800_rfcsr_write(rt2x00dev, 54, 0x78);
+                rt2800_rfcsr_write(rt2x00dev, 55, 0x43);
+                rt2800_rfcsr_write(rt2x00dev, 56, 0x02);
+                rt2800_rfcsr_write(rt2x00dev, 57, 0x80);
+                rt2800_rfcsr_write(rt2x00dev, 58, 0x7f);
+                rt2800_rfcsr_write(rt2x00dev, 59, 0x09);
+                rt2800_rfcsr_write(rt2x00dev, 60, 0x45);
+                rt2800_rfcsr_write(rt2x00dev, 61, 0xc1);
         } else if (rt2x00_rt(rt2x00dev, RT3390)) {
                 rt2800_rfcsr_write(rt2x00dev, 0, 0xa0);
                 rt2800_rfcsr_write(rt2x00dev, 1, 0xe1);
@@ -3946,6 +4149,12 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
                 rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
         }
 
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                rt2800_rfcsr_read(rt2x00dev, 29, &rfcsr);
+                rt2x00_set_field8(&rfcsr, RFCSR29_RSSI_GAIN, 3);
+                rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
+        }
+
         if (rt2x00_rt(rt2x00dev, RT5390) ||
                 rt2x00_rt(rt2x00dev, RT5392)) {
                 rt2800_rfcsr_read(rt2x00dev, 38, &rfcsr);
@@ -4052,9 +4261,14 @@ EXPORT_SYMBOL_GPL(rt2800_disable_radio);
 int rt2800_efuse_detect(struct rt2x00_dev *rt2x00dev)
 {
         u32 reg;
+        u16 efuse_ctrl_reg;
 
-        rt2800_register_read(rt2x00dev, EFUSE_CTRL, &reg);
+        if (rt2x00_rt(rt2x00dev, RT3290))
+                efuse_ctrl_reg = EFUSE_CTRL_3290;
+        else
+                efuse_ctrl_reg = EFUSE_CTRL;
 
+        rt2800_register_read(rt2x00dev, efuse_ctrl_reg, &reg);
         return rt2x00_get_field32(reg, EFUSE_CTRL_PRESENT);
 }
 EXPORT_SYMBOL_GPL(rt2800_efuse_detect);
@@ -4062,27 +4276,44 @@ EXPORT_SYMBOL_GPL(rt2800_efuse_detect);
 static void rt2800_efuse_read(struct rt2x00_dev *rt2x00dev, unsigned int i)
 {
         u32 reg;
-
+        u16 efuse_ctrl_reg;
+        u16 efuse_data0_reg;
+        u16 efuse_data1_reg;
+        u16 efuse_data2_reg;
+        u16 efuse_data3_reg;
+
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                efuse_ctrl_reg = EFUSE_CTRL_3290;
+                efuse_data0_reg = EFUSE_DATA0_3290;
+                efuse_data1_reg = EFUSE_DATA1_3290;
+                efuse_data2_reg = EFUSE_DATA2_3290;
+                efuse_data3_reg = EFUSE_DATA3_3290;
+        } else {
+                efuse_ctrl_reg = EFUSE_CTRL;
+                efuse_data0_reg = EFUSE_DATA0;
+                efuse_data1_reg = EFUSE_DATA1;
+                efuse_data2_reg = EFUSE_DATA2;
+                efuse_data3_reg = EFUSE_DATA3;
+        }
         mutex_lock(&rt2x00dev->csr_mutex);
 
-        rt2800_register_read_lock(rt2x00dev, EFUSE_CTRL, &reg);
+        rt2800_register_read_lock(rt2x00dev, efuse_ctrl_reg, &reg);
         rt2x00_set_field32(&reg, EFUSE_CTRL_ADDRESS_IN, i);
         rt2x00_set_field32(&reg, EFUSE_CTRL_MODE, 0);
         rt2x00_set_field32(&reg, EFUSE_CTRL_KICK, 1);
-        rt2800_register_write_lock(rt2x00dev, EFUSE_CTRL, reg);
+        rt2800_register_write_lock(rt2x00dev, efuse_ctrl_reg, reg);
 
         /* Wait until the EEPROM has been loaded */
-        rt2800_regbusy_read(rt2x00dev, EFUSE_CTRL, EFUSE_CTRL_KICK, &reg);
-
+        rt2800_regbusy_read(rt2x00dev, efuse_ctrl_reg, EFUSE_CTRL_KICK, &reg);
         /* Apparently the data is read from end to start */
-        rt2800_register_read_lock(rt2x00dev, EFUSE_DATA3, &reg);
+        rt2800_register_read_lock(rt2x00dev, efuse_data3_reg, &reg);
         /* The returned value is in CPU order, but eeprom is le */
         *(u32 *)&rt2x00dev->eeprom[i] = cpu_to_le32(reg);
-        rt2800_register_read_lock(rt2x00dev, EFUSE_DATA2, &reg);
+        rt2800_register_read_lock(rt2x00dev, efuse_data2_reg, &reg);
         *(u32 *)&rt2x00dev->eeprom[i + 2] = cpu_to_le32(reg);
-        rt2800_register_read_lock(rt2x00dev, EFUSE_DATA1, &reg);
+        rt2800_register_read_lock(rt2x00dev, efuse_data1_reg, &reg);
         *(u32 *)&rt2x00dev->eeprom[i + 4] = cpu_to_le32(reg);
-        rt2800_register_read_lock(rt2x00dev, EFUSE_DATA0, &reg);
+        rt2800_register_read_lock(rt2x00dev, efuse_data0_reg, &reg);
         *(u32 *)&rt2x00dev->eeprom[i + 6] = cpu_to_le32(reg);
 
         mutex_unlock(&rt2x00dev->csr_mutex);
@@ -4244,9 +4475,14 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
          * RT28xx/RT30xx: defined in "EEPROM_NIC_CONF0_RF_TYPE" field
          * RT53xx: defined in "EEPROM_CHIP_ID" field
          */
-        rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
-        if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390 ||
-                rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5392)
+        if (rt2x00_rt(rt2x00dev, RT3290))
+                rt2800_register_read(rt2x00dev, MAC_CSR0_3290, &reg);
+        else
+                rt2800_register_read(rt2x00dev, MAC_CSR0, &reg);
+
+        if (rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT3290 ||
+            rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5390 ||
+            rt2x00_get_field32(reg, MAC_CSR0_CHIPSET) == RT5392)
                 rt2x00_eeprom_read(rt2x00dev, EEPROM_CHIP_ID, &value);
         else
                 value = rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RF_TYPE);
@@ -4261,6 +4497,7 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
         case RT3070:
         case RT3071:
         case RT3090:
+        case RT3290:
         case RT3390:
         case RT3572:
         case RT5390:
@@ -4281,6 +4518,7 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
         case RF3021:
         case RF3022:
         case RF3052:
+        case RF3290:
         case RF3320:
         case RF5360:
         case RF5370:
@@ -4597,6 +4835,7 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
                    rt2x00_rf(rt2x00dev, RF2020) ||
                    rt2x00_rf(rt2x00dev, RF3021) ||
                    rt2x00_rf(rt2x00dev, RF3022) ||
+                   rt2x00_rf(rt2x00dev, RF3290) ||
                    rt2x00_rf(rt2x00dev, RF3320) ||
                    rt2x00_rf(rt2x00dev, RF5360) ||
                    rt2x00_rf(rt2x00dev, RF5370) ||
@@ -4685,6 +4924,7 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
         case RF3022:
         case RF3320:
         case RF3052:
+        case RF3290:
         case RF5360:
         case RF5370:
         case RF5372:
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index 206158b6742..dd436125fe3 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -280,7 +280,13 @@ static void rt2800pci_stop_queue(struct data_queue *queue)
  */
 static char *rt2800pci_get_firmware_name(struct rt2x00_dev *rt2x00dev)
 {
-        return FIRMWARE_RT2860;
+        /*
+         * Chip rt3290 use specific 4KB firmware named rt3290.bin.
+         */
+        if (rt2x00_rt(rt2x00dev, RT3290))
+                return FIRMWARE_RT3290;
+        else
+                return FIRMWARE_RT2860;
 }
 
 static int rt2800pci_write_firmware(struct rt2x00_dev *rt2x00dev,
@@ -974,6 +980,66 @@ static int rt2800pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
         return rt2800_validate_eeprom(rt2x00dev);
 }
 
+static int rt2800_enable_wlan_rt3290(struct rt2x00_dev *rt2x00dev)
+{
+        u32 reg;
+        int i, count;
+
+        rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+        if ((rt2x00_get_field32(reg, WLAN_EN) == 1))
+                return 0;
+
+        rt2x00_set_field32(&reg, WLAN_GPIO_OUT_OE_BIT_ALL, 0xff);
+        rt2x00_set_field32(&reg, FRC_WL_ANT_SET, 1);
+        rt2x00_set_field32(&reg, WLAN_CLK_EN, 0);
+        rt2x00_set_field32(&reg, WLAN_EN, 1);
+        rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+
+        udelay(REGISTER_BUSY_DELAY);
+
+        count = 0;
+        do {
+                /*
+                 * Check PLL_LD & XTAL_RDY.
+                 */
+                for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+                        rt2800_register_read(rt2x00dev, CMB_CTRL, &reg);
+                        if ((rt2x00_get_field32(reg, PLL_LD) == 1) &&
+                                (rt2x00_get_field32(reg, XTAL_RDY) == 1))
+                                        break;
+                        udelay(REGISTER_BUSY_DELAY);
+                }
+
+                if (i >= REGISTER_BUSY_COUNT) {
+
+                        if (count >= 10)
+                                return -EIO;
+
+                        rt2800_register_write(rt2x00dev, 0x58, 0x018);
+                        udelay(REGISTER_BUSY_DELAY);
+                        rt2800_register_write(rt2x00dev, 0x58, 0x418);
+                        udelay(REGISTER_BUSY_DELAY);
+                        rt2800_register_write(rt2x00dev, 0x58, 0x618);
+                        udelay(REGISTER_BUSY_DELAY);
+                        count++;
+                } else {
+                        count = 0;
+                }
+
+                rt2800_register_read(rt2x00dev, WLAN_FUN_CTRL, &reg);
+                rt2x00_set_field32(&reg, PCIE_APP0_CLK_REQ, 0);
+                rt2x00_set_field32(&reg, WLAN_CLK_EN, 1);
+                rt2x00_set_field32(&reg, WLAN_RESET, 1);
+                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+                udelay(10);
+                rt2x00_set_field32(&reg, WLAN_RESET, 0);
+                rt2800_register_write(rt2x00dev, WLAN_FUN_CTRL, reg);
+                udelay(10);
+                rt2800_register_write(rt2x00dev, INT_SOURCE_CSR, 0x7fffffff);
+        } while (count != 0);
+
+        return 0;
+}
 static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 {
         int retval;
@@ -997,6 +1063,17 @@ static int rt2800pci_probe_hw(struct rt2x00_dev *rt2x00dev)
                 return retval;
 
         /*
+         * In probe phase call rt2800_enable_wlan_rt3290 to enable wlan
+         * clk for rt3290. That avoid the MCU fail in start phase.
+         */
+        if (rt2x00_rt(rt2x00dev, RT3290)) {
+                retval = rt2800_enable_wlan_rt3290(rt2x00dev);
+
+                if (retval)
+                        return retval;
+        }
+
+        /*
          * This device has multiple filters for control frames
          * and has a separate filter for PS Poll frames.
          */
@@ -1175,6 +1252,9 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
         { PCI_DEVICE(0x1432, 0x7768) },
         { PCI_DEVICE(0x1462, 0x891a) },
         { PCI_DEVICE(0x1a3b, 0x1059) },
+#ifdef CONFIG_RT2800PCI_RT3290
+        { PCI_DEVICE(0x1814, 0x3290) },
+#endif
 #ifdef CONFIG_RT2800PCI_RT33XX
         { PCI_DEVICE(0x1814, 0x3390) },
 #endif
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.h b/drivers/net/wireless/rt2x00/rt2800pci.h
index 70e050d904c..ab22a087c50 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.h
+++ b/drivers/net/wireless/rt2x00/rt2800pci.h
@@ -47,6 +47,7 @@
  * 8051 firmware image.
  */
 #define FIRMWARE_RT2860                 "rt2860.bin"
+#define FIRMWARE_RT3290                 "rt3290.bin"
 #define FIRMWARE_IMAGE_BASE             0x2000
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 20a50407289..6cf336595e2 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -971,6 +971,7 @@ static struct usb_device_id rt2800usb_device_table[] = {
         { USB_DEVICE(0x0411, 0x015d) },
         { USB_DEVICE(0x0411, 0x016f) },
         { USB_DEVICE(0x0411, 0x01a2) },
+        { USB_DEVICE(0x0411, 0x01ee) },
         /* Corega */
         { USB_DEVICE(0x07aa, 0x002f) },
         { USB_DEVICE(0x07aa, 0x003c) },
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 8f754025b06..8afb546c2b2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -187,6 +187,7 @@ struct rt2x00_chip {
 #define RT3070          0x3070
 #define RT3071          0x3071
 #define RT3090          0x3090  /* 2.4GHz PCIe */
+#define RT3290          0x3290
 #define RT3390          0x3390
 #define RT3572          0x3572
 #define RT3593          0x3593
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 0a4653a92ca..a0c8caef3b0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -256,6 +256,7 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops)
         struct ieee80211_hw *hw;
         struct rt2x00_dev *rt2x00dev;
         int retval;
+        u16 chip;
 
         retval = pci_enable_device(pci_dev);
         if (retval) {
@@ -305,6 +306,14 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct rt2x00_ops *ops)
         if (retval)
                 goto exit_free_device;
 
+        /*
+         * Because rt3290 chip use different efuse offset to read efuse data.
+         * So before read efuse it need to indicate it is the
+         * rt3290 or not.
+         */
+        pci_read_config_word(pci_dev, PCI_DEVICE_ID, &chip);
+        rt2x00dev->chip.rt = chip;
+
         retval = rt2x00lib_probe_dev(rt2x00dev);
         if (retval)
                 goto exit_free_reg;
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 2bebcb71a1e..3b505395d86 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -47,6 +47,8 @@ static DEFINE_PCI_DEVICE_TABLE(rtl8180_table) = {
         { PCI_DEVICE(0x1799, 0x6001) },
         { PCI_DEVICE(0x1799, 0x6020) },
         { PCI_DEVICE(PCI_VENDOR_ID_DLINK, 0x3300) },
+        { PCI_DEVICE(0x1186, 0x3301) },
+        { PCI_DEVICE(0x1432, 0x7106) },
         { }
 };
 
diff --git a/drivers/net/wireless/rtl818x/rtl8187/leds.c b/drivers/net/wireless/rtl818x/rtl8187/leds.c
index 2e0de2f5f0f..c2d5b495c17 100644
--- a/drivers/net/wireless/rtl818x/rtl8187/leds.c
+++ b/drivers/net/wireless/rtl818x/rtl8187/leds.c
@@ -117,7 +117,7 @@ static void rtl8187_led_brightness_set(struct led_classdev *led_dev,
                         radio_on = true;
                 } else if (radio_on) {
                         radio_on = false;
-                        cancel_delayed_work_sync(&priv->led_on);
+                        cancel_delayed_work(&priv->led_on);
                         ieee80211_queue_delayed_work(hw, &priv->led_off, 0);
                 }
         } else if (radio_on) {
diff --git a/drivers/net/wireless/rtlwifi/cam.c b/drivers/net/wireless/rtlwifi/cam.c
index 3d8cc4a0c86..6a2d72beb00 100644
--- a/drivers/net/wireless/rtlwifi/cam.c
+++ b/drivers/net/wireless/rtlwifi/cam.c
@@ -128,7 +128,7 @@ u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
         u32 us_config;
         struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-        RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+        RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
                  "EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, ulUseDK=%x MacAddr %pM\n",
                  ul_entry_idx, ul_key_id, ul_enc_alg,
                  ul_default_key, mac_addr);
@@ -342,7 +342,8 @@ void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
                         /* Remove from HW Security CAM */
                         memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
                         rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
-                        pr_info("&&&&&&&&&del entry %d\n", i);
+                        RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+                                 "del CAM entry %d\n", i);
                 }
         }
         return;
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 2062ea1d7c8..36bffbc4519 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -1273,17 +1273,18 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
          *after reset, release previous pending packet,
          *and force the  tx idx to the first one
          */
-        spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
         for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
                 if (rtlpci->tx_ring[i].desc) {
                         struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
 
                         while (skb_queue_len(&ring->queue)) {
-                                struct rtl_tx_desc *entry =
-                                    &ring->desc[ring->idx];
-                                struct sk_buff *skb =
-                                    __skb_dequeue(&ring->queue);
+                                struct rtl_tx_desc *entry;
+                                struct sk_buff *skb;
 
+                                spin_lock_irqsave(&rtlpriv->locks.irq_th_lock,
+                                                  flags);
+                                entry = &ring->desc[ring->idx];
+                                skb = __skb_dequeue(&ring->queue);
                                 pci_unmap_single(rtlpci->pdev,
                                                  rtlpriv->cfg->ops->
                                                          get_desc((u8 *)
@@ -1291,15 +1292,15 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
                                                          true,
                                                          HW_DESC_TXBUFF_ADDR),
                                                  skb->len, PCI_DMA_TODEVICE);
-                                kfree_skb(skb);
                                 ring->idx = (ring->idx + 1) % ring->entries;
+                                spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
+                                                  flags);
+                                kfree_skb(skb);
                         }
                         ring->idx = 0;
                 }
         }
 
-        spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
         return 0;
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
index 8d7099bc472..b917a2a3caf 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/phy.c
@@ -1247,6 +1247,9 @@ static void _rtl92s_phy_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
                 /* Read HT 40 OFDM TX power */
                 ofdmpowerLevel[0] = rtlefuse->txpwrlevel_ht40_2s[0][index];
                 ofdmpowerLevel[1] = rtlefuse->txpwrlevel_ht40_2s[1][index];
+        } else {
+                ofdmpowerLevel[0] = 0;
+                ofdmpowerLevel[1] = 0;
         }
 }
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
index 730bcc91952..ad4b4803482 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/sw.c
@@ -29,7 +29,6 @@
 
 #include "../wifi.h"
 #include "../core.h"
-#include "../pci.h"
 #include "../base.h"
 #include "../pci.h"
 #include "reg.h"
diff --git a/drivers/net/wireless/ti/wl1251/cmd.c b/drivers/net/wireless/ti/wl1251/cmd.c
index d14d69d733a..6822b845efc 100644
--- a/drivers/net/wireless/ti/wl1251/cmd.c
+++ b/drivers/net/wireless/ti/wl1251/cmd.c
@@ -277,15 +277,6 @@ int wl1251_cmd_join(struct wl1251 *wl, u8 bss_type, u8 channel,
         join->rx_config_options = wl->rx_config;
         join->rx_filter_options = wl->rx_filter;
 
-        /*
-         * FIXME: disable temporarily all filters because after commit
-         * 9cef8737 "mac80211: fix managed mode BSSID handling" broke
-         * association. The filter logic needs to be implemented properly
-         * and once that is done, this hack can be removed.
-         */
-        join->rx_config_options = 0;
-        join->rx_filter_options = WL1251_DEFAULT_RX_FILTER;
-
         join->basic_rate_set = RATE_MASK_1MBPS | RATE_MASK_2MBPS |
                 RATE_MASK_5_5MBPS | RATE_MASK_11MBPS;
 
diff --git a/drivers/net/wireless/ti/wl1251/main.c b/drivers/net/wireless/ti/wl1251/main.c
index d1afb8e3b2e..3118c425bcf 100644
--- a/drivers/net/wireless/ti/wl1251/main.c
+++ b/drivers/net/wireless/ti/wl1251/main.c
@@ -334,6 +334,12 @@ static int wl1251_join(struct wl1251 *wl, u8 bss_type, u8 channel,
         if (ret < 0)
                 goto out;
 
+        /*
+         * Join command applies filters, and if we are not associated,
+         * BSSID filter must be disabled for association to work.
+         */
+        if (is_zero_ether_addr(wl->bssid))
+                wl->rx_config &= ~CFG_BSSID_FILTER_EN;
 
         ret = wl1251_cmd_join(wl, bss_type, channel, beacon_interval,
                               dtim_period);
@@ -348,33 +354,6 @@ out:
         return ret;
 }
 
-static void wl1251_filter_work(struct work_struct *work)
-{
-        struct wl1251 *wl =
-                container_of(work, struct wl1251, filter_work);
-        int ret;
-
-        mutex_lock(&wl->mutex);
-
-        if (wl->state == WL1251_STATE_OFF)
-                goto out;
-
-        ret = wl1251_ps_elp_wakeup(wl);
-        if (ret < 0)
-                goto out;
-
-        ret = wl1251_join(wl, wl->bss_type, wl->channel, wl->beacon_int,
-                          wl->dtim_period);
-        if (ret < 0)
-                goto out_sleep;
-
-out_sleep:
-        wl1251_ps_elp_sleep(wl);
-
-out:
-        mutex_unlock(&wl->mutex);
-}
-
 static void wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct wl1251 *wl = hw->priv;
@@ -478,7 +457,6 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
 
         cancel_work_sync(&wl->irq_work);
         cancel_work_sync(&wl->tx_work);
-        cancel_work_sync(&wl->filter_work);
         cancel_delayed_work_sync(&wl->elp_work);
 
         mutex_lock(&wl->mutex);
@@ -681,13 +659,15 @@ out:
                                   FIF_FCSFAIL | \
                                   FIF_BCN_PRBRESP_PROMISC | \
                                   FIF_CONTROL | \
-                                  FIF_OTHER_BSS)
+                                  FIF_OTHER_BSS | \
+                                  FIF_PROBE_REQ)
 
 static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
                                        unsigned int changed,
                                        unsigned int *total,u64 multicast)
 {
         struct wl1251 *wl = hw->priv;
+        int ret;
 
         wl1251_debug(DEBUG_MAC80211, "mac80211 configure filter");
 
@@ -698,7 +678,7 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
                 /* no filters which we support changed */
                 return;
 
-        /* FIXME: wl->rx_config and wl->rx_filter are not protected */
+        mutex_lock(&wl->mutex);
 
         wl->rx_config = WL1251_DEFAULT_RX_CONFIG;
         wl->rx_filter = WL1251_DEFAULT_RX_FILTER;
@@ -721,15 +701,25 @@ static void wl1251_op_configure_filter(struct ieee80211_hw *hw,
         }
         if (*total & FIF_CONTROL)
                 wl->rx_filter |= CFG_RX_CTL_EN;
-        if (*total & FIF_OTHER_BSS)
-                wl->rx_filter &= ~CFG_BSSID_FILTER_EN;
+        if (*total & FIF_OTHER_BSS || is_zero_ether_addr(wl->bssid))
+                wl->rx_config &= ~CFG_BSSID_FILTER_EN;
+        if (*total & FIF_PROBE_REQ)
+                wl->rx_filter |= CFG_RX_PREQ_EN;
 
-        /*
-         * FIXME: workqueues need to be properly cancelled on stop(), for
-         * now let's just disable changing the filter settings. They will
-         * be updated any on config().
-         */
-        /* schedule_work(&wl->filter_work); */
+        if (wl->state == WL1251_STATE_OFF)
+                goto out;
+
+        ret = wl1251_ps_elp_wakeup(wl);
+        if (ret < 0)
+                goto out;
+
+        /* send filters to firmware */
+        wl1251_acx_rx_config(wl, wl->rx_config, wl->rx_filter);
+
+        wl1251_ps_elp_sleep(wl);
+
+out:
+        mutex_unlock(&wl->mutex);
 }
 
 /* HW encryption */
@@ -1390,7 +1380,6 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
 
         skb_queue_head_init(&wl->tx_queue);
 
-        INIT_WORK(&wl->filter_work, wl1251_filter_work);
         INIT_DELAYED_WORK(&wl->elp_work, wl1251_elp_work);
         wl->channel = WL1251_DEFAULT_CHANNEL;
         wl->scanning = false;
diff --git a/drivers/net/wireless/ti/wl1251/wl1251.h b/drivers/net/wireless/ti/wl1251/wl1251.h
index 9d8f5816c6f..fd02060038d 100644
--- a/drivers/net/wireless/ti/wl1251/wl1251.h
+++ b/drivers/net/wireless/ti/wl1251/wl1251.h
@@ -315,7 +315,6 @@ struct wl1251 {
         bool tx_queue_stopped;
 
         struct work_struct tx_work;
-        struct work_struct filter_work;
 
         /* Pending TX frames */
         struct sk_buff *tx_frames[16];
diff --git a/drivers/net/wireless/ti/wl12xx/cmd.c b/drivers/net/wireless/ti/wl12xx/cmd.c
index 50ba7480b79..30be784a40d 100644
--- a/drivers/net/wireless/ti/wl12xx/cmd.c
+++ b/drivers/net/wireless/ti/wl12xx/cmd.c
@@ -174,7 +174,7 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
         struct wl1271_nvs_file *nvs = (struct wl1271_nvs_file *)wl->nvs;
         struct wl1271_radio_parms_cmd *radio_parms;
         struct wl1271_ini_general_params *gp = &nvs->general_params;
-        int ret;
+        int ret, fem_idx;
 
         if (!wl->nvs)
                 return -ENODEV;
@@ -185,11 +185,13 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
 
         radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
 
+        fem_idx = WL12XX_FEM_TO_NVS_ENTRY(gp->tx_bip_fem_manufacturer);
+
         /* 2.4GHz parameters */
         memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
                sizeof(struct wl1271_ini_band_params_2));
         memcpy(&radio_parms->dyn_params_2,
-               &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+               &nvs->dyn_radio_params_2[fem_idx].params,
                sizeof(struct wl1271_ini_fem_params_2));
 
         /* 5GHz parameters */
@@ -197,7 +199,7 @@ int wl1271_cmd_radio_parms(struct wl1271 *wl)
                &nvs->stat_radio_params_5,
                sizeof(struct wl1271_ini_band_params_5));
         memcpy(&radio_parms->dyn_params_5,
-               &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+               &nvs->dyn_radio_params_5[fem_idx].params,
                sizeof(struct wl1271_ini_fem_params_5));
 
         wl1271_dump(DEBUG_CMD, "TEST_CMD_INI_FILE_RADIO_PARAM: ",
@@ -216,7 +218,7 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
         struct wl128x_nvs_file *nvs = (struct wl128x_nvs_file *)wl->nvs;
         struct wl128x_radio_parms_cmd *radio_parms;
         struct wl128x_ini_general_params *gp = &nvs->general_params;
-        int ret;
+        int ret, fem_idx;
 
         if (!wl->nvs)
                 return -ENODEV;
@@ -227,11 +229,13 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
 
         radio_parms->test.id = TEST_CMD_INI_FILE_RADIO_PARAM;
 
+        fem_idx = WL12XX_FEM_TO_NVS_ENTRY(gp->tx_bip_fem_manufacturer);
+
         /* 2.4GHz parameters */
         memcpy(&radio_parms->static_params_2, &nvs->stat_radio_params_2,
                sizeof(struct wl128x_ini_band_params_2));
         memcpy(&radio_parms->dyn_params_2,
-               &nvs->dyn_radio_params_2[gp->tx_bip_fem_manufacturer].params,
+               &nvs->dyn_radio_params_2[fem_idx].params,
                sizeof(struct wl128x_ini_fem_params_2));
 
         /* 5GHz parameters */
@@ -239,7 +243,7 @@ int wl128x_cmd_radio_parms(struct wl1271 *wl)
                &nvs->stat_radio_params_5,
                sizeof(struct wl128x_ini_band_params_5));
         memcpy(&radio_parms->dyn_params_5,
-               &nvs->dyn_radio_params_5[gp->tx_bip_fem_manufacturer].params,
+               &nvs->dyn_radio_params_5[fem_idx].params,
                sizeof(struct wl128x_ini_fem_params_5));
 
         radio_parms->fem_vendor_and_options = nvs->fem_vendor_and_options;
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index 85d1600ee34..47ba2e0017f 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -246,6 +246,7 @@ static struct wlcore_conf wl12xx_conf = {
                 .forced_ps                   = false,
                 .keep_alive_interval         = 55000,
                 .max_listen_interval         = 20,
+                .sta_sleep_auth              = WL1271_PSM_ILLEGAL,
         },
         .itrim = {
                 .enable = false,
@@ -597,8 +598,10 @@ static const int wl12xx_rtable[REG_TABLE_LEN] = {
 #define WL128X_FW_NAME_SINGLE   "ti-connectivity/wl128x-fw-4-sr.bin"
 #define WL128X_PLT_FW_NAME      "ti-connectivity/wl128x-fw-4-plt.bin"
 
-static void wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
+static int wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 {
+        int ret;
+
         if (wl->chip.id != CHIP_ID_1283_PG20) {
                 struct wl1271_acx_mem_map *wl_mem_map = wl->target_mem_map;
                 struct wl127x_rx_mem_pool_addr rx_mem_addr;
@@ -615,9 +618,13 @@ static void wl127x_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 
                 rx_mem_addr.addr_extra = rx_mem_addr.addr + 4;
 
-                wl1271_write(wl, WL1271_SLV_REG_DATA,
-                             &rx_mem_addr, sizeof(rx_mem_addr), false);
+                ret = wlcore_write(wl, WL1271_SLV_REG_DATA, &rx_mem_addr,
+                                   sizeof(rx_mem_addr), false);
+                if (ret < 0)
+                        return ret;
         }
+
+        return 0;
 }
 
 static int wl12xx_identify_chip(struct wl1271 *wl)
@@ -681,64 +688,95 @@ out:
         return ret;
 }
 
-static void wl12xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
+static int __must_check wl12xx_top_reg_write(struct wl1271 *wl, int addr,
+                                             u16 val)
 {
+        int ret;
+
         /* write address >> 1 + 0x30000 to OCP_POR_CTR */
         addr = (addr >> 1) + 0x30000;
-        wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+        ret = wlcore_write32(wl, WL12XX_OCP_POR_CTR, addr);
+        if (ret < 0)
+                goto out;
 
         /* write value to OCP_POR_WDATA */
-        wl1271_write32(wl, WL12XX_OCP_DATA_WRITE, val);
+        ret = wlcore_write32(wl, WL12XX_OCP_DATA_WRITE, val);
+        if (ret < 0)
+                goto out;
 
         /* write 1 to OCP_CMD */
-        wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_WRITE);
+        ret = wlcore_write32(wl, WL12XX_OCP_CMD, OCP_CMD_WRITE);
+        if (ret < 0)
+                goto out;
+
+out:
+        return ret;
 }
 
-static u16 wl12xx_top_reg_read(struct wl1271 *wl, int addr)
+static int __must_check wl12xx_top_reg_read(struct wl1271 *wl, int addr,
+                                            u16 *out)
 {
         u32 val;
         int timeout = OCP_CMD_LOOP;
+        int ret;
 
         /* write address >> 1 + 0x30000 to OCP_POR_CTR */
         addr = (addr >> 1) + 0x30000;
-        wl1271_write32(wl, WL12XX_OCP_POR_CTR, addr);
+        ret = wlcore_write32(wl, WL12XX_OCP_POR_CTR, addr);
+        if (ret < 0)
+                return ret;
 
         /* write 2 to OCP_CMD */
-        wl1271_write32(wl, WL12XX_OCP_CMD, OCP_CMD_READ);
+        ret = wlcore_write32(wl, WL12XX_OCP_CMD, OCP_CMD_READ);
+        if (ret < 0)
+                return ret;
 
         /* poll for data ready */
         do {
-                val = wl1271_read32(wl, WL12XX_OCP_DATA_READ);
+                ret = wlcore_read32(wl, WL12XX_OCP_DATA_READ, &val);
+                if (ret < 0)
+                        return ret;
         } while (!(val & OCP_READY_MASK) && --timeout);
 
         if (!timeout) {
                 wl1271_warning("Top register access timed out.");
-                return 0xffff;
+                return -ETIMEDOUT;
         }
 
         /* check data status and return if OK */
-        if ((val & OCP_STATUS_MASK) == OCP_STATUS_OK)
-                return val & 0xffff;
-        else {
+        if ((val & OCP_STATUS_MASK) != OCP_STATUS_OK) {
                 wl1271_warning("Top register access returned error.");
-                return 0xffff;
+                return -EIO;
         }
+
+        if (out)
+                *out = val & 0xffff;
+
+        return 0;
 }
 
 static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
 {
         u16 spare_reg;
+        int ret;
 
         /* Mask bits [2] & [8:4] in the sys_clk_cfg register */
-        spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+        ret = wl12xx_top_reg_read(wl, WL_SPARE_REG, &spare_reg);
+        if (ret < 0)
+                return ret;
+
         if (spare_reg == 0xFFFF)
                 return -EFAULT;
         spare_reg |= (BIT(3) | BIT(5) | BIT(6));
-        wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+        ret = wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+        if (ret < 0)
+                return ret;
 
         /* Enable FREF_CLK_REQ & mux MCS and coex PLLs to FREF */
-        wl12xx_top_reg_write(wl, SYS_CLK_CFG_REG,
-                             WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
+        ret = wl12xx_top_reg_write(wl, SYS_CLK_CFG_REG,
+                                   WL_CLK_REQ_TYPE_PG2 | MCS_PLL_CLK_SEL_FREF);
+        if (ret < 0)
+                return ret;
 
         /* Delay execution for 15msec, to let the HW settle */
         mdelay(15);
@@ -749,8 +787,12 @@ static int wl128x_switch_tcxo_to_fref(struct wl1271 *wl)
 static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
 {
         u16 tcxo_detection;
+        int ret;
+
+        ret = wl12xx_top_reg_read(wl, TCXO_CLK_DETECT_REG, &tcxo_detection);
+        if (ret < 0)
+                return false;
 
-        tcxo_detection = wl12xx_top_reg_read(wl, TCXO_CLK_DETECT_REG);
         if (tcxo_detection & TCXO_DET_FAILED)
                 return false;
 
@@ -760,8 +802,12 @@ static bool wl128x_is_tcxo_valid(struct wl1271 *wl)
 static bool wl128x_is_fref_valid(struct wl1271 *wl)
 {
         u16 fref_detection;
+        int ret;
+
+        ret = wl12xx_top_reg_read(wl, FREF_CLK_DETECT_REG, &fref_detection);
+        if (ret < 0)
+                return false;
 
-        fref_detection = wl12xx_top_reg_read(wl, FREF_CLK_DETECT_REG);
         if (fref_detection & FREF_CLK_DETECT_FAIL)
                 return false;
 
@@ -770,11 +816,21 @@ static bool wl128x_is_fref_valid(struct wl1271 *wl)
 
 static int wl128x_manually_configure_mcs_pll(struct wl1271 *wl)
 {
-        wl12xx_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
-        wl12xx_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
-        wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, MCS_PLL_CONFIG_REG_VAL);
+        int ret;
 
-        return 0;
+        ret = wl12xx_top_reg_write(wl, MCS_PLL_M_REG, MCS_PLL_M_REG_VAL);
+        if (ret < 0)
+                goto out;
+
+        ret = wl12xx_top_reg_write(wl, MCS_PLL_N_REG, MCS_PLL_N_REG_VAL);
+        if (ret < 0)
+                goto out;
+
+        ret = wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG,
+                                   MCS_PLL_CONFIG_REG_VAL);
+
+out:
+        return ret;
 }
 
 static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
@@ -783,13 +839,19 @@ static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
         u16 pll_config;
         u8 input_freq;
         struct wl12xx_priv *priv = wl->priv;
+        int ret;
 
         /* Mask bits [3:1] in the sys_clk_cfg register */
-        spare_reg = wl12xx_top_reg_read(wl, WL_SPARE_REG);
+        ret = wl12xx_top_reg_read(wl, WL_SPARE_REG, &spare_reg);
+        if (ret < 0)
+                return ret;
+
         if (spare_reg == 0xFFFF)
                 return -EFAULT;
         spare_reg |= BIT(2);
-        wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+        ret = wl12xx_top_reg_write(wl, WL_SPARE_REG, spare_reg);
+        if (ret < 0)
+                return ret;
 
         /* Handle special cases of the TCXO clock */
         if (priv->tcxo_clock == WL12XX_TCXOCLOCK_16_8 ||
@@ -799,14 +861,17 @@ static int wl128x_configure_mcs_pll(struct wl1271 *wl, int clk)
         /* Set the input frequency according to the selected clock source */
         input_freq = (clk & 1) + 1;
 
-        pll_config = wl12xx_top_reg_read(wl, MCS_PLL_CONFIG_REG);
+        ret = wl12xx_top_reg_read(wl, MCS_PLL_CONFIG_REG, &pll_config);
+        if (ret < 0)
+                return ret;
+
         if (pll_config == 0xFFFF)
                 return -EFAULT;
         pll_config |= (input_freq << MCS_SEL_IN_FREQ_SHIFT);
         pll_config |= MCS_PLL_ENABLE_HP;
-        wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
+        ret = wl12xx_top_reg_write(wl, MCS_PLL_CONFIG_REG, pll_config);
 
-        return 0;
+        return ret;
 }
 
 /*
@@ -820,6 +885,7 @@ static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
 {
         struct wl12xx_priv *priv = wl->priv;
         u16 sys_clk_cfg;
+        int ret;
 
         /* For XTAL-only modes, FREF will be used after switching from TCXO */
         if (priv->ref_clock == WL12XX_REFCLOCK_26_XTAL ||
@@ -830,7 +896,10 @@ static int wl128x_boot_clk(struct wl1271 *wl, int *selected_clock)
         }
 
         /* Query the HW, to determine which clock source we should use */
-        sys_clk_cfg = wl12xx_top_reg_read(wl, SYS_CLK_CFG_REG);
+        ret = wl12xx_top_reg_read(wl, SYS_CLK_CFG_REG, &sys_clk_cfg);
+        if (ret < 0)
+                return ret;
+
         if (sys_clk_cfg == 0xFFFF)
                 return -EINVAL;
         if (sys_clk_cfg & PRCM_CM_EN_MUX_WLAN_FREF)
@@ -865,6 +934,7 @@ static int wl127x_boot_clk(struct wl1271 *wl)
         struct wl12xx_priv *priv = wl->priv;
         u32 pause;
         u32 clk;
+        int ret;
 
         if (WL127X_PG_GET_MAJOR(wl->hw_pg_ver) < 3)
                 wl->quirks |= WLCORE_QUIRK_END_OF_TRANSACTION;
@@ -885,48 +955,74 @@ static int wl127x_boot_clk(struct wl1271 *wl)
         if (priv->ref_clock != CONF_REF_CLK_19_2_E) {
                 u16 val;
                 /* Set clock type (open drain) */
-                val = wl12xx_top_reg_read(wl, OCP_REG_CLK_TYPE);
+                ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_TYPE, &val);
+                if (ret < 0)
+                        goto out;
+
                 val &= FREF_CLK_TYPE_BITS;
-                wl12xx_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
+                ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_TYPE, val);
+                if (ret < 0)
+                        goto out;
 
                 /* Set clock pull mode (no pull) */
-                val = wl12xx_top_reg_read(wl, OCP_REG_CLK_PULL);
+                ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_PULL, &val);
+                if (ret < 0)
+                        goto out;
+
                 val |= NO_PULL;
-                wl12xx_top_reg_write(wl, OCP_REG_CLK_PULL, val);
+                ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_PULL, val);
+                if (ret < 0)
+                        goto out;
         } else {
                 u16 val;
                 /* Set clock polarity */
-                val = wl12xx_top_reg_read(wl, OCP_REG_CLK_POLARITY);
+                ret = wl12xx_top_reg_read(wl, OCP_REG_CLK_POLARITY, &val);
+                if (ret < 0)
+                        goto out;
+
                 val &= FREF_CLK_POLARITY_BITS;
                 val |= CLK_REQ_OUTN_SEL;
-                wl12xx_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
+                ret = wl12xx_top_reg_write(wl, OCP_REG_CLK_POLARITY, val);
+                if (ret < 0)
+                        goto out;
         }
 
-        wl1271_write32(wl, WL12XX_PLL_PARAMETERS, clk);
+        ret = wlcore_write32(wl, WL12XX_PLL_PARAMETERS, clk);
+        if (ret < 0)
+                goto out;
 
-        pause = wl1271_read32(wl, WL12XX_PLL_PARAMETERS);
+        ret = wlcore_read32(wl, WL12XX_PLL_PARAMETERS, &pause);
+        if (ret < 0)
+                goto out;
 
         wl1271_debug(DEBUG_BOOT, "pause1 0x%x", pause);
 
         pause &= ~(WU_COUNTER_PAUSE_VAL);
         pause |= WU_COUNTER_PAUSE_VAL;
-        wl1271_write32(wl, WL12XX_WU_COUNTER_PAUSE, pause);
+        ret = wlcore_write32(wl, WL12XX_WU_COUNTER_PAUSE, pause);
 
-        return 0;
+out:
+        return ret;
 }
 
 static int wl1271_boot_soft_reset(struct wl1271 *wl)
 {
         unsigned long timeout;
         u32 boot_data;
+        int ret = 0;
 
         /* perform soft reset */
-        wl1271_write32(wl, WL12XX_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
+        ret = wlcore_write32(wl, WL12XX_SLV_SOFT_RESET, ACX_SLV_SOFT_RESET_BIT);
+        if (ret < 0)
+                goto out;
 
         /* SOFT_RESET is self clearing */
         timeout = jiffies + usecs_to_jiffies(SOFT_RESET_MAX_TIME);
         while (1) {
-                boot_data = wl1271_read32(wl, WL12XX_SLV_SOFT_RESET);
+                ret = wlcore_read32(wl, WL12XX_SLV_SOFT_RESET, &boot_data);
+                if (ret < 0)
+                        goto out;
+
                 wl1271_debug(DEBUG_BOOT, "soft reset bootdata 0x%x", boot_data);
                 if ((boot_data & ACX_SLV_SOFT_RESET_BIT) == 0)
                         break;
@@ -942,12 +1038,15 @@ static int wl1271_boot_soft_reset(struct wl1271 *wl)
         }
 
         /* disable Rx/Tx */
-        wl1271_write32(wl, WL12XX_ENABLE, 0x0);
+        ret = wlcore_write32(wl, WL12XX_ENABLE, 0x0);
+        if (ret < 0)
+                goto out;
 
         /* disable auto calibration on start*/
-        wl1271_write32(wl, WL12XX_SPARE_A2, 0xffff);
+        ret = wlcore_write32(wl, WL12XX_SPARE_A2, 0xffff);
 
-        return 0;
+out:
+        return ret;
 }
 
 static int wl12xx_pre_boot(struct wl1271 *wl)
@@ -968,16 +1067,23 @@ static int wl12xx_pre_boot(struct wl1271 *wl)
         }
 
         /* Continue the ELP wake up sequence */
-        wl1271_write32(wl, WL12XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+        ret = wlcore_write32(wl, WL12XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+        if (ret < 0)
+                goto out;
+
         udelay(500);
 
-        wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+        if (ret < 0)
+                goto out;
 
         /* Read-modify-write DRPW_SCRATCH_START register (see next state)
            to be used by DRPw FW. The RTRIM value will be added by the FW
            before taking DRPw out of reset */
 
-        clk = wl1271_read32(wl, WL12XX_DRPW_SCRATCH_START);
+        ret = wlcore_read32(wl, WL12XX_DRPW_SCRATCH_START, &clk);
+        if (ret < 0)
+                goto out;
 
         wl1271_debug(DEBUG_BOOT, "clk2 0x%x", clk);
 
@@ -986,12 +1092,18 @@ static int wl12xx_pre_boot(struct wl1271 *wl)
         else
                 clk |= (priv->ref_clock << 1) << 4;
 
-        wl1271_write32(wl, WL12XX_DRPW_SCRATCH_START, clk);
+        ret = wlcore_write32(wl, WL12XX_DRPW_SCRATCH_START, clk);
+        if (ret < 0)
+                goto out;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        if (ret < 0)
+                goto out;
 
         /* Disable interrupts */
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+        if (ret < 0)
+                goto out;
 
         ret = wl1271_boot_soft_reset(wl);
         if (ret < 0)
@@ -1001,47 +1113,72 @@ out:
         return ret;
 }
 
-static void wl12xx_pre_upload(struct wl1271 *wl)
+static int wl12xx_pre_upload(struct wl1271 *wl)
 {
-        u32 tmp, polarity;
+        u32 tmp;
+        u16 polarity;
+        int ret;
 
         /* write firmware's last address (ie. it's length) to
          * ACX_EEPROMLESS_IND_REG */
         wl1271_debug(DEBUG_BOOT, "ACX_EEPROMLESS_IND_REG");
 
-        wl1271_write32(wl, WL12XX_EEPROMLESS_IND, WL12XX_EEPROMLESS_IND);
+        ret = wlcore_write32(wl, WL12XX_EEPROMLESS_IND, WL12XX_EEPROMLESS_IND);
+        if (ret < 0)
+                goto out;
 
-        tmp = wlcore_read_reg(wl, REG_CHIP_ID_B);
+        ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &tmp);
+        if (ret < 0)
+                goto out;
 
         wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
 
         /* 6. read the EEPROM parameters */
-        tmp = wl1271_read32(wl, WL12XX_SCR_PAD2);
+        ret = wlcore_read32(wl, WL12XX_SCR_PAD2, &tmp);
+        if (ret < 0)
+                goto out;
 
         /* WL1271: The reference driver skips steps 7 to 10 (jumps directly
          * to upload_fw) */
 
-        if (wl->chip.id == CHIP_ID_1283_PG20)
-                wl12xx_top_reg_write(wl, SDIO_IO_DS, HCI_IO_DS_6MA);
+        if (wl->chip.id == CHIP_ID_1283_PG20) {
+                ret = wl12xx_top_reg_write(wl, SDIO_IO_DS, HCI_IO_DS_6MA);
+                if (ret < 0)
+                        goto out;
+        }
 
         /* polarity must be set before the firmware is loaded */
-        polarity = wl12xx_top_reg_read(wl, OCP_REG_POLARITY);
+        ret = wl12xx_top_reg_read(wl, OCP_REG_POLARITY, &polarity);
+        if (ret < 0)
+                goto out;
 
         /* We use HIGH polarity, so unset the LOW bit */
         polarity &= ~POLARITY_LOW;
-        wl12xx_top_reg_write(wl, OCP_REG_POLARITY, polarity);
+        ret = wl12xx_top_reg_write(wl, OCP_REG_POLARITY, polarity);
 
+out:
+        return ret;
 }
 
-static void wl12xx_enable_interrupts(struct wl1271 *wl)
+static int wl12xx_enable_interrupts(struct wl1271 *wl)
 {
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL12XX_ACX_ALL_EVENTS_VECTOR);
+        int ret;
+
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+                               WL12XX_ACX_ALL_EVENTS_VECTOR);
+        if (ret < 0)
+                goto out;
 
         wlcore_enable_interrupts(wl);
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK,
-                         WL1271_ACX_INTR_ALL & ~(WL12XX_INTR_MASK));
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+                               WL1271_ACX_INTR_ALL & ~(WL12XX_INTR_MASK));
+        if (ret < 0)
+                goto out;
 
-        wl1271_write32(wl, WL12XX_HI_CFG, HI_CFG_DEF_VAL);
+        ret = wlcore_write32(wl, WL12XX_HI_CFG, HI_CFG_DEF_VAL);
+
+out:
+        return ret;
 }
 
 static int wl12xx_boot(struct wl1271 *wl)
@@ -1056,7 +1193,9 @@ static int wl12xx_boot(struct wl1271 *wl)
         if (ret < 0)
                 goto out;
 
-        wl12xx_pre_upload(wl);
+        ret = wl12xx_pre_upload(wl);
+        if (ret < 0)
+                goto out;
 
         ret = wlcore_boot_upload_firmware(wl);
         if (ret < 0)
@@ -1066,22 +1205,30 @@ static int wl12xx_boot(struct wl1271 *wl)
         if (ret < 0)
                 goto out;
 
-        wl12xx_enable_interrupts(wl);
+        ret = wl12xx_enable_interrupts(wl);
 
 out:
         return ret;
 }
 
-static void wl12xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
+static int wl12xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
                                void *buf, size_t len)
 {
-        wl1271_write(wl, cmd_box_addr, buf, len, false);
-        wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_CMD);
+        int ret;
+
+        ret = wlcore_write(wl, cmd_box_addr, buf, len, false);
+        if (ret < 0)
+                return ret;
+
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_CMD);
+
+        return ret;
 }
 
-static void wl12xx_ack_event(struct wl1271 *wl)
+static int wl12xx_ack_event(struct wl1271 *wl)
 {
-        wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL12XX_INTR_TRIG_EVENT_ACK);
+        return wlcore_write_reg(wl, REG_INTERRUPT_TRIG,
+                                WL12XX_INTR_TRIG_EVENT_ACK);
 }
 
 static u32 wl12xx_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
@@ -1161,13 +1308,13 @@ static u32 wl12xx_get_rx_packet_len(struct wl1271 *wl, void *rx_data,
         return data_len - sizeof(*desc) - desc->pad_len;
 }
 
-static void wl12xx_tx_delayed_compl(struct wl1271 *wl)
+static int wl12xx_tx_delayed_compl(struct wl1271 *wl)
 {
         if (wl->fw_status_1->tx_results_counter ==
             (wl->tx_results_count & 0xff))
-                return;
+                return 0;
 
-        wl1271_tx_complete(wl);
+        return wlcore_tx_complete(wl);
 }
 
 static int wl12xx_hw_init(struct wl1271 *wl)
@@ -1268,39 +1415,58 @@ static bool wl12xx_mac_in_fuse(struct wl1271 *wl)
         return supported;
 }
 
-static void wl12xx_get_fuse_mac(struct wl1271 *wl)
+static int wl12xx_get_fuse_mac(struct wl1271 *wl)
 {
         u32 mac1, mac2;
+        int ret;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_DRPW]);
+        if (ret < 0)
+                goto out;
 
-        mac1 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1);
-        mac2 = wl1271_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2);
+        ret = wlcore_read32(wl, WL12XX_REG_FUSE_BD_ADDR_1, &mac1);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_read32(wl, WL12XX_REG_FUSE_BD_ADDR_2, &mac2);
+        if (ret < 0)
+                goto out;
 
         /* these are the two parts of the BD_ADDR */
         wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
                 ((mac1 & 0xff000000) >> 24);
         wl->fuse_nic_addr = mac1 & 0xffffff;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+
+out:
+        return ret;
 }
 
-static s8 wl12xx_get_pg_ver(struct wl1271 *wl)
+static int wl12xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
 {
-        u32 die_info;
+        u16 die_info;
+        int ret;
 
         if (wl->chip.id == CHIP_ID_1283_PG20)
-                die_info = wl12xx_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1);
+                ret = wl12xx_top_reg_read(wl, WL128X_REG_FUSE_DATA_2_1,
+                                          &die_info);
         else
-                die_info = wl12xx_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1);
+                ret = wl12xx_top_reg_read(wl, WL127X_REG_FUSE_DATA_2_1,
+                                          &die_info);
 
-        return (s8) (die_info & PG_VER_MASK) >> PG_VER_OFFSET;
+        if (ret >= 0 && ver)
+                *ver = (s8)((die_info & PG_VER_MASK) >> PG_VER_OFFSET);
+
+        return ret;
 }
 
-static void wl12xx_get_mac(struct wl1271 *wl)
+static int wl12xx_get_mac(struct wl1271 *wl)
 {
         if (wl12xx_mac_in_fuse(wl))
-                wl12xx_get_fuse_mac(wl);
+                return wl12xx_get_fuse_mac(wl);
+
+        return 0;
 }
 
 static void wl12xx_set_tx_desc_csum(struct wl1271 *wl,
@@ -1448,10 +1614,8 @@ static int __devinit wl12xx_probe(struct platform_device *pdev)
         wl->hw_min_ht_rate = WL12XX_CONF_HW_RXTX_RATE_MCS0;
         wl->fw_status_priv_len = 0;
         wl->stats.fw_stats_len = sizeof(struct wl12xx_acx_statistics);
-        memcpy(&wl->ht_cap[IEEE80211_BAND_2GHZ], &wl12xx_ht_cap,
-               sizeof(wl12xx_ht_cap));
-        memcpy(&wl->ht_cap[IEEE80211_BAND_5GHZ], &wl12xx_ht_cap,
-               sizeof(wl12xx_ht_cap));
+        wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ, &wl12xx_ht_cap);
+        wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ, &wl12xx_ht_cap);
         wl12xx_conf_init(wl);
 
         if (!fref_param) {
diff --git a/drivers/net/wireless/ti/wl18xx/acx.h b/drivers/net/wireless/ti/wl18xx/acx.h
index ebbaf611e97..e2609a6b734 100644
--- a/drivers/net/wireless/ti/wl18xx/acx.h
+++ b/drivers/net/wireless/ti/wl18xx/acx.h
@@ -32,25 +32,21 @@ enum {
 /* numbers of bits the length field takes (add 1 for the actual number) */
 #define WL18XX_HOST_IF_LEN_SIZE_FIELD 15
 
-#define WL18XX_ACX_EVENTS_VECTOR_PG1    (WL1271_ACX_INTR_WATCHDOG      | \
-                                        WL1271_ACX_INTR_INIT_COMPLETE | \
-                                        WL1271_ACX_INTR_EVENT_A       | \
-                                        WL1271_ACX_INTR_EVENT_B       | \
-                                        WL1271_ACX_INTR_CMD_COMPLETE  | \
-                                        WL1271_ACX_INTR_HW_AVAILABLE  | \
-                                        WL1271_ACX_INTR_DATA)
-
-#define WL18XX_ACX_EVENTS_VECTOR_PG2    (WL18XX_ACX_EVENTS_VECTOR_PG1 | \
-                                        WL1271_ACX_SW_INTR_WATCHDOG)
-
-#define WL18XX_INTR_MASK_PG1            (WL1271_ACX_INTR_WATCHDOG      | \
-                                        WL1271_ACX_INTR_EVENT_A       | \
-                                        WL1271_ACX_INTR_EVENT_B       | \
-                                        WL1271_ACX_INTR_HW_AVAILABLE  | \
-                                        WL1271_ACX_INTR_DATA)
-
-#define WL18XX_INTR_MASK_PG2            (WL18XX_INTR_MASK_PG1         | \
-                                        WL1271_ACX_SW_INTR_WATCHDOG)
+#define WL18XX_ACX_EVENTS_VECTOR        (WL1271_ACX_INTR_WATCHDOG       | \
+                                         WL1271_ACX_INTR_INIT_COMPLETE  | \
+                                         WL1271_ACX_INTR_EVENT_A        | \
+                                         WL1271_ACX_INTR_EVENT_B        | \
+                                         WL1271_ACX_INTR_CMD_COMPLETE   | \
+                                         WL1271_ACX_INTR_HW_AVAILABLE   | \
+                                         WL1271_ACX_INTR_DATA           | \
+                                         WL1271_ACX_SW_INTR_WATCHDOG)
+
+#define WL18XX_INTR_MASK                (WL1271_ACX_INTR_WATCHDOG       | \
+                                         WL1271_ACX_INTR_EVENT_A        | \
+                                         WL1271_ACX_INTR_EVENT_B        | \
+                                         WL1271_ACX_INTR_HW_AVAILABLE   | \
+                                         WL1271_ACX_INTR_DATA           | \
+                                         WL1271_ACX_SW_INTR_WATCHDOG)
 
 struct wl18xx_acx_host_config_bitmap {
         struct acx_header header;
diff --git a/drivers/net/wireless/ti/wl18xx/io.c b/drivers/net/wireless/ti/wl18xx/io.c
index 598c057e722..0c06ccfd1b8 100644
--- a/drivers/net/wireless/ti/wl18xx/io.c
+++ b/drivers/net/wireless/ti/wl18xx/io.c
@@ -24,37 +24,52 @@
 
 #include "io.h"
 
-void wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
+int wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val)
 {
         u32 tmp;
+        int ret;
 
         if (WARN_ON(addr % 2))
-                return;
+                return -EINVAL;
 
         if ((addr % 4) == 0) {
-                tmp = wl1271_read32(wl, addr);
+                ret = wlcore_read32(wl, addr, &tmp);
+                if (ret < 0)
+                        goto out;
+
                 tmp = (tmp & 0xffff0000) | val;
-                wl1271_write32(wl, addr, tmp);
+                ret = wlcore_write32(wl, addr, tmp);
         } else {
-                tmp = wl1271_read32(wl, addr - 2);
+                ret = wlcore_read32(wl, addr - 2, &tmp);
+                if (ret < 0)
+                        goto out;
+
                 tmp = (tmp & 0xffff) | (val << 16);
-                wl1271_write32(wl, addr - 2, tmp);
+                ret = wlcore_write32(wl, addr - 2, tmp);
         }
+
+out:
+        return ret;
 }
 
-u16 wl18xx_top_reg_read(struct wl1271 *wl, int addr)
+int wl18xx_top_reg_read(struct wl1271 *wl, int addr, u16 *out)
 {
         u32 val;
+        int ret;
 
         if (WARN_ON(addr % 2))
-                return 0;
+                return -EINVAL;
 
         if ((addr % 4) == 0) {
                 /* address is 4-bytes aligned */
-                val = wl1271_read32(wl, addr);
-                return val & 0xffff;
+                ret = wlcore_read32(wl, addr, &val);
+                if (ret >= 0 && out)
+                        *out = val & 0xffff;
         } else {
-                val = wl1271_read32(wl, addr - 2);
-                return (val & 0xffff0000) >> 16;
+                ret = wlcore_read32(wl, addr - 2, &val);
+                if (ret >= 0 && out)
+                        *out = (val & 0xffff0000) >> 16;
         }
+
+        return ret;
 }
diff --git a/drivers/net/wireless/ti/wl18xx/io.h b/drivers/net/wireless/ti/wl18xx/io.h
index be4e126ff61..c32ae30277d 100644
--- a/drivers/net/wireless/ti/wl18xx/io.h
+++ b/drivers/net/wireless/ti/wl18xx/io.h
@@ -22,7 +22,7 @@
 #ifndef __WL18XX_IO_H__
 #define __WL18XX_IO_H__
 
-void wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val);
-u16 wl18xx_top_reg_read(struct wl1271 *wl, int addr);
+int __must_check wl18xx_top_reg_write(struct wl1271 *wl, int addr, u16 val);
+int __must_check wl18xx_top_reg_read(struct wl1271 *wl, int addr, u16 *out);
 
 #endif /* __WL18XX_IO_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index ed9c3650e08..5e583be8f67 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -43,10 +43,11 @@
 
 #define WL18XX_RX_CHECKSUM_MASK      0x40
 
-static char *ht_mode_param = "wide";
+static char *ht_mode_param = "default";
 static char *board_type_param = "hdk";
 static bool checksum_param = false;
 static bool enable_11a_param = true;
+static int num_rx_desc_param = -1;
 
 /* phy paramters */
 static int dc2dc_param = -1;
@@ -372,6 +373,7 @@ static struct wlcore_conf wl18xx_conf = {
                 .forced_ps                   = false,
                 .keep_alive_interval         = 55000,
                 .max_listen_interval         = 20,
+                .sta_sleep_auth              = WL1271_PSM_ILLEGAL,
         },
         .itrim = {
                 .enable = false,
@@ -606,24 +608,15 @@ static int wl18xx_identify_chip(struct wl1271 *wl)
                 wl->plt_fw_name = WL18XX_FW_NAME;
                 wl->quirks |= WLCORE_QUIRK_NO_ELP |
                               WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN |
+                              WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN |
                               WLCORE_QUIRK_TX_PAD_LAST_FRAME;
-
                 break;
         case CHIP_ID_185x_PG10:
-                wl1271_debug(DEBUG_BOOT, "chip id 0x%x (185x PG10)",
-                             wl->chip.id);
-                wl->sr_fw_name = WL18XX_FW_NAME;
-                /* wl18xx uses the same firmware for PLT */
-                wl->plt_fw_name = WL18XX_FW_NAME;
-                wl->quirks |= WLCORE_QUIRK_NO_ELP |
-                        WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED |
-                        WLCORE_QUIRK_RX_BLOCKSIZE_ALIGN |
-                        WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;
-
-                /* PG 1.0 has some problems with MCS_13, so disable it */
-                wl->ht_cap[IEEE80211_BAND_2GHZ].mcs.rx_mask[1] &= ~BIT(5);
+                wl1271_warning("chip id 0x%x (185x PG10) is deprecated",
+                               wl->chip.id);
+                ret = -ENODEV;
+                goto out;
 
-                break;
         default:
                 wl1271_warning("unsupported chip id: 0x%x", wl->chip.id);
                 ret = -ENODEV;
@@ -634,123 +627,178 @@ out:
         return ret;
 }
 
-static void wl18xx_set_clk(struct wl1271 *wl)
+static int wl18xx_set_clk(struct wl1271 *wl)
 {
-        u32 clk_freq;
+        u16 clk_freq;
+        int ret;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        if (ret < 0)
+                goto out;
 
         /* TODO: PG2: apparently we need to read the clk type */
 
-        clk_freq = wl18xx_top_reg_read(wl, PRIMARY_CLK_DETECT);
+        ret = wl18xx_top_reg_read(wl, PRIMARY_CLK_DETECT, &clk_freq);
+        if (ret < 0)
+                goto out;
+
         wl1271_debug(DEBUG_BOOT, "clock freq %d (%d, %d, %d, %d, %s)", clk_freq,
                      wl18xx_clk_table[clk_freq].n, wl18xx_clk_table[clk_freq].m,
                      wl18xx_clk_table[clk_freq].p, wl18xx_clk_table[clk_freq].q,
                      wl18xx_clk_table[clk_freq].swallow ? "swallow" : "spit");
 
-        wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_N, wl18xx_clk_table[clk_freq].n);
-        wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_M, wl18xx_clk_table[clk_freq].m);
+        ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_N,
+                                   wl18xx_clk_table[clk_freq].n);
+        if (ret < 0)
+                goto out;
+
+        ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_M,
+                                   wl18xx_clk_table[clk_freq].m);
+        if (ret < 0)
+                goto out;
 
         if (wl18xx_clk_table[clk_freq].swallow) {
                 /* first the 16 lower bits */
-                wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_1,
-                                     wl18xx_clk_table[clk_freq].q &
-                                     PLLSH_WCS_PLL_Q_FACTOR_CFG_1_MASK);
+                ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_1,
+                                           wl18xx_clk_table[clk_freq].q &
+                                           PLLSH_WCS_PLL_Q_FACTOR_CFG_1_MASK);
+                if (ret < 0)
+                        goto out;
+
                 /* then the 16 higher bits, masked out */
-                wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_2,
-                                     (wl18xx_clk_table[clk_freq].q >> 16) &
-                                     PLLSH_WCS_PLL_Q_FACTOR_CFG_2_MASK);
+                ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_Q_FACTOR_CFG_2,
+                                        (wl18xx_clk_table[clk_freq].q >> 16) &
+                                        PLLSH_WCS_PLL_Q_FACTOR_CFG_2_MASK);
+                if (ret < 0)
+                        goto out;
 
                 /* first the 16 lower bits */
-                wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_1,
-                                     wl18xx_clk_table[clk_freq].p &
-                                     PLLSH_WCS_PLL_P_FACTOR_CFG_1_MASK);
+                ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_1,
+                                           wl18xx_clk_table[clk_freq].p &
+                                           PLLSH_WCS_PLL_P_FACTOR_CFG_1_MASK);
+                if (ret < 0)
+                        goto out;
+
                 /* then the 16 higher bits, masked out */
-                wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_2,
-                                     (wl18xx_clk_table[clk_freq].p >> 16) &
-                                     PLLSH_WCS_PLL_P_FACTOR_CFG_2_MASK);
+                ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_P_FACTOR_CFG_2,
+                                        (wl18xx_clk_table[clk_freq].p >> 16) &
+                                        PLLSH_WCS_PLL_P_FACTOR_CFG_2_MASK);
         } else {
-                wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_SWALLOW_EN,
-                                     PLLSH_WCS_PLL_SWALLOW_EN_VAL2);
+                ret = wl18xx_top_reg_write(wl, PLLSH_WCS_PLL_SWALLOW_EN,
+                                           PLLSH_WCS_PLL_SWALLOW_EN_VAL2);
         }
+
+out:
+        return ret;
 }
 
-static void wl18xx_boot_soft_reset(struct wl1271 *wl)
+static int wl18xx_boot_soft_reset(struct wl1271 *wl)
 {
+        int ret;
+
         /* disable Rx/Tx */
-        wl1271_write32(wl, WL18XX_ENABLE, 0x0);
+        ret = wlcore_write32(wl, WL18XX_ENABLE, 0x0);
+        if (ret < 0)
+                goto out;
 
         /* disable auto calibration on start*/
-        wl1271_write32(wl, WL18XX_SPARE_A2, 0xffff);
+        ret = wlcore_write32(wl, WL18XX_SPARE_A2, 0xffff);
+
+out:
+        return ret;
 }
 
 static int wl18xx_pre_boot(struct wl1271 *wl)
 {
-        wl18xx_set_clk(wl);
+        int ret;
+
+        ret = wl18xx_set_clk(wl);
+        if (ret < 0)
+                goto out;
 
         /* Continue the ELP wake up sequence */
-        wl1271_write32(wl, WL18XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+        ret = wlcore_write32(wl, WL18XX_WELP_ARM_COMMAND, WELP_ARM_COMMAND_VAL);
+        if (ret < 0)
+                goto out;
+
         udelay(500);
 
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        if (ret < 0)
+                goto out;
 
         /* Disable interrupts */
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, WL1271_ACX_INTR_ALL);
+        if (ret < 0)
+                goto out;
 
-        wl18xx_boot_soft_reset(wl);
+        ret = wl18xx_boot_soft_reset(wl);
 
-        return 0;
+out:
+        return ret;
 }
 
-static void wl18xx_pre_upload(struct wl1271 *wl)
+static int wl18xx_pre_upload(struct wl1271 *wl)
 {
         u32 tmp;
+        int ret;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        if (ret < 0)
+                goto out;
 
         /* TODO: check if this is all needed */
-        wl1271_write32(wl, WL18XX_EEPROMLESS_IND, WL18XX_EEPROMLESS_IND);
+        ret = wlcore_write32(wl, WL18XX_EEPROMLESS_IND, WL18XX_EEPROMLESS_IND);
+        if (ret < 0)
+                goto out;
 
-        tmp = wlcore_read_reg(wl, REG_CHIP_ID_B);
+        ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &tmp);
+        if (ret < 0)
+                goto out;
 
         wl1271_debug(DEBUG_BOOT, "chip id 0x%x", tmp);
 
-        tmp = wl1271_read32(wl, WL18XX_SCR_PAD2);
+        ret = wlcore_read32(wl, WL18XX_SCR_PAD2, &tmp);
+
+out:
+        return ret;
 }
 
-static void wl18xx_set_mac_and_phy(struct wl1271 *wl)
+static int wl18xx_set_mac_and_phy(struct wl1271 *wl)
 {
         struct wl18xx_priv *priv = wl->priv;
-        size_t len;
+        int ret;
 
-        /* the parameters struct is smaller for PG1 */
-        if (wl->chip.id == CHIP_ID_185x_PG10)
-                len = offsetof(struct wl18xx_mac_and_phy_params, psat) + 1;
-        else
-                len = sizeof(struct wl18xx_mac_and_phy_params);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_PHY_INIT]);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_write(wl, WL18XX_PHY_INIT_MEM_ADDR, (u8 *)&priv->conf.phy,
+                           sizeof(struct wl18xx_mac_and_phy_params), false);
 
-        wlcore_set_partition(wl, &wl->ptable[PART_PHY_INIT]);
-        wl1271_write(wl, WL18XX_PHY_INIT_MEM_ADDR, (u8 *)&priv->conf.phy, len,
-                     false);
+out:
+        return ret;
 }
 
-static void wl18xx_enable_interrupts(struct wl1271 *wl)
+static int wl18xx_enable_interrupts(struct wl1271 *wl)
 {
         u32 event_mask, intr_mask;
+        int ret;
 
-        if (wl->chip.id == CHIP_ID_185x_PG10) {
-                event_mask = WL18XX_ACX_EVENTS_VECTOR_PG1;
-                intr_mask = WL18XX_INTR_MASK_PG1;
-        } else {
-                event_mask = WL18XX_ACX_EVENTS_VECTOR_PG2;
-                intr_mask = WL18XX_INTR_MASK_PG2;
-        }
+        event_mask = WL18XX_ACX_EVENTS_VECTOR;
+        intr_mask = WL18XX_INTR_MASK;
 
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK, event_mask);
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK, event_mask);
+        if (ret < 0)
+                goto out;
 
         wlcore_enable_interrupts(wl);
-        wlcore_write_reg(wl, REG_INTERRUPT_MASK,
-                         WL1271_ACX_INTR_ALL & ~intr_mask);
+
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_MASK,
+                               WL1271_ACX_INTR_ALL & ~intr_mask);
+
+out:
+        return ret;
 }
 
 static int wl18xx_boot(struct wl1271 *wl)
@@ -761,25 +809,29 @@ static int wl18xx_boot(struct wl1271 *wl)
         if (ret < 0)
                 goto out;
 
-        wl18xx_pre_upload(wl);
+        ret = wl18xx_pre_upload(wl);
+        if (ret < 0)
+                goto out;
 
         ret = wlcore_boot_upload_firmware(wl);
         if (ret < 0)
                 goto out;
 
-        wl18xx_set_mac_and_phy(wl);
+        ret = wl18xx_set_mac_and_phy(wl);
+        if (ret < 0)
+                goto out;
 
         ret = wlcore_boot_run_firmware(wl);
         if (ret < 0)
                 goto out;
 
-        wl18xx_enable_interrupts(wl);
+        ret = wl18xx_enable_interrupts(wl);
 
 out:
         return ret;
 }
 
-static void wl18xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
+static int wl18xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
                                void *buf, size_t len)
 {
         struct wl18xx_priv *priv = wl->priv;
@@ -787,13 +839,14 @@ static void wl18xx_trigger_cmd(struct wl1271 *wl, int cmd_box_addr,
         memcpy(priv->cmd_buf, buf, len);
         memset(priv->cmd_buf + len, 0, WL18XX_CMD_MAX_SIZE - len);
 
-        wl1271_write(wl, cmd_box_addr, priv->cmd_buf, WL18XX_CMD_MAX_SIZE,
-                     false);
+        return wlcore_write(wl, cmd_box_addr, priv->cmd_buf,
+                            WL18XX_CMD_MAX_SIZE, false);
 }
 
-static void wl18xx_ack_event(struct wl1271 *wl)
+static int wl18xx_ack_event(struct wl1271 *wl)
 {
-        wlcore_write_reg(wl, REG_INTERRUPT_TRIG, WL18XX_INTR_TRIG_EVENT_ACK);
+        return wlcore_write_reg(wl, REG_INTERRUPT_TRIG,
+                                WL18XX_INTR_TRIG_EVENT_ACK);
 }
 
 static u32 wl18xx_calc_tx_blocks(struct wl1271 *wl, u32 len, u32 spare_blks)
@@ -975,34 +1028,32 @@ static u32 wl18xx_ap_get_mimo_wide_rate_mask(struct wl1271 *wl,
         } else if (!strcmp(ht_mode_param, "mimo")) {
                 wl1271_debug(DEBUG_ACX, "using MIMO rate mask");
 
-                /*
-                 * PG 1.0 has some problems with MCS_13, so disable it
-                 *
-                 * TODO: instead of hacking this in here, we should
-                 * make it more general and change a bit in the
-                 * wlvif->rate_set instead.
-                 */
-                if (wl->chip.id == CHIP_ID_185x_PG10)
-                        return CONF_TX_MIMO_RATES & ~CONF_HW_BIT_RATE_MCS_13;
-
                 return CONF_TX_MIMO_RATES;
         } else {
                 return 0;
         }
 }
 
-static s8 wl18xx_get_pg_ver(struct wl1271 *wl)
+static int wl18xx_get_pg_ver(struct wl1271 *wl, s8 *ver)
 {
         u32 fuse;
+        int ret;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_read32(wl, WL18XX_REG_FUSE_DATA_1_3, &fuse);
+        if (ret < 0)
+                goto out;
 
-        fuse = wl1271_read32(wl, WL18XX_REG_FUSE_DATA_1_3);
-        fuse = (fuse & WL18XX_PG_VER_MASK) >> WL18XX_PG_VER_OFFSET;
+        if (ver)
+                *ver = (fuse & WL18XX_PG_VER_MASK) >> WL18XX_PG_VER_OFFSET;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
 
-        return (s8)fuse;
+out:
+        return ret;
 }
 
 #define WL18XX_CONF_FILE_NAME "ti-connectivity/wl18xx-conf.bin"
@@ -1021,8 +1072,7 @@ static int wl18xx_conf_init(struct wl1271 *wl, struct device *dev)
         }
 
         if (fw->size != WL18XX_CONF_SIZE) {
-                wl1271_error("configuration binary file size is wrong, "
-                             "expected %ld got %zd",
+                wl1271_error("configuration binary file size is wrong, expected %zu got %zu",
                              WL18XX_CONF_SIZE, fw->size);
                 ret = -EINVAL;
                 goto out;
@@ -1069,26 +1119,41 @@ out:
 
 static int wl18xx_plt_init(struct wl1271 *wl)
 {
-        wl1271_write32(wl, WL18XX_SCR_PAD8, WL18XX_SCR_PAD8_PLT);
+        int ret;
+
+        ret = wlcore_write32(wl, WL18XX_SCR_PAD8, WL18XX_SCR_PAD8_PLT);
+        if (ret < 0)
+                return ret;
 
         return wl->ops->boot(wl);
 }
 
-static void wl18xx_get_mac(struct wl1271 *wl)
+static int wl18xx_get_mac(struct wl1271 *wl)
 {
         u32 mac1, mac2;
+        int ret;
+
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        if (ret < 0)
+                goto out;
 
-        wlcore_set_partition(wl, &wl->ptable[PART_TOP_PRCM_ELP_SOC]);
+        ret = wlcore_read32(wl, WL18XX_REG_FUSE_BD_ADDR_1, &mac1);
+        if (ret < 0)
+                goto out;
 
-        mac1 = wl1271_read32(wl, WL18XX_REG_FUSE_BD_ADDR_1);
-        mac2 = wl1271_read32(wl, WL18XX_REG_FUSE_BD_ADDR_2);
+        ret = wlcore_read32(wl, WL18XX_REG_FUSE_BD_ADDR_2, &mac2);
+        if (ret < 0)
+                goto out;
 
         /* these are the two parts of the BD_ADDR */
         wl->fuse_oui_addr = ((mac2 & 0xffff) << 8) +
                 ((mac1 & 0xff000000) >> 24);
         wl->fuse_nic_addr = (mac1 & 0xffffff);
 
-        wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_DOWN]);
+
+out:
+        return ret;
 }
 
 static int wl18xx_handle_static_data(struct wl1271 *wl,
@@ -1214,8 +1279,8 @@ static struct wlcore_ops wl18xx_ops = {
         .pre_pkt_send   = wl18xx_pre_pkt_send,
 };
 
-/* HT cap appropriate for wide channels */
-static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap = {
+/* HT cap appropriate for wide channels in 2Ghz */
+static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap_2ghz = {
         .cap = IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
                IEEE80211_HT_CAP_SUP_WIDTH_20_40 | IEEE80211_HT_CAP_DSSSCCK40,
         .ht_supported = true,
@@ -1228,40 +1293,42 @@ static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap = {
                 },
 };
 
-/* HT cap appropriate for SISO 20 */
-static struct ieee80211_sta_ht_cap wl18xx_siso20_ht_cap = {
-        .cap = IEEE80211_HT_CAP_SGI_20,
+/* HT cap appropriate for wide channels in 5Ghz */
+static struct ieee80211_sta_ht_cap wl18xx_siso40_ht_cap_5ghz = {
+        .cap = IEEE80211_HT_CAP_SGI_20 | IEEE80211_HT_CAP_SGI_40 |
+               IEEE80211_HT_CAP_SUP_WIDTH_20_40,
         .ht_supported = true,
         .ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
         .ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
         .mcs = {
                 .rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
-                .rx_highest = cpu_to_le16(72),
+                .rx_highest = cpu_to_le16(150),
                 .tx_params = IEEE80211_HT_MCS_TX_DEFINED,
                 },
 };
 
-/* HT cap appropriate for MIMO rates in 20mhz channel */
-static struct ieee80211_sta_ht_cap wl18xx_mimo_ht_cap_2ghz = {
+/* HT cap appropriate for SISO 20 */
+static struct ieee80211_sta_ht_cap wl18xx_siso20_ht_cap = {
         .cap = IEEE80211_HT_CAP_SGI_20,
         .ht_supported = true,
         .ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
         .ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
         .mcs = {
-                .rx_mask = { 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, },
-                .rx_highest = cpu_to_le16(144),
+                .rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
+                .rx_highest = cpu_to_le16(72),
                 .tx_params = IEEE80211_HT_MCS_TX_DEFINED,
                 },
 };
 
-static struct ieee80211_sta_ht_cap wl18xx_mimo_ht_cap_5ghz = {
+/* HT cap appropriate for MIMO rates in 20mhz channel */
+static struct ieee80211_sta_ht_cap wl18xx_mimo_ht_cap_2ghz = {
         .cap = IEEE80211_HT_CAP_SGI_20,
         .ht_supported = true,
         .ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
         .ampdu_density = IEEE80211_HT_MPDU_DENSITY_16,
         .mcs = {
-                .rx_mask = { 0xff, 0, 0, 0, 0, 0, 0, 0, 0, 0, },
-                .rx_highest = cpu_to_le16(72),
+                .rx_mask = { 0xff, 0xff, 0, 0, 0, 0, 0, 0, 0, 0, },
+                .rx_highest = cpu_to_le16(144),
                 .tx_params = IEEE80211_HT_MCS_TX_DEFINED,
                 },
 };
@@ -1286,7 +1353,7 @@ static int __devinit wl18xx_probe(struct platform_device *pdev)
         wl->ptable = wl18xx_ptable;
         wl->rtable = wl18xx_rtable;
         wl->num_tx_desc = 32;
-        wl->num_rx_desc = 16;
+        wl->num_rx_desc = 32;
         wl->band_rate_to_idx = wl18xx_band_rate_to_idx;
         wl->hw_tx_rate_tbl_size = WL18XX_CONF_HW_RXTX_RATE_MAX;
         wl->hw_min_ht_rate = WL18XX_CONF_HW_RXTX_RATE_MCS0;
@@ -1294,32 +1361,8 @@ static int __devinit wl18xx_probe(struct platform_device *pdev)
         wl->stats.fw_stats_len = sizeof(struct wl18xx_acx_statistics);
         wl->static_data_priv_len = sizeof(struct wl18xx_static_data_priv);
 
-        if (!strcmp(ht_mode_param, "wide")) {
-                memcpy(&wl->ht_cap[IEEE80211_BAND_2GHZ],
-                       &wl18xx_siso40_ht_cap,
-                       sizeof(wl18xx_siso40_ht_cap));
-                memcpy(&wl->ht_cap[IEEE80211_BAND_5GHZ],
-                       &wl18xx_siso40_ht_cap,
-                       sizeof(wl18xx_siso40_ht_cap));
-        } else if (!strcmp(ht_mode_param, "mimo")) {
-                memcpy(&wl->ht_cap[IEEE80211_BAND_2GHZ],
-                       &wl18xx_mimo_ht_cap_2ghz,
-                       sizeof(wl18xx_mimo_ht_cap_2ghz));
-                memcpy(&wl->ht_cap[IEEE80211_BAND_5GHZ],
-                       &wl18xx_mimo_ht_cap_5ghz,
-                       sizeof(wl18xx_mimo_ht_cap_5ghz));
-        } else if (!strcmp(ht_mode_param, "siso20")) {
-                memcpy(&wl->ht_cap[IEEE80211_BAND_2GHZ],
-                       &wl18xx_siso20_ht_cap,
-                       sizeof(wl18xx_siso20_ht_cap));
-                memcpy(&wl->ht_cap[IEEE80211_BAND_5GHZ],
-                       &wl18xx_siso20_ht_cap,
-                       sizeof(wl18xx_siso20_ht_cap));
-        } else {
-                wl1271_error("invalid ht_mode '%s'", ht_mode_param);
-                ret = -EINVAL;
-                goto out_free;
-        }
+        if (num_rx_desc_param != -1)
+                wl->num_rx_desc = num_rx_desc_param;
 
         ret = wl18xx_conf_init(wl, &pdev->dev);
         if (ret < 0)
@@ -1366,6 +1409,37 @@ static int __devinit wl18xx_probe(struct platform_device *pdev)
         if (dc2dc_param != -1)
                 priv->conf.phy.external_pa_dc2dc = dc2dc_param;
 
+        if (!strcmp(ht_mode_param, "default")) {
+                /*
+                 * Only support mimo with multiple antennas. Fall back to
+                 * siso20.
+                 */
+                if (priv->conf.phy.number_of_assembled_ant2_4 >= 2)
+                        wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+                                          &wl18xx_mimo_ht_cap_2ghz);
+                else
+                        wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+                                          &wl18xx_siso20_ht_cap);
+
+                /* 5Ghz is always wide */
+                wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+                                  &wl18xx_siso40_ht_cap_5ghz);
+        } else if (!strcmp(ht_mode_param, "wide")) {
+                wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+                                  &wl18xx_siso40_ht_cap_2ghz);
+                wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+                                  &wl18xx_siso40_ht_cap_5ghz);
+        } else if (!strcmp(ht_mode_param, "siso20")) {
+                wlcore_set_ht_cap(wl, IEEE80211_BAND_2GHZ,
+                                  &wl18xx_siso20_ht_cap);
+                wlcore_set_ht_cap(wl, IEEE80211_BAND_5GHZ,
+                                  &wl18xx_siso20_ht_cap);
+        } else {
+                wl1271_error("invalid ht_mode '%s'", ht_mode_param);
+                ret = -EINVAL;
+                goto out_free;
+        }
+
         if (!checksum_param) {
                 wl18xx_ops.set_rx_csum = NULL;
                 wl18xx_ops.init_vif = NULL;
@@ -1410,7 +1484,7 @@ static void __exit wl18xx_exit(void)
 module_exit(wl18xx_exit);
 
 module_param_named(ht_mode, ht_mode_param, charp, S_IRUSR);
-MODULE_PARM_DESC(ht_mode, "Force HT mode: wide (default), mimo or siso20");
+MODULE_PARM_DESC(ht_mode, "Force HT mode: wide or siso20");
 
 module_param_named(board_type, board_type_param, charp, S_IRUSR);
 MODULE_PARM_DESC(board_type, "Board type: fpga, hdk (default), evb, com8 or "
@@ -1458,6 +1532,11 @@ module_param_named(pwr_limit_reference_11_abg,
 MODULE_PARM_DESC(pwr_limit_reference_11_abg, "Power limit reference: u8 "
                  "(default is 0xc8)");
 
+module_param_named(num_rx_desc,
+                   num_rx_desc_param, int, S_IRUSR);
+MODULE_PARM_DESC(num_rx_desc_param,
+                 "Number of Rx descriptors: u8 (default is 32)");
+
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Luciano Coelho <coelho@ti.com>");
 MODULE_FIRMWARE(WL18XX_FW_NAME);
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index b9ec42c8375..ce108a736bd 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -70,7 +70,7 @@ int wl1271_acx_sleep_auth(struct wl1271 *wl, u8 sleep_auth)
         struct acx_sleep_auth *auth;
         int ret;
 
-        wl1271_debug(DEBUG_ACX, "acx sleep auth");
+        wl1271_debug(DEBUG_ACX, "acx sleep auth %d", sleep_auth);
 
         auth = kzalloc(sizeof(*auth), GFP_KERNEL);
         if (!auth) {
@@ -81,7 +81,13 @@ int wl1271_acx_sleep_auth(struct wl1271 *wl, u8 sleep_auth)
         auth->sleep_auth = sleep_auth;
 
         ret = wl1271_cmd_configure(wl, ACX_SLEEP_AUTH, auth, sizeof(*auth));
+        if (ret < 0) {
+                wl1271_error("could not configure sleep_auth to %d: %d",
+                             sleep_auth, ret);
+                goto out;
+        }
 
+        wl->sleep_auth = sleep_auth;
 out:
         kfree(auth);
         return ret;
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index c0181258b72..d03215d6b3b 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -118,6 +118,11 @@ enum wl1271_psm_mode {
 
         /* Extreme low power */
         WL1271_PSM_ELP = 2,
+
+        WL1271_PSM_MAX = WL1271_PSM_ELP,
+
+        /* illegal out of band value of PSM mode */
+        WL1271_PSM_ILLEGAL = 0xff
 };
 
 struct acx_sleep_auth {
diff --git a/drivers/net/wireless/ti/wlcore/boot.c b/drivers/net/wireless/ti/wlcore/boot.c
index 0fda500c01c..8965960b841 100644
--- a/drivers/net/wireless/ti/wlcore/boot.c
+++ b/drivers/net/wireless/ti/wlcore/boot.c
@@ -33,16 +33,22 @@
 #include "rx.h"
 #include "hw_ops.h"
 
-static void wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
+static int wl1271_boot_set_ecpu_ctrl(struct wl1271 *wl, u32 flag)
 {
         u32 cpu_ctrl;
+        int ret;
 
         /* 10.5.0 run the firmware (I) */
-        cpu_ctrl = wlcore_read_reg(wl, REG_ECPU_CONTROL);
+        ret = wlcore_read_reg(wl, REG_ECPU_CONTROL, &cpu_ctrl);
+        if (ret < 0)
+                goto out;
 
         /* 10.5.1 run the firmware (II) */
         cpu_ctrl |= flag;
-        wlcore_write_reg(wl, REG_ECPU_CONTROL, cpu_ctrl);
+        ret = wlcore_write_reg(wl, REG_ECPU_CONTROL, cpu_ctrl);
+
+out:
+        return ret;
 }
 
 static int wlcore_boot_parse_fw_ver(struct wl1271 *wl,
@@ -87,7 +93,9 @@ static int wlcore_boot_static_data(struct wl1271 *wl)
                 goto out;
         }
 
-        wl1271_read(wl, wl->cmd_box_addr, static_data, len, false);
+        ret = wlcore_read(wl, wl->cmd_box_addr, static_data, len, false);
+        if (ret < 0)
+                goto out_free;
 
         ret = wlcore_boot_parse_fw_ver(wl, static_data);
         if (ret < 0)
@@ -109,6 +117,7 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
         struct wlcore_partition_set partition;
         int addr, chunk_num, partition_limit;
         u8 *p, *chunk;
+        int ret;
 
         /* whal_FwCtrl_LoadFwImageSm() */
 
@@ -130,7 +139,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
 
         memcpy(&partition, &wl->ptable[PART_DOWN], sizeof(partition));
         partition.mem.start = dest;
-        wlcore_set_partition(wl, &partition);
+        ret = wlcore_set_partition(wl, &partition);
+        if (ret < 0)
+                return ret;
 
         /* 10.1 set partition limit and chunk num */
         chunk_num = 0;
@@ -144,7 +155,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
                         partition_limit = chunk_num * CHUNK_SIZE +
                                 wl->ptable[PART_DOWN].mem.size;
                         partition.mem.start = addr;
-                        wlcore_set_partition(wl, &partition);
+                        ret = wlcore_set_partition(wl, &partition);
+                        if (ret < 0)
+                                return ret;
                 }
 
                 /* 10.3 upload the chunk */
@@ -153,7 +166,9 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
                 memcpy(chunk, p, CHUNK_SIZE);
                 wl1271_debug(DEBUG_BOOT, "uploading fw chunk 0x%p to 0x%x",
                              p, addr);
-                wl1271_write(wl, addr, chunk, CHUNK_SIZE, false);
+                ret = wlcore_write(wl, addr, chunk, CHUNK_SIZE, false);
+                if (ret < 0)
+                        goto out;
 
                 chunk_num++;
         }
@@ -164,10 +179,11 @@ static int wl1271_boot_upload_firmware_chunk(struct wl1271 *wl, void *buf,
         memcpy(chunk, p, fw_data_len % CHUNK_SIZE);
         wl1271_debug(DEBUG_BOOT, "uploading fw last chunk (%zd B) 0x%p to 0x%x",
                      fw_data_len % CHUNK_SIZE, p, addr);
-        wl1271_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
+        ret = wlcore_write(wl, addr, chunk, fw_data_len % CHUNK_SIZE, false);
 
+out:
         kfree(chunk);
-        return 0;
+        return ret;
 }
 
 int wlcore_boot_upload_firmware(struct wl1271 *wl)
@@ -210,6 +226,7 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
         int i;
         u32 dest_addr, val;
         u8 *nvs_ptr, *nvs_aligned;
+        int ret;
 
         if (wl->nvs == NULL) {
                 wl1271_error("NVS file is needed during boot");
@@ -307,7 +324,9 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
                         wl1271_debug(DEBUG_BOOT,
                                      "nvs burst write 0x%x: 0x%x",
                                      dest_addr, val);
-                        wl1271_write32(wl, dest_addr, val);
+                        ret = wlcore_write32(wl, dest_addr, val);
+                        if (ret < 0)
+                                return ret;
 
                         nvs_ptr += 4;
                         dest_addr += 4;
@@ -333,7 +352,9 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
         nvs_len -= nvs_ptr - (u8 *)wl->nvs;
 
         /* Now we must set the partition correctly */
-        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        if (ret < 0)
+                return ret;
 
         /* Copy the NVS tables to a new block to ensure alignment */
         nvs_aligned = kmemdup(nvs_ptr, nvs_len, GFP_KERNEL);
@@ -341,11 +362,11 @@ int wlcore_boot_upload_nvs(struct wl1271 *wl)
                 return -ENOMEM;
 
         /* And finally we upload the NVS tables */
-        wlcore_write_data(wl, REG_CMD_MBOX_ADDRESS,
-                          nvs_aligned, nvs_len, false);
+        ret = wlcore_write_data(wl, REG_CMD_MBOX_ADDRESS, nvs_aligned, nvs_len,
+                                false);
 
         kfree(nvs_aligned);
-        return 0;
+        return ret;
 
 out_badnvs:
         wl1271_error("nvs data is malformed");
@@ -359,11 +380,17 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
         u32 chip_id, intr;
 
         /* Make sure we have the boot partition */
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        if (ret < 0)
+                return ret;
 
-        wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
+        ret = wl1271_boot_set_ecpu_ctrl(wl, ECPU_CONTROL_HALT);
+        if (ret < 0)
+                return ret;
 
-        chip_id = wlcore_read_reg(wl, REG_CHIP_ID_B);
+        ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &chip_id);
+        if (ret < 0)
+                return ret;
 
         wl1271_debug(DEBUG_BOOT, "chip id after firmware boot: 0x%x", chip_id);
 
@@ -376,7 +403,9 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
         loop = 0;
         while (loop++ < INIT_LOOP) {
                 udelay(INIT_LOOP_DELAY);
-                intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+                ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+                if (ret < 0)
+                        return ret;
 
                 if (intr == 0xffffffff) {
                         wl1271_error("error reading hardware complete "
@@ -385,8 +414,10 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
                 }
                 /* check that ACX_INTR_INIT_COMPLETE is enabled */
                 else if (intr & WL1271_ACX_INTR_INIT_COMPLETE) {
-                        wlcore_write_reg(wl, REG_INTERRUPT_ACK,
-                                         WL1271_ACX_INTR_INIT_COMPLETE);
+                        ret = wlcore_write_reg(wl, REG_INTERRUPT_ACK,
+                                               WL1271_ACX_INTR_INIT_COMPLETE);
+                        if (ret < 0)
+                                return ret;
                         break;
                 }
         }
@@ -398,12 +429,17 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
         }
 
         /* get hardware config command mail box */
-        wl->cmd_box_addr = wlcore_read_reg(wl, REG_COMMAND_MAILBOX_PTR);
+        ret = wlcore_read_reg(wl, REG_COMMAND_MAILBOX_PTR, &wl->cmd_box_addr);
+        if (ret < 0)
+                return ret;
 
         wl1271_debug(DEBUG_MAILBOX, "cmd_box_addr 0x%x", wl->cmd_box_addr);
 
         /* get hardware config event mail box */
-        wl->mbox_ptr[0] = wlcore_read_reg(wl, REG_EVENT_MAILBOX_PTR);
+        ret = wlcore_read_reg(wl, REG_EVENT_MAILBOX_PTR, &wl->mbox_ptr[0]);
+        if (ret < 0)
+                return ret;
+
         wl->mbox_ptr[1] = wl->mbox_ptr[0] + sizeof(struct event_mailbox);
 
         wl1271_debug(DEBUG_MAILBOX, "MBOX ptrs: 0x%x 0x%x",
@@ -445,9 +481,9 @@ int wlcore_boot_run_firmware(struct wl1271 *wl)
         }
 
         /* set the working partition to its "running" mode offset */
-        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
 
         /* firmware startup completed */
-        return 0;
+        return ret;
 }
 EXPORT_SYMBOL_GPL(wlcore_boot_run_firmware);
diff --git a/drivers/net/wireless/ti/wlcore/cmd.c b/drivers/net/wireless/ti/wlcore/cmd.c
index 885364ca434..56c7a2342fd 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.c
+++ b/drivers/net/wireless/ti/wlcore/cmd.c
@@ -65,17 +65,24 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
         WARN_ON(len % 4 != 0);
         WARN_ON(test_bit(WL1271_FLAG_IN_ELP, &wl->flags));
 
-        wl1271_write(wl, wl->cmd_box_addr, buf, len, false);
+        ret = wlcore_write(wl, wl->cmd_box_addr, buf, len, false);
+        if (ret < 0)
+                goto fail;
 
         /*
          * TODO: we just need this because one bit is in a different
          * place.  Is there any better way?
          */
-        wl->ops->trigger_cmd(wl, wl->cmd_box_addr, buf, len);
+        ret = wl->ops->trigger_cmd(wl, wl->cmd_box_addr, buf, len);
+        if (ret < 0)
+                goto fail;
 
         timeout = jiffies + msecs_to_jiffies(WL1271_COMMAND_TIMEOUT);
 
-        intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+        ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+        if (ret < 0)
+                goto fail;
+
         while (!(intr & WL1271_ACX_INTR_CMD_COMPLETE)) {
                 if (time_after(jiffies, timeout)) {
                         wl1271_error("command complete timeout");
@@ -89,13 +96,18 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
                 else
                         msleep(1);
 
-                intr = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR);
+                ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &intr);
+                if (ret < 0)
+                        goto fail;
         }
 
         /* read back the status code of the command */
         if (res_len == 0)
                 res_len = sizeof(struct wl1271_cmd_header);
-        wl1271_read(wl, wl->cmd_box_addr, cmd, res_len, false);
+
+        ret = wlcore_read(wl, wl->cmd_box_addr, cmd, res_len, false);
+        if (ret < 0)
+                goto fail;
 
         status = le16_to_cpu(cmd->status);
         if (status != CMD_STATUS_SUCCESS) {
@@ -104,11 +116,14 @@ int wl1271_cmd_send(struct wl1271 *wl, u16 id, void *buf, size_t len,
                 goto fail;
         }
 
-        wlcore_write_reg(wl, REG_INTERRUPT_ACK, WL1271_ACX_INTR_CMD_COMPLETE);
+        ret = wlcore_write_reg(wl, REG_INTERRUPT_ACK,
+                               WL1271_ACX_INTR_CMD_COMPLETE);
+        if (ret < 0)
+                goto fail;
+
         return 0;
 
 fail:
-        WARN_ON(1);
         wl12xx_queue_recovery_work(wl);
         return ret;
 }
@@ -117,35 +132,45 @@ fail:
  * Poll the mailbox event field until any of the bits in the mask is set or a
  * timeout occurs (WL1271_EVENT_TIMEOUT in msecs)
  */
-static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl, u32 mask)
+static int wl1271_cmd_wait_for_event_or_timeout(struct wl1271 *wl,
+                                                u32 mask, bool *timeout)
 {
         u32 *events_vector;
         u32 event;
-        unsigned long timeout;
+        unsigned long timeout_time;
         int ret = 0;
 
+        *timeout = false;
+
         events_vector = kmalloc(sizeof(*events_vector), GFP_KERNEL | GFP_DMA);
         if (!events_vector)
                 return -ENOMEM;
 
-        timeout = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
+        timeout_time = jiffies + msecs_to_jiffies(WL1271_EVENT_TIMEOUT);
 
         do {
-                if (time_after(jiffies, timeout)) {
+                if (time_after(jiffies, timeout_time)) {
                         wl1271_debug(DEBUG_CMD, "timeout waiting for event %d",
                                      (int)mask);
-                        ret = -ETIMEDOUT;
+                        *timeout = true;
                         goto out;
                 }
 
                 msleep(1);
 
                 /* read from both event fields */
-                wl1271_read(wl, wl->mbox_ptr[0], events_vector,
-                            sizeof(*events_vector), false);
+                ret = wlcore_read(wl, wl->mbox_ptr[0], events_vector,
+                                  sizeof(*events_vector), false);
+                if (ret < 0)
+                        goto out;
+
                 event = *events_vector & mask;
-                wl1271_read(wl, wl->mbox_ptr[1], events_vector,
-                            sizeof(*events_vector), false);
+
+                ret = wlcore_read(wl, wl->mbox_ptr[1], events_vector,
+                                  sizeof(*events_vector), false);
+                if (ret < 0)
+                        goto out;
+
                 event |= *events_vector & mask;
         } while (!event);
 
@@ -157,9 +182,10 @@ out:
 static int wl1271_cmd_wait_for_event(struct wl1271 *wl, u32 mask)
 {
         int ret;
+        bool timeout = false;
 
-        ret = wl1271_cmd_wait_for_event_or_timeout(wl, mask);
-        if (ret != 0) {
+        ret = wl1271_cmd_wait_for_event_or_timeout(wl, mask, &timeout);
+        if (ret != 0 || timeout) {
                 wl12xx_queue_recovery_work(wl);
                 return ret;
         }
@@ -1412,6 +1438,7 @@ int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
 {
         struct wl12xx_cmd_remove_peer *cmd;
         int ret;
+        bool timeout = false;
 
         wl1271_debug(DEBUG_CMD, "cmd remove peer %d", (int)hlid);
 
@@ -1432,12 +1459,16 @@ int wl12xx_cmd_remove_peer(struct wl1271 *wl, u8 hlid)
                 goto out_free;
         }
 
+        ret = wl1271_cmd_wait_for_event_or_timeout(wl,
+                                           PEER_REMOVE_COMPLETE_EVENT_ID,
+                                           &timeout);
         /*
          * We are ok with a timeout here. The event is sometimes not sent
-         * due to a firmware bug.
+         * due to a firmware bug. In case of another error (like SDIO timeout)
+         * queue a recovery.
          */
-        wl1271_cmd_wait_for_event_or_timeout(wl,
-                                             PEER_REMOVE_COMPLETE_EVENT_ID);
+        if (ret)
+                wl12xx_queue_recovery_work(wl);
 
 out_free:
         kfree(cmd);
@@ -1754,7 +1785,9 @@ int wl12xx_stop_dev(struct wl1271 *wl, struct wl12xx_vif *wlvif)
                 return -EINVAL;
 
         /* flush all pending packets */
-        wl1271_tx_work_locked(wl);
+        ret = wlcore_tx_work_locked(wl);
+        if (ret < 0)
+                goto out;
 
         if (test_bit(wlvif->dev_role_id, wl->roc_map)) {
                 ret = wl12xx_croc(wl, wlvif->dev_role_id);
diff --git a/drivers/net/wireless/ti/wlcore/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index 85171f2bf68..c8a6510c72c 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -652,4 +652,25 @@ struct wl12xx_cmd_stop_channel_switch {
         struct wl1271_cmd_header header;
 } __packed;
 
+/* Used to check radio status after calibration */
+#define MAX_TLV_LENGTH          500
+#define TEST_CMD_P2G_CAL        2       /* TX BiP */
+
+struct wl1271_cmd_cal_p2g {
+        struct wl1271_cmd_header header;
+
+        struct wl1271_cmd_test_header test;
+
+        __le32 ver;
+        __le16 len;
+        u8 buf[MAX_TLV_LENGTH];
+        u8 type;
+        u8 padding;
+
+        __le16 radio_status;
+
+        u8 sub_band_mask;
+        u8 padding2;
+} __packed;
+
 #endif /* __WL1271_CMD_H__ */
diff --git a/drivers/net/wireless/ti/wlcore/conf.h b/drivers/net/wireless/ti/wlcore/conf.h
index 03c63587233..d77224f2ac6 100644
--- a/drivers/net/wireless/ti/wlcore/conf.h
+++ b/drivers/net/wireless/ti/wlcore/conf.h
@@ -951,6 +951,12 @@ struct conf_conn_settings {
          * Range: u16
          */
         u8 max_listen_interval;
+
+        /*
+         * Default sleep authorization for a new STA interface. This determines
+         * whether we can go to ELP.
+         */
+        u8 sta_sleep_auth;
 } __packed;
 
 enum {
@@ -1276,7 +1282,7 @@ struct conf_hangover_settings {
  * version, the two LSB are the lower driver's private conf
  * version.
  */
-#define WLCORE_CONF_VERSION     (0x0001 << 16)
+#define WLCORE_CONF_VERSION     (0x0002 << 16)
 #define WLCORE_CONF_MASK        0xffff0000
 #define WLCORE_CONF_SIZE        (sizeof(struct wlcore_conf_header) +    \
                                  sizeof(struct wlcore_conf))
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index 689a847005c..80dbc5304fa 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -38,6 +38,8 @@
 /* ms */
 #define WL1271_DEBUGFS_STATS_LIFETIME 1000
 
+#define WLCORE_MAX_BLOCK_SIZE ((size_t)(4*PAGE_SIZE))
+
 /* debugfs macros idea from mac80211 */
 int wl1271_format_buffer(char __user *userbuf, size_t count,
                          loff_t *ppos, char *fmt, ...)
@@ -963,6 +965,257 @@ static const struct file_operations fw_stats_raw_ops = {
         .llseek = default_llseek,
 };
 
+static ssize_t sleep_auth_read(struct file *file, char __user *user_buf,
+                               size_t count, loff_t *ppos)
+{
+        struct wl1271 *wl = file->private_data;
+
+        return wl1271_format_buffer(user_buf, count,
+                                    ppos, "%d\n",
+                                    wl->sleep_auth);
+}
+
+static ssize_t sleep_auth_write(struct file *file,
+                                const char __user *user_buf,
+                                size_t count, loff_t *ppos)
+{
+        struct wl1271 *wl = file->private_data;
+        unsigned long value;
+        int ret;
+
+        ret = kstrtoul_from_user(user_buf, count, 0, &value);
+        if (ret < 0) {
+                wl1271_warning("illegal value in sleep_auth");
+                return -EINVAL;
+        }
+
+        if (value < 0 || value > WL1271_PSM_MAX) {
+                wl1271_warning("sleep_auth must be between 0 and %d",
+                               WL1271_PSM_MAX);
+                return -ERANGE;
+        }
+
+        mutex_lock(&wl->mutex);
+
+        wl->conf.conn.sta_sleep_auth = value;
+
+        if (wl->state == WL1271_STATE_OFF) {
+                /* this will show up on "read" in case we are off */
+                wl->sleep_auth = value;
+                goto out;
+        }
+
+        ret = wl1271_ps_elp_wakeup(wl);
+        if (ret < 0)
+                goto out;
+
+        ret = wl1271_acx_sleep_auth(wl, value);
+        if (ret < 0)
+                goto out_sleep;
+
+out_sleep:
+        wl1271_ps_elp_sleep(wl);
+out:
+        mutex_unlock(&wl->mutex);
+        return count;
+}
+
+static const struct file_operations sleep_auth_ops = {
+        .read = sleep_auth_read,
+        .write = sleep_auth_write,
+        .open = simple_open,
+        .llseek = default_llseek,
+};
+
+static ssize_t dev_mem_read(struct file *file,
+             char __user *user_buf, size_t count,
+             loff_t *ppos)
+{
+        struct wl1271 *wl = file->private_data;
+        struct wlcore_partition_set part, old_part;
+        size_t bytes = count;
+        int ret;
+        char *buf;
+
+        /* only requests of dword-aligned size and offset are supported */
+        if (bytes % 4)
+                return -EINVAL;
+
+        if (*ppos % 4)
+                return -EINVAL;
+
+        /* function should return in reasonable time */
+        bytes = min(bytes, WLCORE_MAX_BLOCK_SIZE);
+
+        if (bytes == 0)
+                return -EINVAL;
+
+        memset(&part, 0, sizeof(part));
+        part.mem.start = file->f_pos;
+        part.mem.size = bytes;
+
+        buf = kmalloc(bytes, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
+        mutex_lock(&wl->mutex);
+
+        if (wl->state == WL1271_STATE_OFF) {
+                ret = -EFAULT;
+                goto skip_read;
+        }
+
+        ret = wl1271_ps_elp_wakeup(wl);
+        if (ret < 0)
+                goto skip_read;
+
+        /* store current partition and switch partition */
+        memcpy(&old_part, &wl->curr_part, sizeof(old_part));
+        ret = wlcore_set_partition(wl, &part);
+        if (ret < 0)
+                goto part_err;
+
+        ret = wlcore_raw_read(wl, 0, buf, bytes, false);
+        if (ret < 0)
+                goto read_err;
+
+read_err:
+        /* recover partition */
+        ret = wlcore_set_partition(wl, &old_part);
+        if (ret < 0)
+                goto part_err;
+
+part_err:
+        wl1271_ps_elp_sleep(wl);
+
+skip_read:
+        mutex_unlock(&wl->mutex);
+
+        if (ret == 0) {
+                ret = copy_to_user(user_buf, buf, bytes);
+                if (ret < bytes) {
+                        bytes -= ret;
+                        *ppos += bytes;
+                        ret = 0;
+                } else {
+                        ret = -EFAULT;
+                }
+        }
+
+        kfree(buf);
+
+        return ((ret == 0) ? bytes : ret);
+}
+
+static ssize_t dev_mem_write(struct file *file, const char __user *user_buf,
+                size_t count, loff_t *ppos)
+{
+        struct wl1271 *wl = file->private_data;
+        struct wlcore_partition_set part, old_part;
+        size_t bytes = count;
+        int ret;
+        char *buf;
+
+        /* only requests of dword-aligned size and offset are supported */
+        if (bytes % 4)
+                return -EINVAL;
+
+        if (*ppos % 4)
+                return -EINVAL;
+
+        /* function should return in reasonable time */
+        bytes = min(bytes, WLCORE_MAX_BLOCK_SIZE);
+
+        if (bytes == 0)
+                return -EINVAL;
+
+        memset(&part, 0, sizeof(part));
+        part.mem.start = file->f_pos;
+        part.mem.size = bytes;
+
+        buf = kmalloc(bytes, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
+
+        ret = copy_from_user(buf, user_buf, bytes);
+        if (ret) {
+                ret = -EFAULT;
+                goto err_out;
+        }
+
+        mutex_lock(&wl->mutex);
+
+        if (wl->state == WL1271_STATE_OFF) {
+                ret = -EFAULT;
+                goto skip_write;
+        }
+
+        ret = wl1271_ps_elp_wakeup(wl);
+        if (ret < 0)
+                goto skip_write;
+
+        /* store current partition and switch partition */
+        memcpy(&old_part, &wl->curr_part, sizeof(old_part));
+        ret = wlcore_set_partition(wl, &part);
+        if (ret < 0)
+                goto part_err;
+
+        ret = wlcore_raw_write(wl, 0, buf, bytes, false);
+        if (ret < 0)
+                goto write_err;
+
+write_err:
+        /* recover partition */
+        ret = wlcore_set_partition(wl, &old_part);
+        if (ret < 0)
+                goto part_err;
+
+part_err:
+        wl1271_ps_elp_sleep(wl);
+
+skip_write:
+        mutex_unlock(&wl->mutex);
+
+        if (ret == 0)
+                *ppos += bytes;
+
+err_out:
+        kfree(buf);
+
+        return ((ret == 0) ? bytes : ret);
+}
+
+static loff_t dev_mem_seek(struct file *file, loff_t offset, int orig)
+{
+        loff_t ret;
+
+        /* only requests of dword-aligned size and offset are supported */
+        if (offset % 4)
+                return -EINVAL;
+
+        switch (orig) {
+        case SEEK_SET:
+                file->f_pos = offset;
+                ret = file->f_pos;
+                break;
+        case SEEK_CUR:
+                file->f_pos += offset;
+                ret = file->f_pos;
+                break;
+        default:
+                ret = -EINVAL;
+        }
+
+        return ret;
+}
+
+static const struct file_operations dev_mem_ops = {
+        .open = simple_open,
+        .read = dev_mem_read,
+        .write = dev_mem_write,
+        .llseek = dev_mem_seek,
+};
+
 static int wl1271_debugfs_add_files(struct wl1271 *wl,
                                     struct dentry *rootdir)
 {
@@ -988,6 +1241,7 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl,
         DEBUGFS_ADD(irq_blk_threshold, rootdir);
         DEBUGFS_ADD(irq_timeout, rootdir);
         DEBUGFS_ADD(fw_stats_raw, rootdir);
+        DEBUGFS_ADD(sleep_auth, rootdir);
 
         streaming = debugfs_create_dir("rx_streaming", rootdir);
         if (!streaming || IS_ERR(streaming))
@@ -996,6 +1250,7 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl,
         DEBUGFS_ADD_PREFIX(rx_streaming, interval, streaming);
         DEBUGFS_ADD_PREFIX(rx_streaming, always, streaming);
 
+        DEBUGFS_ADD_PREFIX(dev, mem, rootdir);
 
         return 0;
 
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index c976f040986..48907054d49 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -105,6 +105,7 @@ static int wl1271_event_process(struct wl1271 *wl)
         u32 vector;
         bool disconnect_sta = false;
         unsigned long sta_bitmap = 0;
+        int ret;
 
         wl1271_event_mbox_dump(mbox);
 
@@ -228,7 +229,9 @@ static int wl1271_event_process(struct wl1271 *wl)
 
         if ((vector & DUMMY_PACKET_EVENT_ID)) {
                 wl1271_debug(DEBUG_EVENT, "DUMMY_PACKET_ID_EVENT_ID");
-                wl1271_tx_dummy_packet(wl);
+                ret = wl1271_tx_dummy_packet(wl);
+                if (ret < 0)
+                        return ret;
         }
 
         /*
@@ -301,8 +304,10 @@ int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
                 return -EINVAL;
 
         /* first we read the mbox descriptor */
-        wl1271_read(wl, wl->mbox_ptr[mbox_num], wl->mbox,
-                    sizeof(*wl->mbox), false);
+        ret = wlcore_read(wl, wl->mbox_ptr[mbox_num], wl->mbox,
+                          sizeof(*wl->mbox), false);
+        if (ret < 0)
+                return ret;
 
         /* process the descriptor */
         ret = wl1271_event_process(wl);
@@ -313,7 +318,7 @@ int wl1271_event_handle(struct wl1271 *wl, u8 mbox_num)
          * TODO: we just need this because one bit is in a different
          * place.  Is there any better way?
          */
-        wl->ops->ack_event(wl);
+        ret = wl->ops->ack_event(wl);
 
-        return 0;
+        return ret;
 }
diff --git a/drivers/net/wireless/ti/wlcore/hw_ops.h b/drivers/net/wireless/ti/wlcore/hw_ops.h
index 9e7787ba961..2673d783ec1 100644
--- a/drivers/net/wireless/ti/wlcore/hw_ops.h
+++ b/drivers/net/wireless/ti/wlcore/hw_ops.h
@@ -65,11 +65,13 @@ wlcore_hw_get_rx_buf_align(struct wl1271 *wl, u32 rx_desc)
         return wl->ops->get_rx_buf_align(wl, rx_desc);
 }
 
-static inline void
+static inline int
 wlcore_hw_prepare_read(struct wl1271 *wl, u32 rx_desc, u32 len)
 {
         if (wl->ops->prepare_read)
-                wl->ops->prepare_read(wl, rx_desc, len);
+                return wl->ops->prepare_read(wl, rx_desc, len);
+
+        return 0;
 }
 
 static inline u32
@@ -81,10 +83,12 @@ wlcore_hw_get_rx_packet_len(struct wl1271 *wl, void *rx_data, u32 data_len)
         return wl->ops->get_rx_packet_len(wl, rx_data, data_len);
 }
 
-static inline void wlcore_hw_tx_delayed_compl(struct wl1271 *wl)
+static inline int wlcore_hw_tx_delayed_compl(struct wl1271 *wl)
 {
         if (wl->ops->tx_delayed_compl)
-                wl->ops->tx_delayed_compl(wl);
+                return wl->ops->tx_delayed_compl(wl);
+
+        return 0;
 }
 
 static inline void wlcore_hw_tx_immediate_compl(struct wl1271 *wl)
diff --git a/drivers/net/wireless/ti/wlcore/ini.h b/drivers/net/wireless/ti/wlcore/ini.h
index 4cf9ecc5621..d24fe3bbc67 100644
--- a/drivers/net/wireless/ti/wlcore/ini.h
+++ b/drivers/net/wireless/ti/wlcore/ini.h
@@ -172,7 +172,19 @@ struct wl128x_ini_fem_params_5 {
 
 /* NVS data structure */
 #define WL1271_INI_NVS_SECTION_SIZE                  468
-#define WL1271_INI_FEM_MODULE_COUNT                  2
+
+/* We have four FEM module types: 0-RFMD, 1-TQS, 2-SKW, 3-TQS_HP */
+#define WL1271_INI_FEM_MODULE_COUNT                  4
+
+/*
+ * In NVS we only store two FEM module entries -
+ *        FEM modules 0,2,3 are stored in entry 0
+ *        FEM module 1 is stored in entry 1
+ */
+#define WL12XX_NVS_FEM_MODULE_COUNT                  2
+
+#define WL12XX_FEM_TO_NVS_ENTRY(ini_fem_module)      \
+        ((ini_fem_module) == 1 ? 1 : 0)
 
 #define WL1271_INI_LEGACY_NVS_FILE_SIZE              800
 
@@ -188,13 +200,13 @@ struct wl1271_nvs_file {
         struct {
                 struct wl1271_ini_fem_params_2 params;
                 u8 padding;
-        } dyn_radio_params_2[WL1271_INI_FEM_MODULE_COUNT];
+        } dyn_radio_params_2[WL12XX_NVS_FEM_MODULE_COUNT];
         struct wl1271_ini_band_params_5 stat_radio_params_5;
         u8 padding3;
         struct {
                 struct wl1271_ini_fem_params_5 params;
                 u8 padding;
-        } dyn_radio_params_5[WL1271_INI_FEM_MODULE_COUNT];
+        } dyn_radio_params_5[WL12XX_NVS_FEM_MODULE_COUNT];
 } __packed;
 
 struct wl128x_nvs_file {
@@ -209,12 +221,12 @@ struct wl128x_nvs_file {
         struct {
                 struct wl128x_ini_fem_params_2 params;
                 u8 padding;
-        } dyn_radio_params_2[WL1271_INI_FEM_MODULE_COUNT];
+        } dyn_radio_params_2[WL12XX_NVS_FEM_MODULE_COUNT];
         struct wl128x_ini_band_params_5 stat_radio_params_5;
         u8 padding3;
         struct {
                 struct wl128x_ini_fem_params_5 params;
                 u8 padding;
-        } dyn_radio_params_5[WL1271_INI_FEM_MODULE_COUNT];
+        } dyn_radio_params_5[WL12XX_NVS_FEM_MODULE_COUNT];
 } __packed;
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/init.c b/drivers/net/wireless/ti/wlcore/init.c
index 645abd4b660..8a8a8971bef 100644
--- a/drivers/net/wireless/ti/wlcore/init.c
+++ b/drivers/net/wireless/ti/wlcore/init.c
@@ -554,29 +554,28 @@ int wl1271_init_vif_specific(struct wl1271 *wl, struct ieee80211_vif *vif)
         bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
         int ret, i;
 
-        /*
-         * consider all existing roles before configuring psm.
-         * TODO: reconfigure on interface removal.
-         */
-        if (!wl->ap_count) {
-                if (is_ap) {
-                        /* Configure for power always on */
+        /* consider all existing roles before configuring psm. */
+
+        if (wl->ap_count == 0 && is_ap) { /* first AP */
+                /* Configure for power always on */
+                ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+                if (ret < 0)
+                        return ret;
+        /* first STA, no APs */
+        } else if (wl->sta_count == 0 && wl->ap_count == 0 && !is_ap) {
+                u8 sta_auth = wl->conf.conn.sta_sleep_auth;
+                /* Configure for power according to debugfs */
+                if (sta_auth != WL1271_PSM_ILLEGAL)
+                        ret = wl1271_acx_sleep_auth(wl, sta_auth);
+                /* Configure for power always on */
+                else if (wl->quirks & WLCORE_QUIRK_NO_ELP)
                         ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
-                        if (ret < 0)
-                                return ret;
-                } else if (!wl->sta_count) {
-                        if (wl->quirks & WLCORE_QUIRK_NO_ELP) {
-                                /* Configure for power always on */
-                                ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
-                                if (ret < 0)
-                                        return ret;
-                        } else {
-                                /* Configure for ELP power saving */
-                                ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
-                                if (ret < 0)
-                                        return ret;
-                        }
-                }
+                /* Configure for ELP power saving */
+                else
+                        ret = wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+
+                if (ret < 0)
+                        return ret;
         }
 
         /* Mode specific init */
diff --git a/drivers/net/wireless/ti/wlcore/io.c b/drivers/net/wireless/ti/wlcore/io.c
index 7cd0081aede..9976219c4e4 100644
--- a/drivers/net/wireless/ti/wlcore/io.c
+++ b/drivers/net/wireless/ti/wlcore/io.c
@@ -48,6 +48,12 @@ void wlcore_disable_interrupts(struct wl1271 *wl)
 }
 EXPORT_SYMBOL_GPL(wlcore_disable_interrupts);
 
+void wlcore_disable_interrupts_nosync(struct wl1271 *wl)
+{
+        disable_irq_nosync(wl->irq);
+}
+EXPORT_SYMBOL_GPL(wlcore_disable_interrupts_nosync);
+
 void wlcore_enable_interrupts(struct wl1271 *wl)
 {
         enable_irq(wl->irq);
@@ -122,9 +128,11 @@ EXPORT_SYMBOL_GPL(wlcore_translate_addr);
  *                                    |    |
  *
  */
-void wlcore_set_partition(struct wl1271 *wl,
-                          const struct wlcore_partition_set *p)
+int wlcore_set_partition(struct wl1271 *wl,
+                         const struct wlcore_partition_set *p)
 {
+        int ret;
+
         /* copy partition info */
         memcpy(&wl->curr_part, p, sizeof(*p));
 
@@ -137,28 +145,41 @@ void wlcore_set_partition(struct wl1271 *wl,
         wl1271_debug(DEBUG_IO, "mem3_start %08X mem3_size %08X",
                      p->mem3.start, p->mem3.size);
 
-        wl1271_raw_write32(wl, HW_PART0_START_ADDR, p->mem.start);
-        wl1271_raw_write32(wl, HW_PART0_SIZE_ADDR, p->mem.size);
-        wl1271_raw_write32(wl, HW_PART1_START_ADDR, p->reg.start);
-        wl1271_raw_write32(wl, HW_PART1_SIZE_ADDR, p->reg.size);
-        wl1271_raw_write32(wl, HW_PART2_START_ADDR, p->mem2.start);
-        wl1271_raw_write32(wl, HW_PART2_SIZE_ADDR, p->mem2.size);
+        ret = wlcore_raw_write32(wl, HW_PART0_START_ADDR, p->mem.start);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_raw_write32(wl, HW_PART0_SIZE_ADDR, p->mem.size);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_raw_write32(wl, HW_PART1_START_ADDR, p->reg.start);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_raw_write32(wl, HW_PART1_SIZE_ADDR, p->reg.size);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_raw_write32(wl, HW_PART2_START_ADDR, p->mem2.start);
+        if (ret < 0)
+                goto out;
+
+        ret = wlcore_raw_write32(wl, HW_PART2_SIZE_ADDR, p->mem2.size);
+        if (ret < 0)
+                goto out;
+
         /*
          * We don't need the size of the last partition, as it is
          * automatically calculated based on the total memory size and
          * the sizes of the previous partitions.
          */
-        wl1271_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
-}
-EXPORT_SYMBOL_GPL(wlcore_set_partition);
+        ret = wlcore_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
 
-void wlcore_select_partition(struct wl1271 *wl, u8 part)
-{
-        wl1271_debug(DEBUG_IO, "setting partition %d", part);
-
-        wlcore_set_partition(wl, &wl->ptable[part]);
+out:
+        return ret;
 }
-EXPORT_SYMBOL_GPL(wlcore_select_partition);
+EXPORT_SYMBOL_GPL(wlcore_set_partition);
 
 void wl1271_io_reset(struct wl1271 *wl)
 {
diff --git a/drivers/net/wireless/ti/wlcore/io.h b/drivers/net/wireless/ti/wlcore/io.h
index 8942954b56a..fef80adc8bf 100644
--- a/drivers/net/wireless/ti/wlcore/io.h
+++ b/drivers/net/wireless/ti/wlcore/io.h
@@ -45,6 +45,7 @@
 struct wl1271;
 
 void wlcore_disable_interrupts(struct wl1271 *wl);
+void wlcore_disable_interrupts_nosync(struct wl1271 *wl);
 void wlcore_enable_interrupts(struct wl1271 *wl);
 
 void wl1271_io_reset(struct wl1271 *wl);
@@ -52,79 +53,113 @@ void wl1271_io_init(struct wl1271 *wl);
 int wlcore_translate_addr(struct wl1271 *wl, int addr);
 
 /* Raw target IO, address is not translated */
-static inline void wl1271_raw_write(struct wl1271 *wl, int addr, void *buf,
-                                    size_t len, bool fixed)
+static inline int __must_check wlcore_raw_write(struct wl1271 *wl, int addr,
+                                                void *buf, size_t len,
+                                                bool fixed)
 {
-        wl->if_ops->write(wl->dev, addr, buf, len, fixed);
+        int ret;
+
+        if (test_bit(WL1271_FLAG_SDIO_FAILED, &wl->flags))
+                return -EIO;
+
+        ret = wl->if_ops->write(wl->dev, addr, buf, len, fixed);
+        if (ret)
+                set_bit(WL1271_FLAG_SDIO_FAILED, &wl->flags);
+
+        return ret;
 }
 
-static inline void wl1271_raw_read(struct wl1271 *wl, int addr, void *buf,
-                                   size_t len, bool fixed)
+static inline int __must_check wlcore_raw_read(struct wl1271 *wl, int addr,
+                                               void *buf, size_t len,
+                                               bool fixed)
 {
-        wl->if_ops->read(wl->dev, addr, buf, len, fixed);
+        int ret;
+
+        if (test_bit(WL1271_FLAG_SDIO_FAILED, &wl->flags))
+                return -EIO;
+
+        ret = wl->if_ops->read(wl->dev, addr, buf, len, fixed);
+        if (ret)
+                set_bit(WL1271_FLAG_SDIO_FAILED, &wl->flags);
+
+        return ret;
 }
 
-static inline void wlcore_raw_read_data(struct wl1271 *wl, int reg, void *buf,
-                                        size_t len, bool fixed)
+static inline int __must_check wlcore_raw_read_data(struct wl1271 *wl, int reg,
+                                                    void *buf, size_t len,
+                                                    bool fixed)
 {
-        wl1271_raw_read(wl, wl->rtable[reg], buf, len, fixed);
+        return wlcore_raw_read(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wlcore_raw_write_data(struct wl1271 *wl, int reg, void *buf,
-                                         size_t len, bool fixed)
+static inline int __must_check wlcore_raw_write_data(struct wl1271 *wl, int reg,
+                                                     void *buf, size_t len,
+                                                     bool fixed)
 {
-        wl1271_raw_write(wl, wl->rtable[reg], buf, len, fixed);
+        return wlcore_raw_write(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline u32 wl1271_raw_read32(struct wl1271 *wl, int addr)
+static inline int __must_check wlcore_raw_read32(struct wl1271 *wl, int addr,
+                                                 u32 *val)
 {
-        wl1271_raw_read(wl, addr, &wl->buffer_32,
-                            sizeof(wl->buffer_32), false);
+        int ret;
+
+        ret = wlcore_raw_read(wl, addr, &wl->buffer_32,
+                              sizeof(wl->buffer_32), false);
+        if (ret < 0)
+                return ret;
+
+        if (val)
+                *val = le32_to_cpu(wl->buffer_32);
 
-        return le32_to_cpu(wl->buffer_32);
+        return 0;
 }
 
-static inline void wl1271_raw_write32(struct wl1271 *wl, int addr, u32 val)
+static inline int __must_check wlcore_raw_write32(struct wl1271 *wl, int addr,
+                                                  u32 val)
 {
         wl->buffer_32 = cpu_to_le32(val);
-        wl1271_raw_write(wl, addr, &wl->buffer_32,
-                             sizeof(wl->buffer_32), false);
+        return wlcore_raw_write(wl, addr, &wl->buffer_32,
+                                sizeof(wl->buffer_32), false);
 }
 
-static inline void wl1271_read(struct wl1271 *wl, int addr, void *buf,
-                               size_t len, bool fixed)
+static inline int __must_check wlcore_read(struct wl1271 *wl, int addr,
+                                           void *buf, size_t len, bool fixed)
 {
         int physical;
 
         physical = wlcore_translate_addr(wl, addr);
 
-        wl1271_raw_read(wl, physical, buf, len, fixed);
+        return wlcore_raw_read(wl, physical, buf, len, fixed);
 }
 
-static inline void wl1271_write(struct wl1271 *wl, int addr, void *buf,
-                                size_t len, bool fixed)
+static inline int __must_check wlcore_write(struct wl1271 *wl, int addr,
+                                            void *buf, size_t len, bool fixed)
 {
         int physical;
 
         physical = wlcore_translate_addr(wl, addr);
 
-        wl1271_raw_write(wl, physical, buf, len, fixed);
+        return wlcore_raw_write(wl, physical, buf, len, fixed);
 }
 
-static inline void wlcore_write_data(struct wl1271 *wl, int reg, void *buf,
-                                     size_t len, bool fixed)
+static inline int __must_check wlcore_write_data(struct wl1271 *wl, int reg,
+                                                 void *buf, size_t len,
+                                                 bool fixed)
 {
-        wl1271_write(wl, wl->rtable[reg], buf, len, fixed);
+        return wlcore_write(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wlcore_read_data(struct wl1271 *wl, int reg, void *buf,
-                                    size_t len, bool fixed)
+static inline int __must_check wlcore_read_data(struct wl1271 *wl, int reg,
+                                                void *buf, size_t len,
+                                                bool fixed)
 {
-        wl1271_read(wl, wl->rtable[reg], buf, len, fixed);
+        return wlcore_read(wl, wl->rtable[reg], buf, len, fixed);
 }
 
-static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
-                                      void *buf, size_t len, bool fixed)
+static inline int __must_check wlcore_read_hwaddr(struct wl1271 *wl, int hwaddr,
+                                                  void *buf, size_t len,
+                                                  bool fixed)
 {
         int physical;
         int addr;
@@ -134,34 +169,47 @@ static inline void wl1271_read_hwaddr(struct wl1271 *wl, int hwaddr,
 
         physical = wlcore_translate_addr(wl, addr);
 
-        wl1271_raw_read(wl, physical, buf, len, fixed);
+        return wlcore_raw_read(wl, physical, buf, len, fixed);
 }
 
-static inline u32 wl1271_read32(struct wl1271 *wl, int addr)
+static inline int __must_check wlcore_read32(struct wl1271 *wl, int addr,
+                                             u32 *val)
 {
-        return wl1271_raw_read32(wl, wlcore_translate_addr(wl, addr));
+        return wlcore_raw_read32(wl, wlcore_translate_addr(wl, addr), val);
 }
 
-static inline void wl1271_write32(struct wl1271 *wl, int addr, u32 val)
+static inline int __must_check wlcore_write32(struct wl1271 *wl, int addr,
+                                              u32 val)
 {
-        wl1271_raw_write32(wl, wlcore_translate_addr(wl, addr), val);
+        return wlcore_raw_write32(wl, wlcore_translate_addr(wl, addr), val);
 }
 
-static inline u32 wlcore_read_reg(struct wl1271 *wl, int reg)
+static inline int __must_check wlcore_read_reg(struct wl1271 *wl, int reg,
+                                               u32 *val)
 {
-        return wl1271_raw_read32(wl,
-                                 wlcore_translate_addr(wl, wl->rtable[reg]));
+        return wlcore_raw_read32(wl,
+                                 wlcore_translate_addr(wl, wl->rtable[reg]),
+                                 val);
 }
 
-static inline void wlcore_write_reg(struct wl1271 *wl, int reg, u32 val)
+static inline int __must_check wlcore_write_reg(struct wl1271 *wl, int reg,
+                                                u32 val)
 {
-        wl1271_raw_write32(wl, wlcore_translate_addr(wl, wl->rtable[reg]), val);
+        return wlcore_raw_write32(wl,
+                                  wlcore_translate_addr(wl, wl->rtable[reg]),
+                                  val);
 }
 
 static inline void wl1271_power_off(struct wl1271 *wl)
 {
-        wl->if_ops->power(wl->dev, false);
-        clear_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
+        int ret;
+
+        if (!test_bit(WL1271_FLAG_GPIO_POWER, &wl->flags))
+                return;
+
+        ret = wl->if_ops->power(wl->dev, false);
+        if (!ret)
+                clear_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
 }
 
 static inline int wl1271_power_on(struct wl1271 *wl)
@@ -173,8 +221,8 @@ static inline int wl1271_power_on(struct wl1271 *wl)
         return ret;
 }
 
-void wlcore_set_partition(struct wl1271 *wl,
-                          const struct wlcore_partition_set *p);
+int wlcore_set_partition(struct wl1271 *wl,
+                         const struct wlcore_partition_set *p);
 
 bool wl1271_set_block_size(struct wl1271 *wl);
 
@@ -182,6 +230,4 @@ bool wl1271_set_block_size(struct wl1271 *wl);
 
 int wl1271_tx_dummy_packet(struct wl1271 *wl);
 
-void wlcore_select_partition(struct wl1271 *wl, u8 part);
-
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index 747a997bc60..0cc9a024022 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -378,9 +378,9 @@ static void wl12xx_irq_update_links_status(struct wl1271 *wl,
         }
 }
 
-static void wl12xx_fw_status(struct wl1271 *wl,
-                             struct wl_fw_status_1 *status_1,
-                             struct wl_fw_status_2 *status_2)
+static int wlcore_fw_status(struct wl1271 *wl,
+                            struct wl_fw_status_1 *status_1,
+                            struct wl_fw_status_2 *status_2)
 {
         struct wl12xx_vif *wlvif;
         struct timespec ts;
@@ -388,12 +388,15 @@ static void wl12xx_fw_status(struct wl1271 *wl,
         int avail, freed_blocks;
         int i;
         size_t status_len;
+        int ret;
 
         status_len = WLCORE_FW_STATUS_1_LEN(wl->num_rx_desc) +
                 sizeof(*status_2) + wl->fw_status_priv_len;
 
-        wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status_1,
-                             status_len, false);
+        ret = wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR, status_1,
+                                   status_len, false);
+        if (ret < 0)
+                return ret;
 
         wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
                      "drv_rx_counter = %d, tx_results_counter = %d)",
@@ -462,6 +465,8 @@ static void wl12xx_fw_status(struct wl1271 *wl,
         getnstimeofday(&ts);
         wl->time_offset = (timespec_to_ns(&ts) >> 10) -
                 (s64)le32_to_cpu(status_2->fw_localtime);
+
+        return 0;
 }
 
 static void wl1271_flush_deferred_work(struct wl1271 *wl)
@@ -489,20 +494,15 @@ static void wl1271_netstack_work(struct work_struct *work)
 
 #define WL1271_IRQ_MAX_LOOPS 256
 
-static irqreturn_t wl1271_irq(int irq, void *cookie)
+static int wlcore_irq_locked(struct wl1271 *wl)
 {
-        int ret;
+        int ret = 0;
         u32 intr;
         int loopcount = WL1271_IRQ_MAX_LOOPS;
-        struct wl1271 *wl = (struct wl1271 *)cookie;
         bool done = false;
         unsigned int defer_count;
         unsigned long flags;
 
-        /* TX might be handled here, avoid redundant work */
-        set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
-        cancel_work_sync(&wl->tx_work);
-
         /*
          * In case edge triggered interrupt must be used, we cannot iterate
          * more than once without introducing race conditions with the hardirq.
@@ -510,8 +510,6 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
         if (wl->platform_quirks & WL12XX_PLATFORM_QUIRK_EDGE_IRQ)
                 loopcount = 1;
 
-        mutex_lock(&wl->mutex);
-
         wl1271_debug(DEBUG_IRQ, "IRQ work");
 
         if (unlikely(wl->state == WL1271_STATE_OFF))
@@ -530,7 +528,9 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
                 clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);
                 smp_mb__after_clear_bit();
 
-                wl12xx_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+                ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+                if (ret < 0)
+                        goto out;
 
                 wlcore_hw_tx_immediate_compl(wl);
 
@@ -544,7 +544,7 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
                 if (unlikely(intr & WL1271_ACX_INTR_WATCHDOG)) {
                         wl1271_error("HW watchdog interrupt received! starting recovery.");
                         wl->watchdog_recovery = true;
-                        wl12xx_queue_recovery_work(wl);
+                        ret = -EIO;
 
                         /* restarting the chip. ignore any other interrupt. */
                         goto out;
@@ -554,7 +554,7 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
                         wl1271_error("SW watchdog interrupt received! "
                                      "starting recovery.");
                         wl->watchdog_recovery = true;
-                        wl12xx_queue_recovery_work(wl);
+                        ret = -EIO;
 
                         /* restarting the chip. ignore any other interrupt. */
                         goto out;
@@ -563,7 +563,9 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
                 if (likely(intr & WL1271_ACX_INTR_DATA)) {
                         wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_DATA");
 
-                        wl12xx_rx(wl, wl->fw_status_1);
+                        ret = wlcore_rx(wl, wl->fw_status_1);
+                        if (ret < 0)
+                                goto out;
 
                         /* Check if any tx blocks were freed */
                         spin_lock_irqsave(&wl->wl_lock, flags);
@@ -574,13 +576,17 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
                                  * In order to avoid starvation of the TX path,
                                  * call the work function directly.
                                  */
-                                wl1271_tx_work_locked(wl);
+                                ret = wlcore_tx_work_locked(wl);
+                                if (ret < 0)
+                                        goto out;
                         } else {
                                 spin_unlock_irqrestore(&wl->wl_lock, flags);
                         }
 
                         /* check for tx results */
-                        wlcore_hw_tx_delayed_compl(wl);
+                        ret = wlcore_hw_tx_delayed_compl(wl);
+                        if (ret < 0)
+                                goto out;
 
                         /* Make sure the deferred queues don't get too long */
                         defer_count = skb_queue_len(&wl->deferred_tx_queue) +
@@ -591,12 +597,16 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
 
                 if (intr & WL1271_ACX_INTR_EVENT_A) {
                         wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_A");
-                        wl1271_event_handle(wl, 0);
+                        ret = wl1271_event_handle(wl, 0);
+                        if (ret < 0)
+                                goto out;
                 }
 
                 if (intr & WL1271_ACX_INTR_EVENT_B) {
                         wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_B");
-                        wl1271_event_handle(wl, 1);
+                        ret = wl1271_event_handle(wl, 1);
+                        if (ret < 0)
+                                goto out;
                 }
 
                 if (intr & WL1271_ACX_INTR_INIT_COMPLETE)
@@ -610,6 +620,25 @@ static irqreturn_t wl1271_irq(int irq, void *cookie)
         wl1271_ps_elp_sleep(wl);
 
 out:
+        return ret;
+}
+
+static irqreturn_t wlcore_irq(int irq, void *cookie)
+{
+        int ret;
+        unsigned long flags;
+        struct wl1271 *wl = cookie;
+
+        /* TX might be handled here, avoid redundant work */
+        set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
+        cancel_work_sync(&wl->tx_work);
+
+        mutex_lock(&wl->mutex);
+
+        ret = wlcore_irq_locked(wl);
+        if (ret)
+                wl12xx_queue_recovery_work(wl);
+
         spin_lock_irqsave(&wl->wl_lock, flags);
         /* In case TX was not handled here, queue TX work */
         clear_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
@@ -743,8 +772,13 @@ out:
 
 void wl12xx_queue_recovery_work(struct wl1271 *wl)
 {
-        if (!test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+        WARN_ON(!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
+
+        /* Avoid a recursive recovery */
+        if (!test_and_set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
+                wlcore_disable_interrupts_nosync(wl);
                 ieee80211_queue_work(wl->hw, &wl->recovery_work);
+        }
 }
 
 size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
@@ -770,14 +804,17 @@ size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
         return len;
 }
 
+#define WLCORE_FW_LOG_END 0x2000000
+
 static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 {
         u32 addr;
-        u32 first_addr;
+        u32 offset;
+        u32 end_of_log;
         u8 *block;
+        int ret;
 
         if ((wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED) ||
-            (wl->conf.fwlog.mode != WL12XX_FWLOG_ON_DEMAND) ||
             (wl->conf.fwlog.mem_blocks == 0))
                 return;
 
@@ -791,34 +828,47 @@ static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
          * Make sure the chip is awake and the logger isn't active.
          * Do not send a stop fwlog command if the fw is hanged.
          */
-        if (!wl1271_ps_elp_wakeup(wl) && !wl->watchdog_recovery)
-                wl12xx_cmd_stop_fwlog(wl);
-        else
+        if (wl1271_ps_elp_wakeup(wl))
                 goto out;
+        if (!wl->watchdog_recovery)
+                wl12xx_cmd_stop_fwlog(wl);
 
         /* Read the first memory block address */
-        wl12xx_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
-        first_addr = le32_to_cpu(wl->fw_status_2->log_start_addr);
-        if (!first_addr)
+        ret = wlcore_fw_status(wl, wl->fw_status_1, wl->fw_status_2);
+        if (ret < 0)
+                goto out;
+
+        addr = le32_to_cpu(wl->fw_status_2->log_start_addr);
+        if (!addr)
                 goto out;
 
+        if (wl->conf.fwlog.mode == WL12XX_FWLOG_CONTINUOUS) {
+                offset = sizeof(addr) + sizeof(struct wl1271_rx_descriptor);
+                end_of_log = WLCORE_FW_LOG_END;
+        } else {
+                offset = sizeof(addr);
+                end_of_log = addr;
+        }
+
         /* Traverse the memory blocks linked list */
-        addr = first_addr;
         do {
                 memset(block, 0, WL12XX_HW_BLOCK_SIZE);
-                wl1271_read_hwaddr(wl, addr, block, WL12XX_HW_BLOCK_SIZE,
-                                   false);
+                ret = wlcore_read_hwaddr(wl, addr, block, WL12XX_HW_BLOCK_SIZE,
+                                         false);
+                if (ret < 0)
+                        goto out;
 
                 /*
                  * Memory blocks are linked to one another. The first 4 bytes
                  * of each memory block hold the hardware address of the next
-                 * one. The last memory block points to the first one.
+                 * one. The last memory block points to the first one in
+                 * on demand mode and is equal to 0x2000000 in continuous mode.
                  */
                 addr = le32_to_cpup((__le32 *)block);
-                if (!wl12xx_copy_fwlog(wl, block + sizeof(addr),
-                                       WL12XX_HW_BLOCK_SIZE - sizeof(addr)))
+                if (!wl12xx_copy_fwlog(wl, block + offset,
+                                       WL12XX_HW_BLOCK_SIZE - offset))
                         break;
-        } while (addr && (addr != first_addr));
+        } while (addr && (addr != end_of_log));
 
         wake_up_interruptible(&wl->fwlog_waitq);
 
@@ -826,6 +876,34 @@ out:
         kfree(block);
 }
 
+static void wlcore_print_recovery(struct wl1271 *wl)
+{
+        u32 pc = 0;
+        u32 hint_sts = 0;
+        int ret;
+
+        wl1271_info("Hardware recovery in progress. FW ver: %s",
+                    wl->chip.fw_ver_str);
+
+        /* change partitions momentarily so we can read the FW pc */
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        if (ret < 0)
+                return;
+
+        ret = wlcore_read_reg(wl, REG_PC_ON_RECOVERY, &pc);
+        if (ret < 0)
+                return;
+
+        ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &hint_sts);
+        if (ret < 0)
+                return;
+
+        wl1271_info("pc: 0x%x, hint_sts: 0x%08x", pc, hint_sts);
+
+        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+}
+
+
 static void wl1271_recovery_work(struct work_struct *work)
 {
         struct wl1271 *wl =
@@ -838,19 +916,9 @@ static void wl1271_recovery_work(struct work_struct *work)
         if (wl->state != WL1271_STATE_ON || wl->plt)
                 goto out_unlock;
 
-        /* Avoid a recursive recovery */
-        set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
-
         wl12xx_read_fwlog_panic(wl);
 
-        /* change partitions momentarily so we can read the FW pc */
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
-        wl1271_info("Hardware recovery in progress. FW ver: %s pc: 0x%x "
-                    "hint_sts: 0x%08x",
-                    wl->chip.fw_ver_str,
-                    wlcore_read_reg(wl, REG_PC_ON_RECOVERY),
-                    wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR));
-        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
+        wlcore_print_recovery(wl);
 
         BUG_ON(bug_on_recovery &&
                !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));
@@ -892,8 +960,6 @@ static void wl1271_recovery_work(struct work_struct *work)
         mutex_unlock(&wl->mutex);
         wl1271_op_stop(wl->hw);
 
-        clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
-
         ieee80211_restart_hw(wl->hw);
 
         /*
@@ -907,9 +973,9 @@ out_unlock:
         mutex_unlock(&wl->mutex);
 }
 
-static void wl1271_fw_wakeup(struct wl1271 *wl)
+static int wlcore_fw_wakeup(struct wl1271 *wl)
 {
-        wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+        return wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
 }
 
 static int wl1271_setup(struct wl1271 *wl)
@@ -945,13 +1011,21 @@ static int wl12xx_set_power_on(struct wl1271 *wl)
         wl1271_io_reset(wl);
         wl1271_io_init(wl);
 
-        wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
+        if (ret < 0)
+                goto fail;
 
         /* ELP module wake up */
-        wl1271_fw_wakeup(wl);
+        ret = wlcore_fw_wakeup(wl);
+        if (ret < 0)
+                goto fail;
 
 out:
         return ret;
+
+fail:
+        wl1271_power_off(wl);
+        return ret;
 }
 
 static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
@@ -1082,6 +1156,7 @@ int wl1271_plt_stop(struct wl1271 *wl)
         mutex_lock(&wl->mutex);
         wl1271_power_off(wl);
         wl->flags = 0;
+        wl->sleep_auth = WL1271_PSM_ILLEGAL;
         wl->state = WL1271_STATE_OFF;
         wl->plt = false;
         wl->rx_counter = 0;
@@ -1173,7 +1248,7 @@ int wl1271_tx_dummy_packet(struct wl1271 *wl)
 
         /* The FW is low on RX memory blocks, so send the dummy packet asap */
         if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags))
-                wl1271_tx_work_locked(wl);
+                return wlcore_tx_work_locked(wl);
 
         /*
          * If the FW TX is busy, TX work will be scheduled by the threaded
@@ -1440,8 +1515,15 @@ static int wl1271_configure_wowlan(struct wl1271 *wl,
         int i, ret;
 
         if (!wow || wow->any || !wow->n_patterns) {
-                wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
-                wl1271_rx_filter_clear_all(wl);
+                ret = wl1271_acx_default_rx_filter_enable(wl, 0,
+                                                          FILTER_SIGNAL);
+                if (ret)
+                        goto out;
+
+                ret = wl1271_rx_filter_clear_all(wl);
+                if (ret)
+                        goto out;
+
                 return 0;
         }
 
@@ -1457,8 +1539,13 @@ static int wl1271_configure_wowlan(struct wl1271 *wl,
                 }
         }
 
-        wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
-        wl1271_rx_filter_clear_all(wl);
+        ret = wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
+        if (ret)
+                goto out;
+
+        ret = wl1271_rx_filter_clear_all(wl);
+        if (ret)
+                goto out;
 
         /* Translate WoWLAN patterns into filters */
         for (i = 0; i < wow->n_patterns; i++) {
@@ -1500,7 +1587,10 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
         if (ret < 0)
                 goto out;
 
-        wl1271_configure_wowlan(wl, wow);
+        ret = wl1271_configure_wowlan(wl, wow);
+        if (ret < 0)
+                goto out_sleep;
+
         ret = wl1271_acx_wake_up_conditions(wl, wlvif,
                                     wl->conf.conn.suspend_wake_up_event,
                                     wl->conf.conn.suspend_listen_interval);
@@ -1508,8 +1598,8 @@ static int wl1271_configure_suspend_sta(struct wl1271 *wl,
         if (ret < 0)
                 wl1271_error("suspend: set wake up conditions failed: %d", ret);
 
+out_sleep:
         wl1271_ps_elp_sleep(wl);
-
 out:
         return ret;
 
@@ -1588,6 +1678,12 @@ static int wl1271_op_suspend(struct ieee80211_hw *hw,
         wl1271_debug(DEBUG_MAC80211, "mac80211 suspend wow=%d", !!wow);
         WARN_ON(!wow);
 
+        /* we want to perform the recovery before suspending */
+        if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
+                wl1271_warning("postponing suspend to perform recovery");
+                return -EBUSY;
+        }
+
         wl1271_tx_flush(wl);
 
         mutex_lock(&wl->mutex);
@@ -1628,7 +1724,8 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
         struct wl1271 *wl = hw->priv;
         struct wl12xx_vif *wlvif;
         unsigned long flags;
-        bool run_irq_work = false;
+        bool run_irq_work = false, pending_recovery;
+        int ret;
 
         wl1271_debug(DEBUG_MAC80211, "mac80211 resume wow=%d",
                      wl->wow_enabled);
@@ -1644,17 +1741,37 @@ static int wl1271_op_resume(struct ieee80211_hw *hw)
                 run_irq_work = true;
         spin_unlock_irqrestore(&wl->wl_lock, flags);
 
+        mutex_lock(&wl->mutex);
+
+        /* test the recovery flag before calling any SDIO functions */
+        pending_recovery = test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
+                                    &wl->flags);
+
         if (run_irq_work) {
                 wl1271_debug(DEBUG_MAC80211,
                              "run postponed irq_work directly");
-                wl1271_irq(0, wl);
+
+                /* don't talk to the HW if recovery is pending */
+                if (!pending_recovery) {
+                        ret = wlcore_irq_locked(wl);
+                        if (ret)
+                                wl12xx_queue_recovery_work(wl);
+                }
+
                 wlcore_enable_interrupts(wl);
         }
 
-        mutex_lock(&wl->mutex);
+        if (pending_recovery) {
+                wl1271_warning("queuing forgotten recovery on resume");
+                ieee80211_queue_work(wl->hw, &wl->recovery_work);
+                goto out;
+        }
+
         wl12xx_for_each_wlvif(wl, wlvif) {
                 wl1271_configure_resume(wl, wlvif);
         }
+
+out:
         wl->wow_enabled = false;
         mutex_unlock(&wl->mutex);
 
@@ -1695,6 +1812,10 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
         wlcore_disable_interrupts(wl);
         mutex_lock(&wl->mutex);
         if (wl->state == WL1271_STATE_OFF) {
+                if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
+                                        &wl->flags))
+                        wlcore_enable_interrupts(wl);
+
                 mutex_unlock(&wl->mutex);
 
                 /*
@@ -1726,6 +1847,13 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
         mutex_lock(&wl->mutex);
 
         wl1271_power_off(wl);
+        /*
+         * In case a recovery was scheduled, interrupts were disabled to avoid
+         * an interrupt storm. Now that the power is down, it is safe to
+         * re-enable interrupts to balance the disable depth
+         */
+        if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
+                wlcore_enable_interrupts(wl);
 
         wl->band = IEEE80211_BAND_2GHZ;
 
@@ -1740,6 +1868,7 @@ static void wl1271_op_stop(struct ieee80211_hw *hw)
         wl->ap_fw_ps_map = 0;
         wl->ap_ps_map = 0;
         wl->sched_scanning = false;
+        wl->sleep_auth = WL1271_PSM_ILLEGAL;
         memset(wl->roles_map, 0, sizeof(wl->roles_map));
         memset(wl->links_map, 0, sizeof(wl->links_map));
         memset(wl->roc_map, 0, sizeof(wl->roc_map));
@@ -2146,6 +2275,7 @@ static void __wl1271_op_remove_interface(struct wl1271 *wl,
 {
         struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
         int i, ret;
+        bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
 
         wl1271_debug(DEBUG_MAC80211, "mac80211 remove interface");
 
@@ -2226,11 +2356,25 @@ deinit:
         wlvif->role_id = WL12XX_INVALID_ROLE_ID;
         wlvif->dev_role_id = WL12XX_INVALID_ROLE_ID;
 
-        if (wlvif->bss_type == BSS_TYPE_AP_BSS)
+        if (is_ap)
                 wl->ap_count--;
         else
                 wl->sta_count--;
 
+        /* Last AP, have more stations. Configure according to STA. */
+        if (wl->ap_count == 0 && is_ap && wl->sta_count) {
+                u8 sta_auth = wl->conf.conn.sta_sleep_auth;
+                /* Configure for power according to debugfs */
+                if (sta_auth != WL1271_PSM_ILLEGAL)
+                        wl1271_acx_sleep_auth(wl, sta_auth);
+                /* Configure for power always on */
+                else if (wl->quirks & WLCORE_QUIRK_NO_ELP)
+                        wl1271_acx_sleep_auth(wl, WL1271_PSM_CAM);
+                /* Configure for ELP power saving */
+                else
+                        wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
+        }
+
         mutex_unlock(&wl->mutex);
 
         del_timer_sync(&wlvif->rx_streaming_timer);
@@ -2448,12 +2592,16 @@ static int wl12xx_config_vif(struct wl1271 *wl, struct wl12xx_vif *wlvif,
              (wlvif->channel != channel) ||
              (wlvif->channel_type != conf->channel_type))) {
                 /* send all pending packets */
-                wl1271_tx_work_locked(wl);
+                ret = wlcore_tx_work_locked(wl);
+                if (ret < 0)
+                        return ret;
+
                 wlvif->band = conf->channel->band;
                 wlvif->channel = channel;
                 wlvif->channel_type = conf->channel_type;
 
                 if (is_ap) {
+                        wl1271_set_band_rate(wl, wlvif);
                         ret = wl1271_init_ap_rates(wl, wlvif);
                         if (ret < 0)
                                 wl1271_error("AP rate policy change failed %d",
@@ -4090,16 +4238,13 @@ out:
 static int wl1271_op_get_survey(struct ieee80211_hw *hw, int idx,
                                 struct survey_info *survey)
 {
-        struct wl1271 *wl = hw->priv;
         struct ieee80211_conf *conf = &hw->conf;
 
         if (idx != 0)
                 return -ENOENT;
 
         survey->channel = conf->channel;
-        survey->filled = SURVEY_INFO_NOISE_DBM;
-        survey->noise = wl->noise;
-
+        survey->filled = 0;
         return 0;
 }
 
@@ -4365,9 +4510,14 @@ static int wl1271_op_ampdu_action(struct ieee80211_hw *hw,
 
         case IEEE80211_AMPDU_RX_STOP:
                 if (!(*ba_bitmap & BIT(tid))) {
-                        ret = -EINVAL;
-                        wl1271_error("no active RX BA session on tid: %d",
+                        /*
+                         * this happens on reconfig - so only output a debug
+                         * message for now, and don't fail the function.
+                         */
+                        wl1271_debug(DEBUG_MAC80211,
+                                     "no active RX BA session on tid: %d",
                                      tid);
+                        ret = 0;
                         break;
                 }
 
@@ -4904,18 +5054,22 @@ static int wl12xx_get_hw_info(struct wl1271 *wl)
         if (ret < 0)
                 goto out;
 
-        wl->chip.id = wlcore_read_reg(wl, REG_CHIP_ID_B);
+        ret = wlcore_read_reg(wl, REG_CHIP_ID_B, &wl->chip.id);
+        if (ret < 0)
+                goto out;
 
         wl->fuse_oui_addr = 0;
         wl->fuse_nic_addr = 0;
 
-        wl->hw_pg_ver = wl->ops->get_pg_ver(wl);
+        ret = wl->ops->get_pg_ver(wl, &wl->hw_pg_ver);
+        if (ret < 0)
+                goto out;
 
         if (wl->ops->get_mac)
-                wl->ops->get_mac(wl);
+                ret = wl->ops->get_mac(wl);
 
-        wl1271_power_off(wl);
 out:
+        wl1271_power_off(wl);
         return ret;
 }
 
@@ -4976,6 +5130,29 @@ static void wl1271_unregister_hw(struct wl1271 *wl)
 
 }
 
+static const struct ieee80211_iface_limit wlcore_iface_limits[] = {
+        {
+                .max = 2,
+                .types = BIT(NL80211_IFTYPE_STATION),
+        },
+        {
+                .max = 1,
+                .types = BIT(NL80211_IFTYPE_AP) |
+                         BIT(NL80211_IFTYPE_P2P_GO) |
+                         BIT(NL80211_IFTYPE_P2P_CLIENT),
+        },
+};
+
+static const struct ieee80211_iface_combination
+wlcore_iface_combinations[] = {
+        {
+          .num_different_channels = 1,
+          .max_interfaces = 2,
+          .limits = wlcore_iface_limits,
+          .n_limits = ARRAY_SIZE(wlcore_iface_limits),
+        },
+};
+
 static int wl1271_init_ieee80211(struct wl1271 *wl)
 {
         static const u32 cipher_suites[] = {
@@ -5069,6 +5246,11 @@ static int wl1271_init_ieee80211(struct wl1271 *wl)
                 NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
                 NL80211_PROBE_RESP_OFFLOAD_SUPPORT_P2P;
 
+        /* allowed interface combinations */
+        wl->hw->wiphy->iface_combinations = wlcore_iface_combinations;
+        wl->hw->wiphy->n_iface_combinations =
+                ARRAY_SIZE(wlcore_iface_combinations);
+
         SET_IEEE80211_DEV(wl->hw, wl->dev);
 
         wl->hw->sta_data_size = sizeof(struct wl1271_station);
@@ -5140,6 +5322,7 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size)
         wl->channel_type = NL80211_CHAN_NO_HT;
         wl->flags = 0;
         wl->sg_enabled = true;
+        wl->sleep_auth = WL1271_PSM_ILLEGAL;
         wl->hw_pg_ver = -1;
         wl->ap_ps_map = 0;
         wl->ap_fw_ps_map = 0;
@@ -5313,7 +5496,7 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
         else
                 irqflags = IRQF_TRIGGER_HIGH | IRQF_ONESHOT;
 
-        ret = request_threaded_irq(wl->irq, wl12xx_hardirq, wl1271_irq,
+        ret = request_threaded_irq(wl->irq, wl12xx_hardirq, wlcore_irq,
                                    irqflags,
                                    pdev->name, wl);
         if (ret < 0) {
@@ -5341,12 +5524,12 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
         ret = wl12xx_get_hw_info(wl);
         if (ret < 0) {
                 wl1271_error("couldn't get hw info");
-                goto out;
+                goto out_irq;
         }
 
         ret = wl->ops->identify_chip(wl);
         if (ret < 0)
-                goto out;
+                goto out_irq;
 
         ret = wl1271_init_ieee80211(wl);
         if (ret)
@@ -5360,7 +5543,7 @@ int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev)
         ret = device_create_file(wl->dev, &dev_attr_bt_coex_state);
         if (ret < 0) {
                 wl1271_error("failed to create sysfs file bt_coex_state");
-                goto out_irq;
+                goto out_unreg;
         }
 
         /* Create sysfs file to get HW PG version */
@@ -5385,6 +5568,9 @@ out_hw_pg_ver:
 out_bt_coex_state:
         device_remove_file(wl->dev, &dev_attr_bt_coex_state);
 
+out_unreg:
+        wl1271_unregister_hw(wl);
+
 out_irq:
         free_irq(wl->irq, wl);
 
diff --git a/drivers/net/wireless/ti/wlcore/ps.c b/drivers/net/wireless/ti/wlcore/ps.c
index 47e81b32f7d..46d36fd30eb 100644
--- a/drivers/net/wireless/ti/wlcore/ps.c
+++ b/drivers/net/wireless/ti/wlcore/ps.c
@@ -35,6 +35,7 @@ void wl1271_elp_work(struct work_struct *work)
         struct delayed_work *dwork;
         struct wl1271 *wl;
         struct wl12xx_vif *wlvif;
+        int ret;
 
         dwork = container_of(work, struct delayed_work, work);
         wl = container_of(dwork, struct wl1271, elp_work);
@@ -63,7 +64,12 @@ void wl1271_elp_work(struct work_struct *work)
         }
 
         wl1271_debug(DEBUG_PSM, "chip to elp");
-        wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_SLEEP);
+        ret = wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_SLEEP);
+        if (ret < 0) {
+                wl12xx_queue_recovery_work(wl);
+                goto out;
+        }
+
         set_bit(WL1271_FLAG_IN_ELP, &wl->flags);
 
 out:
@@ -76,7 +82,7 @@ void wl1271_ps_elp_sleep(struct wl1271 *wl)
         struct wl12xx_vif *wlvif;
         u32 timeout;
 
-        if (wl->quirks & WLCORE_QUIRK_NO_ELP)
+        if (wl->sleep_auth != WL1271_PSM_ELP)
                 return;
 
         /* we shouldn't get consecutive sleep requests */
@@ -135,7 +141,11 @@ int wl1271_ps_elp_wakeup(struct wl1271 *wl)
                 wl->elp_compl = &compl;
         spin_unlock_irqrestore(&wl->wl_lock, flags);
 
-        wl1271_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+        ret = wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
+        if (ret < 0) {
+                wl12xx_queue_recovery_work(wl);
+                goto err;
+        }
 
         if (!pending) {
                 ret = wait_for_completion_timeout(
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index 78200dcacfc..f55e2f9e7ac 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -127,7 +127,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
         }
 
         if (rx_align == WLCORE_RX_BUF_UNALIGNED)
-                reserved = NET_IP_ALIGN;
+                reserved = RX_BUF_ALIGN;
 
         /* the data read starts with the descriptor */
         desc = (struct wl1271_rx_descriptor *) data;
@@ -175,7 +175,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
          */
         memcpy(buf, data + sizeof(*desc), pkt_data_len);
         if (rx_align == WLCORE_RX_BUF_PADDED)
-                skb_pull(skb, NET_IP_ALIGN);
+                skb_pull(skb, RX_BUF_ALIGN);
 
         *hlid = desc->hlid;
 
@@ -200,7 +200,7 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
         return is_data;
 }
 
-void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
 {
         unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
         u32 buf_size;
@@ -211,6 +211,7 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
         u32 pkt_offset, des;
         u8 hlid;
         enum wl_rx_buf_align rx_align;
+        int ret = 0;
 
         while (drv_rx_counter != fw_rx_counter) {
                 buf_size = 0;
@@ -234,9 +235,14 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
 
                 /* Read all available packets at once */
                 des = le32_to_cpu(status->rx_pkt_descs[drv_rx_counter]);
-                wlcore_hw_prepare_read(wl, des, buf_size);
-                wlcore_read_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-                                 buf_size, true);
+                ret = wlcore_hw_prepare_read(wl, des, buf_size);
+                if (ret < 0)
+                        goto out;
+
+                ret = wlcore_read_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+                                       buf_size, true);
+                if (ret < 0)
+                        goto out;
 
                 /* Split data into separate packets */
                 pkt_offset = 0;
@@ -273,11 +279,17 @@ void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status_1 *status)
          * Write the driver's packet counter to the FW. This is only required
          * for older hardware revisions
          */
-        if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
-                wl1271_write32(wl, WL12XX_REG_RX_DRIVER_COUNTER,
-                               wl->rx_counter);
+        if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION) {
+                ret = wlcore_write32(wl, WL12XX_REG_RX_DRIVER_COUNTER,
+                                     wl->rx_counter);
+                if (ret < 0)
+                        goto out;
+        }
 
         wl12xx_rearm_rx_streaming(wl, active_hlids);
+
+out:
+        return ret;
 }
 
 #ifdef CONFIG_PM
@@ -306,14 +318,19 @@ int wl1271_rx_filter_enable(struct wl1271 *wl,
         return 0;
 }
 
-void wl1271_rx_filter_clear_all(struct wl1271 *wl)
+int wl1271_rx_filter_clear_all(struct wl1271 *wl)
 {
-        int i;
+        int i, ret = 0;
 
         for (i = 0; i < WL1271_MAX_RX_FILTERS; i++) {
                 if (!wl->rx_filter_enabled[i])
                         continue;
-                wl1271_rx_filter_enable(wl, i, 0, NULL);
+                ret = wl1271_rx_filter_enable(wl, i, 0, NULL);
+                if (ret)
+                        goto out;
         }
+
+out:
+        return ret;
 }
 #endif /* CONFIG_PM */
diff --git a/drivers/net/wireless/ti/wlcore/rx.h b/drivers/net/wireless/ti/wlcore/rx.h
index 9be78017945..71eba189991 100644
--- a/drivers/net/wireless/ti/wlcore/rx.h
+++ b/drivers/net/wireless/ti/wlcore/rx.h
@@ -103,6 +103,12 @@
 /* If set, the buffer was padded by the FW to be 4 bytes aligned */
 #define RX_BUF_PADDED_PAYLOAD        BIT(30)
 
+/*
+ * Account for the padding inserted by the FW in case of RX_ALIGNMENT
+ * or for fixing alignment in case the packet wasn't aligned.
+ */
+#define RX_BUF_ALIGN                 2
+
 /* Describes the alignment state of a Rx buffer */
 enum wl_rx_buf_align {
         WLCORE_RX_BUF_ALIGNED,
@@ -137,11 +143,11 @@ struct wl1271_rx_descriptor {
         u8  reserved;
 } __packed;
 
-void wl12xx_rx(struct wl1271 *wl, struct wl_fw_status_1 *status);
+int wlcore_rx(struct wl1271 *wl, struct wl_fw_status_1 *status);
 u8 wl1271_rate_to_idx(int rate, enum ieee80211_band band);
 int wl1271_rx_filter_enable(struct wl1271 *wl,
                             int index, bool enable,
                             struct wl12xx_rx_filter *filter);
-void wl1271_rx_filter_clear_all(struct wl1271 *wl);
+int wl1271_rx_filter_clear_all(struct wl1271 *wl);
 
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/sdio.c b/drivers/net/wireless/ti/wlcore/sdio.c
index c67ec482e44..204e69fa932 100644
--- a/drivers/net/wireless/ti/wlcore/sdio.c
+++ b/drivers/net/wireless/ti/wlcore/sdio.c
@@ -71,8 +71,8 @@ static void wl1271_sdio_set_block_size(struct device *child,
         sdio_release_host(func);
 }
 
-static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
-                                 size_t len, bool fixed)
+static int __must_check wl12xx_sdio_raw_read(struct device *child, int addr,
+                                             void *buf, size_t len, bool fixed)
 {
         int ret;
         struct wl12xx_sdio_glue *glue = dev_get_drvdata(child->parent);
@@ -103,12 +103,14 @@ static void wl12xx_sdio_raw_read(struct device *child, int addr, void *buf,
 
         sdio_release_host(func);
 
-        if (ret)
+        if (WARN_ON(ret))
                 dev_err(child->parent, "sdio read failed (%d)\n", ret);
+
+        return ret;
 }
 
-static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
-                                  size_t len, bool fixed)
+static int __must_check wl12xx_sdio_raw_write(struct device *child, int addr,
+                                              void *buf, size_t len, bool fixed)
 {
         int ret;
         struct wl12xx_sdio_glue *glue = dev_get_drvdata(child->parent);
@@ -139,25 +141,30 @@ static void wl12xx_sdio_raw_write(struct device *child, int addr, void *buf,
 
         sdio_release_host(func);
 
-        if (ret)
+        if (WARN_ON(ret))
                 dev_err(child->parent, "sdio write failed (%d)\n", ret);
+
+        return ret;
 }
 
 static int wl12xx_sdio_power_on(struct wl12xx_sdio_glue *glue)
 {
         int ret;
         struct sdio_func *func = dev_to_sdio_func(glue->dev);
+        struct mmc_card *card = func->card;
 
-        /* If enabled, tell runtime PM not to power off the card */
-        if (pm_runtime_enabled(&func->dev)) {
-                ret = pm_runtime_get_sync(&func->dev);
-                if (ret < 0)
-                        goto out;
-        } else {
-                /* Runtime PM is disabled: power up the card manually */
-                ret = mmc_power_restore_host(func->card->host);
-                if (ret < 0)
+        ret = pm_runtime_get_sync(&card->dev);
+        if (ret) {
+                /*
+                 * Runtime PM might be temporarily disabled, or the device
+                 * might have a positive reference counter. Make sure it is
+                 * really powered on.
+                 */
+                ret = mmc_power_restore_host(card->host);
+                if (ret < 0) {
+                        pm_runtime_put_sync(&card->dev);
                         goto out;
+                }
         }
 
         sdio_claim_host(func);
@@ -172,20 +179,21 @@ static int wl12xx_sdio_power_off(struct wl12xx_sdio_glue *glue)
 {
         int ret;
         struct sdio_func *func = dev_to_sdio_func(glue->dev);
+        struct mmc_card *card = func->card;
 
         sdio_claim_host(func);
         sdio_disable_func(func);
         sdio_release_host(func);
 
-        /* Power off the card manually, even if runtime PM is enabled. */
-        ret = mmc_power_save_host(func->card->host);
+        /* Power off the card manually in case it wasn't powered off above */
+        ret = mmc_power_save_host(card->host);
         if (ret < 0)
-                return ret;
+                goto out;
 
-        /* If enabled, let runtime PM know the card is powered off */
-        if (pm_runtime_enabled(&func->dev))
-                ret = pm_runtime_put_sync(&func->dev);
+        /* Let runtime PM know the card is powered off */
+        pm_runtime_put_sync(&card->dev);
 
+out:
         return ret;
 }
 
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index 553cd3cbb98..6420abae40e 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -193,8 +193,8 @@ static int wl12xx_spi_read_busy(struct device *child)
         return -ETIMEDOUT;
 }
 
-static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
-                                size_t len, bool fixed)
+static int __must_check wl12xx_spi_raw_read(struct device *child, int addr,
+                                            void *buf, size_t len, bool fixed)
 {
         struct wl12xx_spi_glue *glue = dev_get_drvdata(child->parent);
         struct wl1271 *wl = dev_get_drvdata(child);
@@ -238,7 +238,7 @@ static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
                 if (!(busy_buf[WL1271_BUSY_WORD_CNT - 1] & 0x1) &&
                     wl12xx_spi_read_busy(child)) {
                         memset(buf, 0, chunk_len);
-                        return;
+                        return 0;
                 }
 
                 spi_message_init(&m);
@@ -256,10 +256,12 @@ static void wl12xx_spi_raw_read(struct device *child, int addr, void *buf,
                 buf += chunk_len;
                 len -= chunk_len;
         }
+
+        return 0;
 }
 
-static void wl12xx_spi_raw_write(struct device *child, int addr, void *buf,
-                                 size_t len, bool fixed)
+static int __must_check wl12xx_spi_raw_write(struct device *child, int addr,
+                                             void *buf, size_t len, bool fixed)
 {
         struct wl12xx_spi_glue *glue = dev_get_drvdata(child->parent);
         struct spi_transfer t[2 * WSPI_MAX_NUM_OF_CHUNKS];
@@ -304,6 +306,8 @@ static void wl12xx_spi_raw_write(struct device *child, int addr, void *buf,
         }
 
         spi_sync(to_spi_device(glue->dev), &m);
+
+        return 0;
 }
 
 static struct wl1271_if_operations spi_ops = {
diff --git a/drivers/net/wireless/ti/wlcore/testmode.c b/drivers/net/wireless/ti/wlcore/testmode.c
index 0e59ea2cdd3..eeb339d61d1 100644
--- a/drivers/net/wireless/ti/wlcore/testmode.c
+++ b/drivers/net/wireless/ti/wlcore/testmode.c
@@ -108,6 +108,20 @@ static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
         }
 
         if (answer) {
+                /* If we got bip calibration answer print radio status */
+                struct wl1271_cmd_cal_p2g *params =
+                        (struct wl1271_cmd_cal_p2g *) buf;
+
+                s16 radio_status = (s16) le16_to_cpu(params->radio_status);
+
+                if (params->test.id == TEST_CMD_P2G_CAL &&
+                    radio_status < 0)
+                        wl1271_warning("testmode cmd: radio status=%d",
+                                        radio_status);
+                else
+                        wl1271_info("testmode cmd: radio status=%d",
+                                        radio_status);
+
                 len = nla_total_size(buf_len);
                 skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
                 if (!skb) {
diff --git a/drivers/net/wireless/ti/wlcore/tx.c b/drivers/net/wireless/ti/wlcore/tx.c
index 6983e7a829d..6a28aeecf00 100644
--- a/drivers/net/wireless/ti/wlcore/tx.c
+++ b/drivers/net/wireless/ti/wlcore/tx.c
@@ -305,11 +305,15 @@ static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
         if (is_dummy || !wlvif)
                 rate_idx = 0;
         else if (wlvif->bss_type != BSS_TYPE_AP_BSS) {
-                /* if the packets are destined for AP (have a STA entry)
-                   send them with AP rate policies, otherwise use default
-                   basic rates */
+                /*
+                 * if the packets are destined for AP (have a STA entry)
+                 * send them with AP rate policies (EAPOLs are an exception),
+                 * otherwise use default basic rates
+                 */
                 if (control->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
                         rate_idx = wlvif->sta.p2p_rate_idx;
+                else if (skb->protocol == cpu_to_be16(ETH_P_PAE))
+                        rate_idx = wlvif->sta.basic_rate_idx;
                 else if (control->control.sta)
                         rate_idx = wlvif->sta.ap_rate_idx;
                 else
@@ -348,8 +352,10 @@ static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
         bool is_dummy;
         bool is_gem = false;
 
-        if (!skb)
+        if (!skb) {
+                wl1271_error("discarding null skb");
                 return -EINVAL;
+        }
 
         info = IEEE80211_SKB_CB(skb);
 
@@ -658,7 +664,17 @@ void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids)
         }
 }
 
-void wl1271_tx_work_locked(struct wl1271 *wl)
+/*
+ * Returns failure values only in case of failed bus ops within this function.
+ * wl1271_prepare_tx_frame retvals won't be returned in order to avoid
+ * triggering recovery by higher layers when not necessary.
+ * In case a FW command fails within wl1271_prepare_tx_frame fails a recovery
+ * will be queued in wl1271_cmd_send. -EAGAIN/-EBUSY from prepare_tx_frame
+ * can occur and are legitimate so don't propagate. -EINVAL will emit a WARNING
+ * within prepare_tx_frame code but there's nothing we should do about those
+ * as well.
+ */
+int wlcore_tx_work_locked(struct wl1271 *wl)
 {
         struct wl12xx_vif *wlvif;
         struct sk_buff *skb;
@@ -666,10 +682,11 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
         u32 buf_offset = 0, last_len = 0;
         bool sent_packets = false;
         unsigned long active_hlids[BITS_TO_LONGS(WL12XX_MAX_LINKS)] = {0};
-        int ret;
+        int ret = 0;
+        int bus_ret = 0;
 
         if (unlikely(wl->state == WL1271_STATE_OFF))
-                return;
+                return 0;
 
         while ((skb = wl1271_skb_dequeue(wl))) {
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -690,8 +707,11 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 
                         buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset,
                                                             last_len);
-                        wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-                                          buf_offset, true);
+                        bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA,
+                                             wl->aggr_buf, buf_offset, true);
+                        if (bus_ret < 0)
+                                goto out;
+
                         sent_packets = true;
                         buf_offset = 0;
                         continue;
@@ -727,8 +747,11 @@ void wl1271_tx_work_locked(struct wl1271 *wl)
 out_ack:
         if (buf_offset) {
                 buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset, last_len);
-                wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
-                                  buf_offset, true);
+                bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
+                                             buf_offset, true);
+                if (bus_ret < 0)
+                        goto out;
+
                 sent_packets = true;
         }
         if (sent_packets) {
@@ -736,13 +759,19 @@ out_ack:
                  * Interrupt the firmware with the new packets. This is only
                  * required for older hardware revisions
                  */
-                if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION)
-                        wl1271_write32(wl, WL12XX_HOST_WR_ACCESS,
-                                       wl->tx_packets_count);
+                if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION) {
+                        bus_ret = wlcore_write32(wl, WL12XX_HOST_WR_ACCESS,
+                                             wl->tx_packets_count);
+                        if (bus_ret < 0)
+                                goto out;
+                }
 
                 wl1271_handle_tx_low_watermark(wl);
         }
         wl12xx_rearm_rx_streaming(wl, active_hlids);
+
+out:
+        return bus_ret;
 }
 
 void wl1271_tx_work(struct work_struct *work)
@@ -755,7 +784,11 @@ void wl1271_tx_work(struct work_struct *work)
         if (ret < 0)
                 goto out;
 
-        wl1271_tx_work_locked(wl);
+        ret = wlcore_tx_work_locked(wl);
+        if (ret < 0) {
+                wl12xx_queue_recovery_work(wl);
+                goto out;
+        }
 
         wl1271_ps_elp_sleep(wl);
 out:
@@ -877,22 +910,28 @@ static void wl1271_tx_complete_packet(struct wl1271 *wl,
 }
 
 /* Called upon reception of a TX complete interrupt */
-void wl1271_tx_complete(struct wl1271 *wl)
+int wlcore_tx_complete(struct wl1271 *wl)
 {
         struct wl1271_acx_mem_map *memmap =
                 (struct wl1271_acx_mem_map *)wl->target_mem_map;
         u32 count, fw_counter;
         u32 i;
+        int ret;
 
         /* read the tx results from the chipset */
-        wl1271_read(wl, le32_to_cpu(memmap->tx_result),
-                    wl->tx_res_if, sizeof(*wl->tx_res_if), false);
+        ret = wlcore_read(wl, le32_to_cpu(memmap->tx_result),
+                          wl->tx_res_if, sizeof(*wl->tx_res_if), false);
+        if (ret < 0)
+                goto out;
+
         fw_counter = le32_to_cpu(wl->tx_res_if->tx_result_fw_counter);
 
         /* write host counter to chipset (to ack) */
-        wl1271_write32(wl, le32_to_cpu(memmap->tx_result) +
-                       offsetof(struct wl1271_tx_hw_res_if,
-                                tx_result_host_counter), fw_counter);
+        ret = wlcore_write32(wl, le32_to_cpu(memmap->tx_result) +
+                             offsetof(struct wl1271_tx_hw_res_if,
+                                      tx_result_host_counter), fw_counter);
+        if (ret < 0)
+                goto out;
 
         count = fw_counter - wl->tx_results_count;
         wl1271_debug(DEBUG_TX, "tx_complete received, packets: %d", count);
@@ -912,8 +951,11 @@ void wl1271_tx_complete(struct wl1271 *wl)
 
                 wl->tx_results_count++;
         }
+
+out:
+        return ret;
 }
-EXPORT_SYMBOL(wl1271_tx_complete);
+EXPORT_SYMBOL(wlcore_tx_complete);
 
 void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid)
 {
diff --git a/drivers/net/wireless/ti/wlcore/tx.h b/drivers/net/wireless/ti/wlcore/tx.h
index fa4be1b9113..1e939b01615 100644
--- a/drivers/net/wireless/ti/wlcore/tx.h
+++ b/drivers/net/wireless/ti/wlcore/tx.h
@@ -234,8 +234,8 @@ static inline int wl1271_tx_total_queue_count(struct wl1271 *wl)
 }
 
 void wl1271_tx_work(struct work_struct *work);
-void wl1271_tx_work_locked(struct wl1271 *wl);
-void wl1271_tx_complete(struct wl1271 *wl);
+int wlcore_tx_work_locked(struct wl1271 *wl);
+int wlcore_tx_complete(struct wl1271 *wl);
 void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif);
 void wl12xx_tx_reset(struct wl1271 *wl);
 void wl1271_tx_flush(struct wl1271 *wl);
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index 761a72f4b8d..e796974df59 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -41,9 +41,9 @@ struct wlcore_ops {
         int (*identify_fw)(struct wl1271 *wl);
         int (*boot)(struct wl1271 *wl);
         int (*plt_init)(struct wl1271 *wl);
-        void (*trigger_cmd)(struct wl1271 *wl, int cmd_box_addr,
-                            void *buf, size_t len);
-        void (*ack_event)(struct wl1271 *wl);
+        int (*trigger_cmd)(struct wl1271 *wl, int cmd_box_addr,
+                           void *buf, size_t len);
+        int (*ack_event)(struct wl1271 *wl);
         u32 (*calc_tx_blocks)(struct wl1271 *wl, u32 len, u32 spare_blks);
         void (*set_tx_desc_blocks)(struct wl1271 *wl,
                                    struct wl1271_tx_hw_descr *desc,
@@ -53,17 +53,17 @@ struct wlcore_ops {
                                      struct sk_buff *skb);
         enum wl_rx_buf_align (*get_rx_buf_align)(struct wl1271 *wl,
                                                  u32 rx_desc);
-        void (*prepare_read)(struct wl1271 *wl, u32 rx_desc, u32 len);
+        int (*prepare_read)(struct wl1271 *wl, u32 rx_desc, u32 len);
         u32 (*get_rx_packet_len)(struct wl1271 *wl, void *rx_data,
                                  u32 data_len);
-        void (*tx_delayed_compl)(struct wl1271 *wl);
+        int (*tx_delayed_compl)(struct wl1271 *wl);
         void (*tx_immediate_compl)(struct wl1271 *wl);
         int (*hw_init)(struct wl1271 *wl);
         int (*init_vif)(struct wl1271 *wl, struct wl12xx_vif *wlvif);
         u32 (*sta_get_ap_rate_mask)(struct wl1271 *wl,
                                     struct wl12xx_vif *wlvif);
-        s8 (*get_pg_ver)(struct wl1271 *wl);
-        void (*get_mac)(struct wl1271 *wl);
+        int (*get_pg_ver)(struct wl1271 *wl, s8 *ver);
+        int (*get_mac)(struct wl1271 *wl);
         void (*set_tx_desc_csum)(struct wl1271 *wl,
                                  struct wl1271_tx_hw_descr *desc,
                                  struct sk_buff *skb);
@@ -387,6 +387,9 @@ struct wl1271 {
 
         /* mutex for protecting the tx_flush function */
         struct mutex flush_mutex;
+
+        /* sleep auth value currently configured to FW */
+        int sleep_auth;
 };
 
 int __devinit wlcore_probe(struct wl1271 *wl, struct platform_device *pdev);
@@ -398,6 +401,13 @@ int wlcore_set_key(struct wl1271 *wl, enum set_key_cmd cmd,
                    struct ieee80211_sta *sta,
                    struct ieee80211_key_conf *key_conf);
 
+static inline void
+wlcore_set_ht_cap(struct wl1271 *wl, enum ieee80211_band band,
+                  struct ieee80211_sta_ht_cap *ht_cap)
+{
+        memcpy(&wl->ht_cap[band], ht_cap, sizeof(*ht_cap));
+}
+
 /* Firmware image load chunk size */
 #define CHUNK_SIZE      16384
 
diff --git a/drivers/net/wireless/ti/wlcore/wlcore_i.h b/drivers/net/wireless/ti/wlcore/wlcore_i.h
index 8260b1e9288..4273a21cdde 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore_i.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore_i.h
@@ -209,10 +209,10 @@ struct wl1271_scan {
 };
 
 struct wl1271_if_operations {
-        void (*read)(struct device *child, int addr, void *buf, size_t len,
-                     bool fixed);
-        void (*write)(struct device *child, int addr, void *buf, size_t len,
-                     bool fixed);
+        int __must_check (*read)(struct device *child, int addr, void *buf,
+                                 size_t len, bool fixed);
+        int __must_check (*write)(struct device *child, int addr, void *buf,
+                                  size_t len, bool fixed);
         void (*reset)(struct device *child);
         void (*init)(struct device *child);
         int (*power)(struct device *child, bool enable);
@@ -247,6 +247,7 @@ enum wl12xx_flags {
         WL1271_FLAG_RECOVERY_IN_PROGRESS,
         WL1271_FLAG_VIF_CHANGE_IN_PROGRESS,
         WL1271_FLAG_INTENDED_FW_RECOVERY,
+        WL1271_FLAG_SDIO_FAILED,
 };
 
 enum wl12xx_vif_flags {
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index 19110f0eb15..9ac829e22e7 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -45,6 +45,9 @@ static const struct usb_device_id pn533_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, pn533_table);
 
+/* How much time we spend listening for initiators */
+#define PN533_LISTEN_TIME 2
+
 /* frame definitions */
 #define PN533_FRAME_TAIL_SIZE 2
 #define PN533_FRAME_SIZE(f) (sizeof(struct pn533_frame) + f->datalen + \
@@ -74,6 +77,10 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_CMD_IN_RELEASE 0x52
 #define PN533_CMD_IN_JUMP_FOR_DEP 0x56
 
+#define PN533_CMD_TG_INIT_AS_TARGET 0x8c
+#define PN533_CMD_TG_GET_DATA 0x86
+#define PN533_CMD_TG_SET_DATA 0x8e
+
 #define PN533_CMD_RESPONSE(cmd) (cmd + 1)
 
 /* PN533 Return codes */
@@ -81,6 +88,9 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_CMD_MI_MASK 0x40
 #define PN533_CMD_RET_SUCCESS 0x00
 
+/* PN533 status codes */
+#define PN533_STATUS_TARGET_RELEASED 0x29
+
 struct pn533;
 
 typedef int (*pn533_cmd_complete_t) (struct pn533 *dev, void *arg,
@@ -97,8 +107,14 @@ struct pn533_fw_version {
 };
 
 /* PN533_CMD_RF_CONFIGURATION */
+#define PN533_CFGITEM_TIMING 0x02
 #define PN533_CFGITEM_MAX_RETRIES 0x05
 
+#define PN533_CONFIG_TIMING_102 0xb
+#define PN533_CONFIG_TIMING_204 0xc
+#define PN533_CONFIG_TIMING_409 0xd
+#define PN533_CONFIG_TIMING_819 0xe
+
 #define PN533_CONFIG_MAX_RETRIES_NO_RETRY 0x00
 #define PN533_CONFIG_MAX_RETRIES_ENDLESS 0xFF
 
@@ -108,6 +124,12 @@ struct pn533_config_max_retries {
         u8 mx_rty_passive_act;
 } __packed;
 
+struct pn533_config_timing {
+        u8 rfu;
+        u8 atr_res_timeout;
+        u8 dep_timeout;
+} __packed;
+
 /* PN533_CMD_IN_LIST_PASSIVE_TARGET */
 
 /* felica commands opcode */
@@ -144,6 +166,7 @@ enum {
         PN533_POLL_MOD_424KBPS_FELICA,
         PN533_POLL_MOD_106KBPS_JEWEL,
         PN533_POLL_MOD_847KBPS_B,
+        PN533_LISTEN_MOD,
 
         __PN533_POLL_MOD_AFTER_LAST,
 };
@@ -211,6 +234,9 @@ const struct pn533_poll_modulations poll_mod[] = {
                 },
                 .len = 3,
         },
+        [PN533_LISTEN_MOD] = {
+                .len = 0,
+        },
 };
 
 /* PN533_CMD_IN_ATR */
@@ -237,7 +263,7 @@ struct pn533_cmd_jump_dep {
         u8 active;
         u8 baud;
         u8 next;
-        u8 gt[];
+        u8 data[];
 } __packed;
 
 struct pn533_cmd_jump_dep_response {
@@ -253,6 +279,29 @@ struct pn533_cmd_jump_dep_response {
         u8 gt[];
 } __packed;
 
+
+/* PN533_TG_INIT_AS_TARGET */
+#define PN533_INIT_TARGET_PASSIVE 0x1
+#define PN533_INIT_TARGET_DEP 0x2
+
+#define PN533_INIT_TARGET_RESP_FRAME_MASK 0x3
+#define PN533_INIT_TARGET_RESP_ACTIVE     0x1
+#define PN533_INIT_TARGET_RESP_DEP        0x4
+
+struct pn533_cmd_init_target {
+        u8 mode;
+        u8 mifare[6];
+        u8 felica[18];
+        u8 nfcid3[10];
+        u8 gb_len;
+        u8 gb[];
+} __packed;
+
+struct pn533_cmd_init_target_response {
+        u8 mode;
+        u8 cmd[];
+} __packed;
+
 struct pn533 {
         struct usb_device *udev;
         struct usb_interface *interface;
@@ -270,22 +319,31 @@ struct pn533 {
 
         struct workqueue_struct *wq;
         struct work_struct cmd_work;
+        struct work_struct poll_work;
         struct work_struct mi_work;
+        struct work_struct tg_work;
+        struct timer_list listen_timer;
         struct pn533_frame *wq_in_frame;
         int wq_in_error;
+        int cancel_listen;
 
         pn533_cmd_complete_t cmd_complete;
         void *cmd_complete_arg;
-        struct semaphore cmd_lock;
+        struct mutex cmd_lock;
         u8 cmd;
 
         struct pn533_poll_modulations *poll_mod_active[PN533_POLL_MOD_MAX + 1];
         u8 poll_mod_count;
         u8 poll_mod_curr;
         u32 poll_protocols;
+        u32 listen_protocols;
+
+        u8 *gb;
+        size_t gb_len;
 
         u8 tgt_available_prots;
         u8 tgt_active_prot;
+        u8 tgt_mode;
 };
 
 struct pn533_frame {
@@ -405,7 +463,7 @@ static void pn533_wq_cmd_complete(struct work_struct *work)
                                         PN533_FRAME_CMD_PARAMS_LEN(in_frame));
 
         if (rc != -EINPROGRESS)
-                up(&dev->cmd_lock);
+                mutex_unlock(&dev->cmd_lock);
 }
 
 static void pn533_recv_response(struct urb *urb)
@@ -583,7 +641,7 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
 
         nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-        if (down_trylock(&dev->cmd_lock))
+        if (!mutex_trylock(&dev->cmd_lock))
                 return -EBUSY;
 
         rc = __pn533_send_cmd_frame_async(dev, out_frame, in_frame,
@@ -593,7 +651,7 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
 
         return 0;
 error:
-        up(&dev->cmd_lock);
+        mutex_unlock(&dev->cmd_lock);
         return rc;
 }
 
@@ -963,6 +1021,11 @@ static int pn533_target_found(struct pn533 *dev,
         return 0;
 }
 
+static inline void pn533_poll_next_mod(struct pn533 *dev)
+{
+        dev->poll_mod_curr = (dev->poll_mod_curr + 1) % dev->poll_mod_count;
+}
+
 static void pn533_poll_reset_mod_list(struct pn533 *dev)
 {
         dev->poll_mod_count = 0;
@@ -975,102 +1038,283 @@ static void pn533_poll_add_mod(struct pn533 *dev, u8 mod_index)
         dev->poll_mod_count++;
 }
 
-static void pn533_poll_create_mod_list(struct pn533 *dev, u32 protocols)
+static void pn533_poll_create_mod_list(struct pn533 *dev,
+                                       u32 im_protocols, u32 tm_protocols)
 {
         pn533_poll_reset_mod_list(dev);
 
-        if (protocols & NFC_PROTO_MIFARE_MASK
-                                        || protocols & NFC_PROTO_ISO14443_MASK
-                                        || protocols & NFC_PROTO_NFC_DEP_MASK)
+        if (im_protocols & NFC_PROTO_MIFARE_MASK
+            || im_protocols & NFC_PROTO_ISO14443_MASK
+            || im_protocols & NFC_PROTO_NFC_DEP_MASK)
                 pn533_poll_add_mod(dev, PN533_POLL_MOD_106KBPS_A);
 
-        if (protocols & NFC_PROTO_FELICA_MASK
-                                        || protocols & NFC_PROTO_NFC_DEP_MASK) {
+        if (im_protocols & NFC_PROTO_FELICA_MASK
+            || im_protocols & NFC_PROTO_NFC_DEP_MASK) {
                 pn533_poll_add_mod(dev, PN533_POLL_MOD_212KBPS_FELICA);
                 pn533_poll_add_mod(dev, PN533_POLL_MOD_424KBPS_FELICA);
         }
 
-        if (protocols & NFC_PROTO_JEWEL_MASK)
+        if (im_protocols & NFC_PROTO_JEWEL_MASK)
                 pn533_poll_add_mod(dev, PN533_POLL_MOD_106KBPS_JEWEL);
 
-        if (protocols & NFC_PROTO_ISO14443_MASK)
+        if (im_protocols & NFC_PROTO_ISO14443_MASK)
                 pn533_poll_add_mod(dev, PN533_POLL_MOD_847KBPS_B);
+
+        if (tm_protocols)
+                pn533_poll_add_mod(dev, PN533_LISTEN_MOD);
+}
+
+static int pn533_start_poll_complete(struct pn533 *dev, void *arg,
+                                     u8 *params, int params_len)
+{
+        struct pn533_poll_response *resp;
+        int rc;
+
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        resp = (struct pn533_poll_response *) params;
+        if (resp->nbtg) {
+                rc = pn533_target_found(dev, resp, params_len);
+
+                /* We must stop the poll after a valid target found */
+                if (rc == 0) {
+                        pn533_poll_reset_mod_list(dev);
+                        return 0;
+                }
+        }
+
+        return -EAGAIN;
 }
 
-static void pn533_start_poll_frame(struct pn533_frame *frame,
-                                        struct pn533_poll_modulations *mod)
+static int pn533_init_target_frame(struct pn533_frame *frame,
+                                   u8 *gb, size_t gb_len)
 {
+        struct pn533_cmd_init_target *cmd;
+        size_t cmd_len;
+        u8 felica_params[18] = {0x1, 0xfe, /* DEP */
+                                0x0, 0x0, 0x0, 0x0, 0x0, 0x0, /* random */
+                                0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+                                0xff, 0xff}; /* System code */
+        u8 mifare_params[6] = {0x1, 0x1, /* SENS_RES */
+                               0x0, 0x0, 0x0,
+                               0x40}; /* SEL_RES for DEP */
+
+        cmd_len = sizeof(struct pn533_cmd_init_target) + gb_len + 1;
+        cmd = kzalloc(cmd_len, GFP_KERNEL);
+        if (cmd == NULL)
+                return -ENOMEM;
+
+        pn533_tx_frame_init(frame, PN533_CMD_TG_INIT_AS_TARGET);
+
+        /* DEP support only */
+        cmd->mode |= PN533_INIT_TARGET_DEP;
+
+        /* Felica params */
+        memcpy(cmd->felica, felica_params, 18);
+        get_random_bytes(cmd->felica + 2, 6);
+
+        /* NFCID3 */
+        memset(cmd->nfcid3, 0, 10);
+        memcpy(cmd->nfcid3, cmd->felica, 8);
 
-        pn533_tx_frame_init(frame, PN533_CMD_IN_LIST_PASSIVE_TARGET);
+        /* MIFARE params */
+        memcpy(cmd->mifare, mifare_params, 6);
 
-        memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), &mod->data, mod->len);
-        frame->datalen += mod->len;
+        /* General bytes */
+        cmd->gb_len = gb_len;
+        memcpy(cmd->gb, gb, gb_len);
+
+        /* Len Tk */
+        cmd->gb[gb_len] = 0;
+
+        memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), cmd, cmd_len);
+
+        frame->datalen += cmd_len;
 
         pn533_tx_frame_finish(frame);
+
+        kfree(cmd);
+
+        return 0;
 }
 
-static int pn533_start_poll_complete(struct pn533 *dev, void *arg,
-                                                u8 *params, int params_len)
+#define PN533_CMD_DATAEXCH_HEAD_LEN (sizeof(struct pn533_frame) + 3)
+#define PN533_CMD_DATAEXCH_DATA_MAXLEN 262
+static int pn533_tm_get_data_complete(struct pn533 *dev, void *arg,
+                                      u8 *params, int params_len)
 {
-        struct pn533_poll_response *resp;
-        struct pn533_poll_modulations *next_mod;
-        int rc;
+        struct sk_buff *skb_resp = arg;
+        struct pn533_frame *in_frame = (struct pn533_frame *) skb_resp->data;
 
         nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
-        if (params_len == -ENOENT) {
-                nfc_dev_dbg(&dev->interface->dev, "Polling operation has been"
-                                                                " stopped");
-                goto stop_poll;
+        if (params_len < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when starting as a target",
+                            params_len);
+
+                return params_len;
         }
 
+        if (params_len > 0 && params[0] != 0) {
+                nfc_tm_deactivated(dev->nfc_dev);
+
+                dev->tgt_mode = 0;
+
+                kfree_skb(skb_resp);
+                return 0;
+        }
+
+        skb_put(skb_resp, PN533_FRAME_SIZE(in_frame));
+        skb_pull(skb_resp, PN533_CMD_DATAEXCH_HEAD_LEN);
+        skb_trim(skb_resp, skb_resp->len - PN533_FRAME_TAIL_SIZE);
+
+        return nfc_tm_data_received(dev->nfc_dev, skb_resp);
+}
+
+static void pn533_wq_tg_get_data(struct work_struct *work)
+{
+        struct pn533 *dev = container_of(work, struct pn533, tg_work);
+        struct pn533_frame *in_frame;
+        struct sk_buff *skb_resp;
+        size_t skb_resp_len;
+
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        skb_resp_len = PN533_CMD_DATAEXCH_HEAD_LEN +
+                PN533_CMD_DATAEXCH_DATA_MAXLEN +
+                PN533_FRAME_TAIL_SIZE;
+
+        skb_resp = nfc_alloc_recv_skb(skb_resp_len, GFP_KERNEL);
+        if (!skb_resp)
+                return;
+
+        in_frame = (struct pn533_frame *)skb_resp->data;
+
+        pn533_tx_frame_init(dev->out_frame, PN533_CMD_TG_GET_DATA);
+        pn533_tx_frame_finish(dev->out_frame);
+
+        pn533_send_cmd_frame_async(dev, dev->out_frame, in_frame,
+                                   skb_resp_len,
+                                   pn533_tm_get_data_complete,
+                                   skb_resp, GFP_KERNEL);
+
+        return;
+}
+
+#define ATR_REQ_GB_OFFSET 17
+static int pn533_init_target_complete(struct pn533 *dev, void *arg,
+                                      u8 *params, int params_len)
+{
+        struct pn533_cmd_init_target_response *resp;
+        u8 frame, comm_mode = NFC_COMM_PASSIVE, *gb;
+        size_t gb_len;
+        int rc;
+
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
         if (params_len < 0) {
-                nfc_dev_err(&dev->interface->dev, "Error %d when running poll",
-                                                                params_len);
-                goto stop_poll;
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when starting as a target",
+                            params_len);
+
+                return params_len;
         }
 
-        resp = (struct pn533_poll_response *) params;
-        if (resp->nbtg) {
-                rc = pn533_target_found(dev, resp, params_len);
+        if (params_len < ATR_REQ_GB_OFFSET + 1)
+                return -EINVAL;
 
-                /* We must stop the poll after a valid target found */
-                if (rc == 0)
-                        goto stop_poll;
+        resp = (struct pn533_cmd_init_target_response *) params;
+
+        nfc_dev_dbg(&dev->interface->dev, "Target mode 0x%x param len %d\n",
+                    resp->mode, params_len);
+
+        frame = resp->mode & PN533_INIT_TARGET_RESP_FRAME_MASK;
+        if (frame == PN533_INIT_TARGET_RESP_ACTIVE)
+                comm_mode = NFC_COMM_ACTIVE;
 
-                if (rc != -EAGAIN)
-                        nfc_dev_err(&dev->interface->dev, "The target found is"
-                                        " not valid - continuing to poll");
+        /* Again, only DEP */
+        if ((resp->mode & PN533_INIT_TARGET_RESP_DEP) == 0)
+                return -EOPNOTSUPP;
+
+        gb = resp->cmd + ATR_REQ_GB_OFFSET;
+        gb_len = params_len - (ATR_REQ_GB_OFFSET + 1);
+
+        rc = nfc_tm_activated(dev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
+                              comm_mode, gb, gb_len);
+        if (rc < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error when signaling target activation");
+                return rc;
         }
 
-        dev->poll_mod_curr = (dev->poll_mod_curr + 1) % dev->poll_mod_count;
+        dev->tgt_mode = 1;
 
-        next_mod = dev->poll_mod_active[dev->poll_mod_curr];
+        queue_work(dev->wq, &dev->tg_work);
 
-        nfc_dev_dbg(&dev->interface->dev, "Polling next modulation (0x%x)",
-                                                        dev->poll_mod_curr);
+        return 0;
+}
+
+static void pn533_listen_mode_timer(unsigned long data)
+{
+        struct pn533 *dev = (struct pn533 *) data;
+
+        nfc_dev_dbg(&dev->interface->dev, "Listen mode timeout");
+
+        /* An ack will cancel the last issued command (poll) */
+        pn533_send_ack(dev, GFP_ATOMIC);
+
+        dev->cancel_listen = 1;
+
+        mutex_unlock(&dev->cmd_lock);
+
+        pn533_poll_next_mod(dev);
+
+        queue_work(dev->wq, &dev->poll_work);
+}
 
-        pn533_start_poll_frame(dev->out_frame, next_mod);
+static int pn533_poll_complete(struct pn533 *dev, void *arg,
+                               u8 *params, int params_len)
+{
+        struct pn533_poll_modulations *cur_mod;
+        int rc;
 
-        /* Don't need to down the semaphore again */
-        rc = __pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
-                                dev->in_maxlen, pn533_start_poll_complete,
-                                NULL, GFP_ATOMIC);
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        if (params_len == -ENOENT) {
+                if (dev->poll_mod_count != 0)
+                        return 0;
+
+                nfc_dev_err(&dev->interface->dev,
+                            "Polling operation has been stopped");
 
-        if (rc == -EPERM) {
-                nfc_dev_dbg(&dev->interface->dev, "Cannot poll next modulation"
-                                        " because poll has been stopped");
                 goto stop_poll;
         }
 
-        if (rc) {
-                nfc_dev_err(&dev->interface->dev, "Error %d when trying to poll"
-                                                        " next modulation", rc);
+        if (params_len < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when running poll", params_len);
+
                 goto stop_poll;
         }
 
-        /* Inform caller function to do not up the semaphore */
-        return -EINPROGRESS;
+        cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+        if (cur_mod->len == 0) {
+                del_timer(&dev->listen_timer);
+
+                return pn533_init_target_complete(dev, arg, params, params_len);
+        } else {
+                rc = pn533_start_poll_complete(dev, arg, params, params_len);
+                if (!rc)
+                        return rc;
+        }
+
+        pn533_poll_next_mod(dev);
+
+        queue_work(dev->wq, &dev->poll_work);
+
+        return 0;
 
 stop_poll:
         pn533_poll_reset_mod_list(dev);
@@ -1078,61 +1322,104 @@ stop_poll:
         return 0;
 }
 
-static int pn533_start_poll(struct nfc_dev *nfc_dev, u32 protocols)
+static void pn533_build_poll_frame(struct pn533 *dev,
+                                   struct pn533_frame *frame,
+                                   struct pn533_poll_modulations *mod)
 {
-        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
-        struct pn533_poll_modulations *start_mod;
-        int rc;
+        nfc_dev_dbg(&dev->interface->dev, "mod len %d\n", mod->len);
 
-        nfc_dev_dbg(&dev->interface->dev, "%s - protocols=0x%x", __func__,
-                                                                protocols);
+        if (mod->len == 0) {
+                /* Listen mode */
+                pn533_init_target_frame(frame, dev->gb, dev->gb_len);
+        } else {
+                /* Polling mode */
+                pn533_tx_frame_init(frame, PN533_CMD_IN_LIST_PASSIVE_TARGET);
 
-        if (dev->poll_mod_count) {
-                nfc_dev_err(&dev->interface->dev, "Polling operation already"
-                                                                " active");
-                return -EBUSY;
-        }
+                memcpy(PN533_FRAME_CMD_PARAMS_PTR(frame), &mod->data, mod->len);
+                frame->datalen += mod->len;
 
-        if (dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev, "Cannot poll with a target"
-                                                        " already activated");
-                return -EBUSY;
+                pn533_tx_frame_finish(frame);
         }
+}
 
-        pn533_poll_create_mod_list(dev, protocols);
+static int pn533_send_poll_frame(struct pn533 *dev)
+{
+        struct pn533_poll_modulations *cur_mod;
+        int rc;
 
-        if (!dev->poll_mod_count) {
-                nfc_dev_err(&dev->interface->dev, "No valid protocols"
-                                                                " specified");
-                rc = -EINVAL;
-                goto error;
+        cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+        pn533_build_poll_frame(dev, dev->out_frame, cur_mod);
+
+        rc = pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
+                                dev->in_maxlen, pn533_poll_complete,
+                                NULL, GFP_KERNEL);
+        if (rc)
+                nfc_dev_err(&dev->interface->dev, "Polling loop error %d", rc);
+
+        return rc;
+}
+
+static void pn533_wq_poll(struct work_struct *work)
+{
+        struct pn533 *dev = container_of(work, struct pn533, poll_work);
+        struct pn533_poll_modulations *cur_mod;
+        int rc;
+
+        cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+        nfc_dev_dbg(&dev->interface->dev,
+                    "%s cancel_listen %d modulation len %d",
+                    __func__, dev->cancel_listen, cur_mod->len);
+
+        if (dev->cancel_listen == 1) {
+                dev->cancel_listen = 0;
+                usb_kill_urb(dev->in_urb);
         }
 
-        nfc_dev_dbg(&dev->interface->dev, "It will poll %d modulations types",
-                                                        dev->poll_mod_count);
+        rc = pn533_send_poll_frame(dev);
+        if (rc)
+                return;
 
-        dev->poll_mod_curr = 0;
-        start_mod = dev->poll_mod_active[dev->poll_mod_curr];
+        if (cur_mod->len == 0 && dev->poll_mod_count > 1)
+                mod_timer(&dev->listen_timer, jiffies + PN533_LISTEN_TIME * HZ);
 
-        pn533_start_poll_frame(dev->out_frame, start_mod);
+        return;
+}
 
-        rc = pn533_send_cmd_frame_async(dev, dev->out_frame, dev->in_frame,
-                                dev->in_maxlen, pn533_start_poll_complete,
-                                NULL, GFP_KERNEL);
+static int pn533_start_poll(struct nfc_dev *nfc_dev,
+                            u32 im_protocols, u32 tm_protocols)
+{
+        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 
-        if (rc) {
-                nfc_dev_err(&dev->interface->dev, "Error %d when trying to"
-                                                        " start poll", rc);
-                goto error;
+        nfc_dev_dbg(&dev->interface->dev,
+                    "%s: im protocols 0x%x tm protocols 0x%x",
+                    __func__, im_protocols, tm_protocols);
+
+        if (dev->tgt_active_prot) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Cannot poll with a target already activated");
+                return -EBUSY;
         }
 
-        dev->poll_protocols = protocols;
+        if (dev->tgt_mode) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Cannot poll while already being activated");
+                return -EBUSY;
+        }
 
-        return 0;
+        if (tm_protocols) {
+                dev->gb = nfc_get_local_general_bytes(nfc_dev, &dev->gb_len);
+                if (dev->gb == NULL)
+                        tm_protocols = 0;
+        }
 
-error:
-        pn533_poll_reset_mod_list(dev);
-        return rc;
+        dev->poll_mod_curr = 0;
+        pn533_poll_create_mod_list(dev, im_protocols, tm_protocols);
+        dev->poll_protocols = im_protocols;
+        dev->listen_protocols = tm_protocols;
+
+        return pn533_send_poll_frame(dev);
 }
 
 static void pn533_stop_poll(struct nfc_dev *nfc_dev)
@@ -1141,6 +1428,8 @@ static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 
         nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
+        del_timer(&dev->listen_timer);
+
         if (!dev->poll_mod_count) {
                 nfc_dev_dbg(&dev->interface->dev, "Polling operation was not"
                                                                 " running");
@@ -1152,6 +1441,8 @@ static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 
         /* prevent pn533_start_poll_complete to issue a new poll meanwhile */
         usb_kill_urb(dev->in_urb);
+
+        pn533_poll_reset_mod_list(dev);
 }
 
 static int pn533_activate_target_nfcdep(struct pn533 *dev)
@@ -1349,13 +1640,29 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
         return 0;
 }
 
+static int pn533_mod_to_baud(struct pn533 *dev)
+{
+        switch (dev->poll_mod_curr) {
+        case PN533_POLL_MOD_106KBPS_A:
+                return 0;
+        case PN533_POLL_MOD_212KBPS_FELICA:
+                return 1;
+        case PN533_POLL_MOD_424KBPS_FELICA:
+                return 2;
+        default:
+                return -EINVAL;
+        }
+}
+
+#define PASSIVE_DATA_LEN 5
 static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
                              u8 comm_mode, u8* gb, size_t gb_len)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
         struct pn533_cmd_jump_dep *cmd;
-        u8 cmd_len;
-        int rc;
+        u8 cmd_len, *data_ptr;
+        u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3};
+        int rc, baud;
 
         nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
@@ -1371,7 +1678,17 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
                 return -EBUSY;
         }
 
+        baud = pn533_mod_to_baud(dev);
+        if (baud < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Invalid curr modulation %d", dev->poll_mod_curr);
+                return baud;
+        }
+
         cmd_len = sizeof(struct pn533_cmd_jump_dep) + gb_len;
+        if (comm_mode == NFC_COMM_PASSIVE)
+                cmd_len += PASSIVE_DATA_LEN;
+
         cmd = kzalloc(cmd_len, GFP_KERNEL);
         if (cmd == NULL)
                 return -ENOMEM;
@@ -1379,10 +1696,18 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
         pn533_tx_frame_init(dev->out_frame, PN533_CMD_IN_JUMP_FOR_DEP);
 
         cmd->active = !comm_mode;
-        cmd->baud = 0;
+        cmd->next = 0;
+        cmd->baud = baud;
+        data_ptr = cmd->data;
+        if (comm_mode == NFC_COMM_PASSIVE && cmd->baud > 0) {
+                memcpy(data_ptr, passive_data, PASSIVE_DATA_LEN);
+                cmd->next |= 1;
+                data_ptr += PASSIVE_DATA_LEN;
+        }
+
         if (gb != NULL && gb_len > 0) {
-                cmd->next = 4; /* We have some Gi */
-                memcpy(cmd->gt, gb, gb_len);
+                cmd->next |= 4; /* We have some Gi */
+                memcpy(data_ptr, gb, gb_len);
         } else {
                 cmd->next = 0;
         }
@@ -1407,15 +1732,25 @@ out:
 
 static int pn533_dep_link_down(struct nfc_dev *nfc_dev)
 {
-        pn533_deactivate_target(nfc_dev, 0);
+        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+
+        pn533_poll_reset_mod_list(dev);
+
+        if (dev->tgt_mode || dev->tgt_active_prot) {
+                pn533_send_ack(dev, GFP_KERNEL);
+                usb_kill_urb(dev->in_urb);
+        }
+
+        dev->tgt_active_prot = 0;
+        dev->tgt_mode = 0;
+
+        skb_queue_purge(&dev->resp_q);
 
         return 0;
 }
 
-#define PN533_CMD_DATAEXCH_HEAD_LEN (sizeof(struct pn533_frame) + 3)
-#define PN533_CMD_DATAEXCH_DATA_MAXLEN 262
-
-static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
+static int pn533_build_tx_frame(struct pn533 *dev, struct sk_buff *skb,
+                                bool target)
 {
         int payload_len = skb->len;
         struct pn533_frame *out_frame;
@@ -1432,14 +1767,20 @@ static int pn533_data_exchange_tx_frame(struct pn533 *dev, struct sk_buff *skb)
                 return -ENOSYS;
         }
 
-        skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
-        out_frame = (struct pn533_frame *) skb->data;
+        if (target == true) {
+                skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN);
+                out_frame = (struct pn533_frame *) skb->data;
 
-        pn533_tx_frame_init(out_frame, PN533_CMD_IN_DATA_EXCHANGE);
+                pn533_tx_frame_init(out_frame, PN533_CMD_IN_DATA_EXCHANGE);
+                tg = 1;
+                memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame), &tg, sizeof(u8));
+                out_frame->datalen += sizeof(u8);
+        } else {
+                skb_push(skb, PN533_CMD_DATAEXCH_HEAD_LEN - 1);
+                out_frame = (struct pn533_frame *) skb->data;
+                pn533_tx_frame_init(out_frame, PN533_CMD_TG_SET_DATA);
+        }
 
-        tg = 1;
-        memcpy(PN533_FRAME_CMD_PARAMS_PTR(out_frame), &tg, sizeof(u8));
-        out_frame->datalen += sizeof(u8);
 
         /* The data is already in the out_frame, just update the datalen */
         out_frame->datalen += payload_len;
@@ -1550,9 +1891,9 @@ error:
         return 0;
 }
 
-static int pn533_data_exchange(struct nfc_dev *nfc_dev,
-                               struct nfc_target *target, struct sk_buff *skb,
-                               data_exchange_cb_t cb, void *cb_context)
+static int pn533_transceive(struct nfc_dev *nfc_dev,
+                            struct nfc_target *target, struct sk_buff *skb,
+                            data_exchange_cb_t cb, void *cb_context)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
         struct pn533_frame *out_frame, *in_frame;
@@ -1570,7 +1911,7 @@ static int pn533_data_exchange(struct nfc_dev *nfc_dev,
                 goto error;
         }
 
-        rc = pn533_data_exchange_tx_frame(dev, skb);
+        rc = pn533_build_tx_frame(dev, skb, true);
         if (rc)
                 goto error;
 
@@ -1618,6 +1959,63 @@ error:
         return rc;
 }
 
+static int pn533_tm_send_complete(struct pn533 *dev, void *arg,
+                                  u8 *params, int params_len)
+{
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        if (params_len < 0) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when sending data",
+                            params_len);
+
+                return params_len;
+        }
+
+        if (params_len > 0 && params[0] != 0) {
+                nfc_tm_deactivated(dev->nfc_dev);
+
+                dev->tgt_mode = 0;
+
+                return 0;
+        }
+
+        queue_work(dev->wq, &dev->tg_work);
+
+        return 0;
+}
+
+static int pn533_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+        struct pn533_frame *out_frame;
+        int rc;
+
+        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        rc = pn533_build_tx_frame(dev, skb, false);
+        if (rc)
+                goto error;
+
+        out_frame = (struct pn533_frame *) skb->data;
+
+        rc = pn533_send_cmd_frame_async(dev, out_frame, dev->in_frame,
+                                        dev->in_maxlen, pn533_tm_send_complete,
+                                        NULL, GFP_KERNEL);
+        if (rc) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error %d when trying to send data", rc);
+                goto error;
+        }
+
+        return 0;
+
+error:
+        kfree_skb(skb);
+
+        return rc;
+}
+
 static void pn533_wq_mi_recv(struct work_struct *work)
 {
         struct pn533 *dev = container_of(work, struct pn533, mi_work);
@@ -1638,7 +2036,7 @@ static void pn533_wq_mi_recv(struct work_struct *work)
 
         skb_reserve(skb_cmd, PN533_CMD_DATAEXCH_HEAD_LEN);
 
-        rc = pn533_data_exchange_tx_frame(dev, skb_cmd);
+        rc = pn533_build_tx_frame(dev, skb_cmd, true);
         if (rc)
                 goto error_frame;
 
@@ -1677,7 +2075,7 @@ error_cmd:
 
         kfree(arg);
 
-        up(&dev->cmd_lock);
+        mutex_unlock(&dev->cmd_lock);
 }
 
 static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
@@ -1712,7 +2110,8 @@ struct nfc_ops pn533_nfc_ops = {
         .stop_poll = pn533_stop_poll,
         .activate_target = pn533_activate_target,
         .deactivate_target = pn533_deactivate_target,
-        .data_exchange = pn533_data_exchange,
+        .im_transceive = pn533_transceive,
+        .tm_send = pn533_tm_send,
 };
 
 static int pn533_probe(struct usb_interface *interface,
@@ -1723,6 +2122,7 @@ static int pn533_probe(struct usb_interface *interface,
         struct usb_host_interface *iface_desc;
         struct usb_endpoint_descriptor *endpoint;
         struct pn533_config_max_retries max_retries;
+        struct pn533_config_timing timing;
         int in_endpoint = 0;
         int out_endpoint = 0;
         int rc = -ENOMEM;
@@ -1735,7 +2135,7 @@ static int pn533_probe(struct usb_interface *interface,
 
         dev->udev = usb_get_dev(interface_to_usbdev(interface));
         dev->interface = interface;
-        sema_init(&dev->cmd_lock, 1);
+        mutex_init(&dev->cmd_lock);
 
         iface_desc = interface->cur_altsetting;
         for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
@@ -1779,12 +2179,18 @@ static int pn533_probe(struct usb_interface *interface,
 
         INIT_WORK(&dev->cmd_work, pn533_wq_cmd_complete);
         INIT_WORK(&dev->mi_work, pn533_wq_mi_recv);
+        INIT_WORK(&dev->tg_work, pn533_wq_tg_get_data);
+        INIT_WORK(&dev->poll_work, pn533_wq_poll);
         dev->wq = alloc_workqueue("pn533",
                                   WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
                                   1);
         if (dev->wq == NULL)
                 goto error;
 
+        init_timer(&dev->listen_timer);
+        dev->listen_timer.data = (unsigned long) dev;
+        dev->listen_timer.function = pn533_listen_mode_timer;
+
         skb_queue_head_init(&dev->resp_q);
 
         usb_set_intfdata(interface, dev);
@@ -1830,13 +2236,29 @@ static int pn533_probe(struct usb_interface *interface,
         if (rc) {
                 nfc_dev_err(&dev->interface->dev, "Error on setting MAX_RETRIES"
                                                                 " config");
-                goto free_nfc_dev;
+                goto unregister_nfc_dev;
+        }
+
+        timing.rfu = PN533_CONFIG_TIMING_102;
+        timing.atr_res_timeout = PN533_CONFIG_TIMING_204;
+        timing.dep_timeout = PN533_CONFIG_TIMING_409;
+
+        rc = pn533_set_configuration(dev, PN533_CFGITEM_TIMING,
+                                (u8 *) &timing, sizeof(timing));
+        if (rc) {
+                nfc_dev_err(&dev->interface->dev,
+                            "Error on setting RF timings");
+                goto unregister_nfc_dev;
         }
 
         return 0;
 
+unregister_nfc_dev:
+        nfc_unregister_device(dev->nfc_dev);
+
 free_nfc_dev:
         nfc_free_device(dev->nfc_dev);
+
 destroy_wq:
         destroy_workqueue(dev->wq);
 error:
@@ -1865,6 +2287,8 @@ static void pn533_disconnect(struct usb_interface *interface)
 
         skb_queue_purge(&dev->resp_q);
 
+        del_timer(&dev->listen_timer);
+
         kfree(dev->in_frame);
         usb_free_urb(dev->in_urb);
         kfree(dev->out_frame);
diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544_hci.c
index 281f18c2fb8..457eac35dc7 100644
--- a/drivers/nfc/pn544_hci.c
+++ b/drivers/nfc/pn544_hci.c
@@ -576,7 +576,8 @@ static int pn544_hci_xmit(struct nfc_shdlc *shdlc, struct sk_buff *skb)
         return pn544_hci_i2c_write(client, skb->data, skb->len);
 }
 
-static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
+static int pn544_hci_start_poll(struct nfc_shdlc *shdlc,
+                                u32 im_protocols, u32 tm_protocols)
 {
         struct nfc_hci_dev *hdev = nfc_shdlc_get_hci_dev(shdlc);
         u8 phases = 0;
@@ -584,7 +585,8 @@ static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
         u8 duration[2];
         u8 activated;
 
-        pr_info(DRIVER_DESC ": %s protocols = %d\n", __func__, protocols);
+        pr_info(DRIVER_DESC ": %s protocols 0x%x 0x%x\n",
+                __func__, im_protocols, tm_protocols);
 
         r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
                                NFC_HCI_EVT_END_OPERATION, NULL, 0);
@@ -604,10 +606,10 @@ static int pn544_hci_start_poll(struct nfc_shdlc *shdlc, u32 protocols)
         if (r < 0)
                 return r;
 
-        if (protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
+        if (im_protocols & (NFC_PROTO_ISO14443_MASK | NFC_PROTO_MIFARE_MASK |
                          NFC_PROTO_JEWEL_MASK))
                 phases |= 1;            /* Type A */
-        if (protocols & NFC_PROTO_FELICA_MASK) {
+        if (im_protocols & NFC_PROTO_FELICA_MASK) {
                 phases |= (1 << 2);     /* Type F 212 */
                 phases |= (1 << 3);     /* Type F 424 */
         }
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index f551e537614..266aa1648a0 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -36,6 +36,7 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4328) },
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4329) },
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432b) },
+        { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432c) },
         { 0, },
 };
 MODULE_DEVICE_TABLE(pci, b43_pci_bridge_tbl);
diff --git a/include/linux/bcma/bcma.h b/include/linux/bcma/bcma.h
index 8deaf6d050c..12334f9190c 100644
--- a/include/linux/bcma/bcma.h
+++ b/include/linux/bcma/bcma.h
@@ -70,6 +70,13 @@ struct bcma_host_ops {
 
 /* Core-ID values. */
 #define BCMA_CORE_OOB_ROUTER            0x367   /* Out of band */
+#define BCMA_CORE_4706_CHIPCOMMON       0x500
+#define BCMA_CORE_4706_SOC_RAM          0x50E
+#define BCMA_CORE_4706_MAC_GBIT         0x52D
+#define BCMA_CORE_AMEMC                 0x52E   /* DDR1/2 memory controller core */
+#define BCMA_CORE_ALTA                  0x534   /* I2S core */
+#define BCMA_CORE_4706_MAC_GBIT_COMMON  0x5DC
+#define BCMA_CORE_DDR23_PHY             0x5DD
 #define BCMA_CORE_INVALID               0x700
 #define BCMA_CORE_CHIPCOMMON            0x800
 #define BCMA_CORE_ILINE20               0x801
diff --git a/include/linux/nfc.h b/include/linux/nfc.h
index 0ae9b5857c8..f4e6dd915b1 100644
--- a/include/linux/nfc.h
+++ b/include/linux/nfc.h
@@ -56,6 +56,10 @@
  *      %NFC_ATTR_PROTOCOLS)
  * @NFC_EVENT_DEVICE_REMOVED: event emitted when a device is removed
  *      (it sends %NFC_ATTR_DEVICE_INDEX)
+ * @NFC_EVENT_TM_ACTIVATED: event emitted when the adapter is activated in
+ *      target mode.
+ * @NFC_EVENT_DEVICE_DEACTIVATED: event emitted when the adapter is deactivated
+ *      from target mode.
  */
 enum nfc_commands {
         NFC_CMD_UNSPEC,
@@ -71,6 +75,8 @@ enum nfc_commands {
         NFC_EVENT_DEVICE_ADDED,
         NFC_EVENT_DEVICE_REMOVED,
         NFC_EVENT_TARGET_LOST,
+        NFC_EVENT_TM_ACTIVATED,
+        NFC_EVENT_TM_DEACTIVATED,
 /* private: internal use only */
         __NFC_CMD_AFTER_LAST
 };
@@ -94,6 +100,8 @@ enum nfc_commands {
  * @NFC_ATTR_TARGET_SENSF_RES: NFC-F targets extra information, max 18 bytes
  * @NFC_ATTR_COMM_MODE: Passive or active mode
  * @NFC_ATTR_RF_MODE: Initiator or target
+ * @NFC_ATTR_IM_PROTOCOLS: Initiator mode protocols to poll for
+ * @NFC_ATTR_TM_PROTOCOLS: Target mode protocols to listen for
  */
 enum nfc_attrs {
         NFC_ATTR_UNSPEC,
@@ -109,6 +117,8 @@ enum nfc_attrs {
         NFC_ATTR_COMM_MODE,
         NFC_ATTR_RF_MODE,
         NFC_ATTR_DEVICE_POWERED,
+        NFC_ATTR_IM_PROTOCOLS,
+        NFC_ATTR_TM_PROTOCOLS,
 /* private: internal use only */
         __NFC_ATTR_AFTER_LAST
 };
@@ -118,6 +128,7 @@ enum nfc_attrs {
 #define NFC_NFCID1_MAXSIZE 10
 #define NFC_SENSB_RES_MAXSIZE 12
 #define NFC_SENSF_RES_MAXSIZE 18
+#define NFC_GB_MAXSIZE        48
 
 /* NFC protocols */
 #define NFC_PROTO_JEWEL         1
@@ -135,6 +146,7 @@ enum nfc_attrs {
 /* NFC RF modes */
 #define NFC_RF_INITIATOR 0
 #define NFC_RF_TARGET    1
+#define NFC_RF_NONE      2
 
 /* NFC protocols masks used in bitsets */
 #define NFC_PROTO_JEWEL_MASK    (1 << NFC_PROTO_JEWEL)
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
new file mode 100644
index 00000000000..6a76e0a0705
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,126 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 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.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+#include <net/bluetooth/l2cap.h>
+
+#define A2MP_FEAT_EXT   0x8000
+
+struct amp_mgr {
+        struct l2cap_conn       *l2cap_conn;
+        struct l2cap_chan       *a2mp_chan;
+        struct kref             kref;
+        __u8                    ident;
+        __u8                    handle;
+        unsigned long           flags;
+};
+
+struct a2mp_cmd {
+        __u8    code;
+        __u8    ident;
+        __le16  len;
+        __u8    data[0];
+} __packed;
+
+/* A2MP command codes */
+#define A2MP_COMMAND_REJ         0x01
+struct a2mp_cmd_rej {
+        __le16  reason;
+        __u8    data[0];
+} __packed;
+
+#define A2MP_DISCOVER_REQ        0x02
+struct a2mp_discov_req {
+        __le16  mtu;
+        __le16  ext_feat;
+} __packed;
+
+struct a2mp_cl {
+        __u8    id;
+        __u8    type;
+        __u8    status;
+} __packed;
+
+#define A2MP_DISCOVER_RSP        0x03
+struct a2mp_discov_rsp {
+        __le16     mtu;
+        __le16     ext_feat;
+        struct a2mp_cl cl[0];
+} __packed;
+
+#define A2MP_CHANGE_NOTIFY       0x04
+#define A2MP_CHANGE_RSP          0x05
+
+#define A2MP_GETINFO_REQ         0x06
+struct a2mp_info_req {
+        __u8       id;
+} __packed;
+
+#define A2MP_GETINFO_RSP         0x07
+struct a2mp_info_rsp {
+        __u8    id;
+        __u8    status;
+        __le32  total_bw;
+        __le32  max_bw;
+        __le32  min_latency;
+        __le16  pal_cap;
+        __le16  assoc_size;
+} __packed;
+
+#define A2MP_GETAMPASSOC_REQ     0x08
+struct a2mp_amp_assoc_req {
+        __u8    id;
+} __packed;
+
+#define A2MP_GETAMPASSOC_RSP     0x09
+struct a2mp_amp_assoc_rsp {
+        __u8    id;
+        __u8    status;
+        __u8    amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_REQ  0x0A
+#define A2MP_DISCONNPHYSLINK_REQ 0x0C
+struct a2mp_physlink_req {
+        __u8    local_id;
+        __u8    remote_id;
+        __u8    amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_RSP  0x0B
+#define A2MP_DISCONNPHYSLINK_RSP 0x0D
+struct a2mp_physlink_rsp {
+        __u8    local_id;
+        __u8    remote_id;
+        __u8    status;
+} __packed;
+
+/* A2MP response status */
+#define A2MP_STATUS_SUCCESS                     0x00
+#define A2MP_STATUS_INVALID_CTRL_ID             0x01
+#define A2MP_STATUS_UNABLE_START_LINK_CREATION  0x02
+#define A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS     0x02
+#define A2MP_STATUS_COLLISION_OCCURED           0x03
+#define A2MP_STATUS_DISCONN_REQ_RECVD           0x04
+#define A2MP_STATUS_PHYS_LINK_EXISTS            0x05
+#define A2MP_STATUS_SECURITY_VIOLATION          0x06
+
+void amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+                                       struct sk_buff *skb);
+
+#endif /* __A2MP_H */
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 961669b648f..565d4bee1e4 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -1,4 +1,4 @@
-/* 
+/*
    BlueZ - Bluetooth protocol stack for Linux
    Copyright (C) 2000-2001 Qualcomm Incorporated
 
@@ -12,22 +12,19 @@
    OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
    IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
-   CLAIM, OR ANY SPECIAL 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 
+   CLAIM, OR ANY SPECIAL 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.
 
-   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 
-   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 
+   ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
+   COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
    SOFTWARE IS DISCLAIMED.
 */
 
 #ifndef __BLUETOOTH_H
 #define __BLUETOOTH_H
 
-#include <asm/types.h>
-#include <asm/byteorder.h>
-#include <linux/list.h>
 #include <linux/poll.h>
 #include <net/sock.h>
 
@@ -168,8 +165,8 @@ typedef struct {
 #define BDADDR_LE_PUBLIC        0x01
 #define BDADDR_LE_RANDOM        0x02
 
-#define BDADDR_ANY   (&(bdaddr_t) {{0, 0, 0, 0, 0, 0}})
-#define BDADDR_LOCAL (&(bdaddr_t) {{0, 0, 0, 0xff, 0xff, 0xff}})
+#define BDADDR_ANY   (&(bdaddr_t) {{0, 0, 0, 0, 0, 0} })
+#define BDADDR_LOCAL (&(bdaddr_t) {{0, 0, 0, 0xff, 0xff, 0xff} })
 
 /* Copy, swap, convert BD Address */
 static inline int bacmp(bdaddr_t *ba1, bdaddr_t *ba2)
@@ -215,7 +212,7 @@ int  bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
                                 struct msghdr *msg, size_t len, int flags);
 int  bt_sock_stream_recvmsg(struct kiocb *iocb, struct socket *sock,
                         struct msghdr *msg, size_t len, int flags);
-uint bt_sock_poll(struct file * file, struct socket *sock, poll_table *wait);
+uint bt_sock_poll(struct file *file, struct socket *sock, poll_table *wait);
 int  bt_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg);
 int  bt_sock_wait_state(struct sock *sk, int state, unsigned long timeo);
 
@@ -225,12 +222,12 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
 
 /* Skb helpers */
 struct l2cap_ctrl {
-        unsigned int    sframe  : 1,
-                        poll    : 1,
-                        final   : 1,
-                        fcs     : 1,
-                        sar     : 2,
-                        super   : 2;
+        unsigned int    sframe:1,
+                        poll:1,
+                        final:1,
+                        fcs:1,
+                        sar:2,
+                        super:2;
         __u16           reqseq;
         __u16           txseq;
         __u8            retries;
@@ -249,7 +246,8 @@ static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
 {
         struct sk_buff *skb;
 
-        if ((skb = alloc_skb(len + BT_SKB_RESERVE, how))) {
+        skb = alloc_skb(len + BT_SKB_RESERVE, how);
+        if (skb) {
                 skb_reserve(skb, BT_SKB_RESERVE);
                 bt_cb(skb)->incoming  = 0;
         }
@@ -261,7 +259,8 @@ static inline struct sk_buff *bt_skb_send_alloc(struct sock *sk,
 {
         struct sk_buff *skb;
 
-        if ((skb = sock_alloc_send_skb(sk, len + BT_SKB_RESERVE, nb, err))) {
+        skb = sock_alloc_send_skb(sk, len + BT_SKB_RESERVE, nb, err);
+        if (skb) {
                 skb_reserve(skb, BT_SKB_RESERVE);
                 bt_cb(skb)->incoming  = 0;
         }
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 66a7b579e31..2a6b0b8b712 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -30,6 +30,9 @@
 #define HCI_MAX_EVENT_SIZE      260
 #define HCI_MAX_FRAME_SIZE      (HCI_MAX_ACL_SIZE + 4)
 
+#define HCI_LINK_KEY_SIZE       16
+#define HCI_AMP_LINK_KEY_SIZE   (2 * HCI_LINK_KEY_SIZE)
+
 /* HCI dev events */
 #define HCI_DEV_REG                     1
 #define HCI_DEV_UNREG                   2
@@ -56,9 +59,12 @@
 #define HCI_BREDR       0x00
 #define HCI_AMP         0x01
 
+/* First BR/EDR Controller shall have ID = 0 */
+#define HCI_BREDR_ID    0
+
 /* HCI device quirks */
 enum {
-        HCI_QUIRK_NO_RESET,
+        HCI_QUIRK_RESET_ON_CLOSE,
         HCI_QUIRK_RAW_DEVICE,
         HCI_QUIRK_FIXUP_BUFFER_SIZE
 };
@@ -133,10 +139,8 @@ enum {
 #define HCIINQUIRY      _IOR('H', 240, int)
 
 /* HCI timeouts */
-#define HCI_CONNECT_TIMEOUT     (40000) /* 40 seconds */
 #define HCI_DISCONN_TIMEOUT     (2000)  /* 2 seconds */
 #define HCI_PAIRING_TIMEOUT     (60000) /* 60 seconds */
-#define HCI_IDLE_TIMEOUT        (6000)  /* 6 seconds */
 #define HCI_INIT_TIMEOUT        (10000) /* 10 seconds */
 #define HCI_CMD_TIMEOUT         (1000)  /* 1 seconds */
 #define HCI_ACL_TX_TIMEOUT      (45000) /* 45 seconds */
@@ -371,7 +375,7 @@ struct hci_cp_reject_conn_req {
 #define HCI_OP_LINK_KEY_REPLY           0x040b
 struct hci_cp_link_key_reply {
         bdaddr_t bdaddr;
-        __u8     link_key[16];
+        __u8     link_key[HCI_LINK_KEY_SIZE];
 } __packed;
 
 #define HCI_OP_LINK_KEY_NEG_REPLY       0x040c
@@ -523,6 +527,28 @@ struct hci_cp_io_capability_neg_reply {
         __u8     reason;
 } __packed;
 
+#define HCI_OP_CREATE_PHY_LINK          0x0435
+struct hci_cp_create_phy_link {
+        __u8     phy_handle;
+        __u8     key_len;
+        __u8     key_type;
+        __u8     key[HCI_AMP_LINK_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_ACCEPT_PHY_LINK          0x0436
+struct hci_cp_accept_phy_link {
+        __u8     phy_handle;
+        __u8     key_len;
+        __u8     key_type;
+        __u8     key[HCI_AMP_LINK_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_DISCONN_PHY_LINK 0x0437
+struct hci_cp_disconn_phy_link {
+        __u8     phy_handle;
+        __u8     reason;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE               0x0803
 struct hci_cp_sniff_mode {
         __le16   handle;
@@ -818,6 +844,31 @@ struct hci_rp_read_local_amp_info {
         __le32   be_flush_to;
 } __packed;
 
+#define HCI_OP_READ_LOCAL_AMP_ASSOC     0x140a
+struct hci_cp_read_local_amp_assoc {
+        __u8     phy_handle;
+        __le16   len_so_far;
+        __le16   max_len;
+} __packed;
+struct hci_rp_read_local_amp_assoc {
+        __u8     status;
+        __u8     phy_handle;
+        __le16   rem_len;
+        __u8     frag[0];
+} __packed;
+
+#define HCI_OP_WRITE_REMOTE_AMP_ASSOC   0x140b
+struct hci_cp_write_remote_amp_assoc {
+        __u8     phy_handle;
+        __le16   len_so_far;
+        __le16   rem_len;
+        __u8     frag[0];
+} __packed;
+struct hci_rp_write_remote_amp_assoc {
+        __u8     status;
+        __u8     phy_handle;
+} __packed;
+
 #define HCI_OP_LE_SET_EVENT_MASK        0x2001
 struct hci_cp_le_set_event_mask {
         __u8     mask[8];
@@ -1048,7 +1099,7 @@ struct hci_ev_link_key_req {
 #define HCI_EV_LINK_KEY_NOTIFY          0x18
 struct hci_ev_link_key_notify {
         bdaddr_t bdaddr;
-        __u8     link_key[16];
+        __u8     link_key[HCI_LINK_KEY_SIZE];
         __u8     key_type;
 } __packed;
 
@@ -1144,6 +1195,12 @@ struct extended_inquiry_info {
         __u8     data[240];
 } __packed;
 
+#define HCI_EV_KEY_REFRESH_COMPLETE     0x30
+struct hci_ev_key_refresh_complete {
+        __u8    status;
+        __le16  handle;
+} __packed;
+
 #define HCI_EV_IO_CAPA_REQUEST          0x31
 struct hci_ev_io_capa_request {
         bdaddr_t bdaddr;
@@ -1190,6 +1247,39 @@ struct hci_ev_le_meta {
         __u8     subevent;
 } __packed;
 
+#define HCI_EV_PHY_LINK_COMPLETE        0x40
+struct hci_ev_phy_link_complete {
+        __u8     status;
+        __u8     phy_handle;
+} __packed;
+
+#define HCI_EV_CHANNEL_SELECTED         0x41
+struct hci_ev_channel_selected {
+        __u8     phy_handle;
+} __packed;
+
+#define HCI_EV_DISCONN_PHY_LINK_COMPLETE        0x42
+struct hci_ev_disconn_phy_link_complete {
+        __u8     status;
+        __u8     phy_handle;
+        __u8     reason;
+} __packed;
+
+#define HCI_EV_LOGICAL_LINK_COMPLETE            0x45
+struct hci_ev_logical_link_complete {
+        __u8     status;
+        __le16   handle;
+        __u8     phy_handle;
+        __u8     flow_spec_id;
+} __packed;
+
+#define HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE    0x46
+struct hci_ev_disconn_logical_link_complete {
+        __u8     status;
+        __le16   handle;
+        __u8     reason;
+} __packed;
+
 #define HCI_EV_NUM_COMP_BLOCKS          0x48
 struct hci_comp_blocks_info {
         __le16   handle;
@@ -1290,7 +1380,6 @@ struct hci_sco_hdr {
         __u8    dlen;
 } __packed;
 
-#include <linux/skbuff.h>
 static inline struct hci_event_hdr *hci_event_hdr(const struct sk_buff *skb)
 {
         return (struct hci_event_hdr *) skb->data;
@@ -1307,12 +1396,12 @@ static inline struct hci_sco_hdr *hci_sco_hdr(const struct sk_buff *skb)
 }
 
 /* Command opcode pack/unpack */
-#define hci_opcode_pack(ogf, ocf)       (__u16) ((ocf & 0x03ff)|(ogf << 10))
+#define hci_opcode_pack(ogf, ocf)       ((__u16) ((ocf & 0x03ff)|(ogf << 10)))
 #define hci_opcode_ogf(op)              (op >> 10)
 #define hci_opcode_ocf(op)              (op & 0x03ff)
 
 /* ACL handle and flags pack/unpack */
-#define hci_handle_pack(h, f)   (__u16) ((h & 0x0fff)|(f << 12))
+#define hci_handle_pack(h, f)   ((__u16) ((h & 0x0fff)|(f << 12)))
 #define hci_handle(h)           (h & 0x0fff)
 #define hci_flags(h)            (h >> 12)
 
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 9fc7728f94e..20fd57367dd 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -25,7 +25,6 @@
 #ifndef __HCI_CORE_H
 #define __HCI_CORE_H
 
-#include <linux/interrupt.h>
 #include <net/bluetooth/hci.h>
 
 /* HCI priority */
@@ -65,7 +64,7 @@ struct discovery_state {
                 DISCOVERY_RESOLVING,
                 DISCOVERY_STOPPING,
         } state;
-        struct list_head        all;            /* All devices found during inquiry */
+        struct list_head        all;    /* All devices found during inquiry */
         struct list_head        unknown;        /* Name state not known */
         struct list_head        resolve;        /* Name needs to be resolved */
         __u32                   timestamp;
@@ -105,7 +104,7 @@ struct link_key {
         struct list_head list;
         bdaddr_t bdaddr;
         u8 type;
-        u8 val[16];
+        u8 val[HCI_LINK_KEY_SIZE];
         u8 pin_len;
 };
 
@@ -333,6 +332,7 @@ struct hci_conn {
         void            *l2cap_data;
         void            *sco_data;
         void            *smp_conn;
+        struct amp_mgr  *amp_mgr;
 
         struct hci_conn *link;
 
@@ -360,7 +360,8 @@ extern int l2cap_connect_cfm(struct hci_conn *hcon, u8 status);
 extern int l2cap_disconn_ind(struct hci_conn *hcon);
 extern int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason);
 extern int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt);
-extern int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags);
+extern int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb,
+                              u16 flags);
 
 extern int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr);
 extern int sco_connect_cfm(struct hci_conn *hcon, __u8 status);
@@ -429,8 +430,8 @@ enum {
 static inline bool hci_conn_ssp_enabled(struct hci_conn *conn)
 {
         struct hci_dev *hdev = conn->hdev;
-        return (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags) &&
-                                test_bit(HCI_CONN_SSP_ENABLED, &conn->flags));
+        return test_bit(HCI_SSP_ENABLED, &hdev->dev_flags) &&
+               test_bit(HCI_CONN_SSP_ENABLED, &conn->flags);
 }
 
 static inline void hci_conn_hash_init(struct hci_dev *hdev)
@@ -640,6 +641,19 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
         dev_set_drvdata(&hdev->dev, data);
 }
 
+/* hci_dev_list shall be locked */
+static inline uint8_t __hci_num_ctrl(void)
+{
+        uint8_t count = 0;
+        struct list_head *p;
+
+        list_for_each(p, &hci_dev_list) {
+                count++;
+        }
+
+        return count;
+}
+
 struct hci_dev *hci_dev_get(int index);
 struct hci_dev *hci_get_route(bdaddr_t *src, bdaddr_t *dst);
 
@@ -661,7 +675,8 @@ int hci_get_conn_info(struct hci_dev *hdev, void __user *arg);
 int hci_get_auth_info(struct hci_dev *hdev, void __user *arg);
 int hci_inquiry(void __user *arg);
 
-struct bdaddr_list *hci_blacklist_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr);
+struct bdaddr_list *hci_blacklist_lookup(struct hci_dev *hdev,
+                                         bdaddr_t *bdaddr);
 int hci_blacklist_clear(struct hci_dev *hdev);
 int hci_blacklist_add(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type);
 int hci_blacklist_del(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type);
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 1c7d1cd5e67..d80e3f0691b 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -40,11 +40,11 @@
 #define L2CAP_DEFAULT_MONITOR_TO        12000   /* 12 seconds */
 #define L2CAP_DEFAULT_MAX_PDU_SIZE      1009    /* Sized for 3-DH5 packet */
 #define L2CAP_DEFAULT_ACK_TO            200
-#define L2CAP_LE_DEFAULT_MTU            23
 #define L2CAP_DEFAULT_MAX_SDU_SIZE      0xFFFF
 #define L2CAP_DEFAULT_SDU_ITIME         0xFFFFFFFF
 #define L2CAP_DEFAULT_ACC_LAT           0xFFFFFFFF
 #define L2CAP_BREDR_MAX_PAYLOAD         1019    /* 3-DH5 packet */
+#define L2CAP_LE_MIN_MTU                23
 
 #define L2CAP_DISC_TIMEOUT              msecs_to_jiffies(100)
 #define L2CAP_DISC_REJ_TIMEOUT          msecs_to_jiffies(5000)
@@ -52,6 +52,8 @@
 #define L2CAP_CONN_TIMEOUT              msecs_to_jiffies(40000)
 #define L2CAP_INFO_TIMEOUT              msecs_to_jiffies(4000)
 
+#define L2CAP_A2MP_DEFAULT_MTU          670
+
 /* L2CAP socket address */
 struct sockaddr_l2 {
         sa_family_t     l2_family;
@@ -229,9 +231,14 @@ struct l2cap_conn_rsp {
         __le16     status;
 } __packed;
 
+/* protocol/service multiplexer (PSM) */
+#define L2CAP_PSM_SDP           0x0001
+#define L2CAP_PSM_RFCOMM        0x0003
+
 /* channel indentifier */
 #define L2CAP_CID_SIGNALING     0x0001
 #define L2CAP_CID_CONN_LESS     0x0002
+#define L2CAP_CID_A2MP          0x0003
 #define L2CAP_CID_LE_DATA       0x0004
 #define L2CAP_CID_LE_SIGNALING  0x0005
 #define L2CAP_CID_SMP           0x0006
@@ -271,6 +278,9 @@ struct l2cap_conf_rsp {
 #define L2CAP_CONF_PENDING      0x0004
 #define L2CAP_CONF_EFS_REJECT   0x0005
 
+/* configuration req/rsp continuation flag */
+#define L2CAP_CONF_FLAG_CONTINUATION    0x0001
+
 struct l2cap_conf_opt {
         __u8       type;
         __u8       len;
@@ -419,11 +429,6 @@ struct l2cap_seq_list {
 #define L2CAP_SEQ_LIST_CLEAR    0xFFFF
 #define L2CAP_SEQ_LIST_TAIL     0x8000
 
-struct srej_list {
-        __u16   tx_seq;
-        struct list_head list;
-};
-
 struct l2cap_chan {
         struct sock *sk;
 
@@ -475,14 +480,12 @@ struct l2cap_chan {
         __u16           expected_ack_seq;
         __u16           expected_tx_seq;
         __u16           buffer_seq;
-        __u16           buffer_seq_srej;
         __u16           srej_save_reqseq;
         __u16           last_acked_seq;
         __u16           frames_sent;
         __u16           unacked_frames;
         __u8            retry_count;
         __u16           srej_queue_next;
-        __u8            num_acked;
         __u16           sdu_len;
         struct sk_buff  *sdu;
         struct sk_buff  *sdu_last_frag;
@@ -515,7 +518,6 @@ struct l2cap_chan {
         struct sk_buff_head     srej_q;
         struct l2cap_seq_list   srej_list;
         struct l2cap_seq_list   retrans_list;
-        struct list_head        srej_l;
 
         struct list_head        list;
         struct list_head        global_l;
@@ -528,10 +530,14 @@ struct l2cap_chan {
 struct l2cap_ops {
         char                    *name;
 
-        struct l2cap_chan       *(*new_connection) (void *data);
-        int                     (*recv) (void *data, struct sk_buff *skb);
-        void                    (*close) (void *data);
-        void                    (*state_change) (void *data, int state);
+        struct l2cap_chan       *(*new_connection) (struct l2cap_chan *chan);
+        int                     (*recv) (struct l2cap_chan * chan,
+                                         struct sk_buff *skb);
+        void                    (*teardown) (struct l2cap_chan *chan, int err);
+        void                    (*close) (struct l2cap_chan *chan);
+        void                    (*state_change) (struct l2cap_chan *chan,
+                                                 int state);
+        void                    (*ready) (struct l2cap_chan *chan);
         struct sk_buff          *(*alloc_skb) (struct l2cap_chan *chan,
                                                unsigned long len, int nb);
 };
@@ -575,6 +581,7 @@ struct l2cap_conn {
 #define L2CAP_CHAN_RAW                  1
 #define L2CAP_CHAN_CONN_LESS            2
 #define L2CAP_CHAN_CONN_ORIENTED        3
+#define L2CAP_CHAN_CONN_FIX_A2MP        4
 
 /* ----- L2CAP socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
@@ -597,6 +604,7 @@ enum {
         CONF_EWS_RECV,
         CONF_LOC_CONF_PEND,
         CONF_REM_CONF_PEND,
+        CONF_NOT_COMPLETE,
 };
 
 #define L2CAP_CONF_MAX_CONF_REQ 2
@@ -713,11 +721,7 @@ static inline bool l2cap_clear_timer(struct l2cap_chan *chan,
 
 #define __set_chan_timer(c, t) l2cap_set_timer(c, &c->chan_timer, (t))
 #define __clear_chan_timer(c) l2cap_clear_timer(c, &c->chan_timer)
-#define __set_retrans_timer(c) l2cap_set_timer(c, &c->retrans_timer, \
-                msecs_to_jiffies(L2CAP_DEFAULT_RETRANS_TO));
 #define __clear_retrans_timer(c) l2cap_clear_timer(c, &c->retrans_timer)
-#define __set_monitor_timer(c) l2cap_set_timer(c, &c->monitor_timer, \
-                msecs_to_jiffies(L2CAP_DEFAULT_MONITOR_TO));
 #define __clear_monitor_timer(c) l2cap_clear_timer(c, &c->monitor_timer)
 #define __set_ack_timer(c) l2cap_set_timer(c, &chan->ack_timer, \
                 msecs_to_jiffies(L2CAP_DEFAULT_ACK_TO));
@@ -736,173 +740,17 @@ static inline __u16 __next_seq(struct l2cap_chan *chan, __u16 seq)
         return (seq + 1) % (chan->tx_win_max + 1);
 }
 
-static inline int l2cap_tx_window_full(struct l2cap_chan *ch)
-{
-        int sub;
-
-        sub = (ch->next_tx_seq - ch->expected_ack_seq) % 64;
-
-        if (sub < 0)
-                sub += 64;
-
-        return sub == ch->remote_tx_win;
-}
-
-static inline __u16 __get_reqseq(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (ctrl & L2CAP_EXT_CTRL_REQSEQ) >>
-                                                L2CAP_EXT_CTRL_REQSEQ_SHIFT;
-        else
-                return (ctrl & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT;
-}
-
-static inline __u32 __set_reqseq(struct l2cap_chan *chan, __u32 reqseq)
+static inline struct l2cap_chan *l2cap_chan_no_new_connection(struct l2cap_chan *chan)
 {
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT) &
-                                                        L2CAP_EXT_CTRL_REQSEQ;
-        else
-                return (reqseq << L2CAP_CTRL_REQSEQ_SHIFT) & L2CAP_CTRL_REQSEQ;
+        return NULL;
 }
 
-static inline __u16 __get_txseq(struct l2cap_chan *chan, __u32 ctrl)
+static inline void l2cap_chan_no_teardown(struct l2cap_chan *chan, int err)
 {
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (ctrl & L2CAP_EXT_CTRL_TXSEQ) >>
-                                                L2CAP_EXT_CTRL_TXSEQ_SHIFT;
-        else
-                return (ctrl & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT;
 }
 
-static inline __u32 __set_txseq(struct l2cap_chan *chan, __u32 txseq)
+static inline void l2cap_chan_no_ready(struct l2cap_chan *chan)
 {
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT) &
-                                                        L2CAP_EXT_CTRL_TXSEQ;
-        else
-                return (txseq << L2CAP_CTRL_TXSEQ_SHIFT) & L2CAP_CTRL_TXSEQ;
-}
-
-static inline bool __is_sframe(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return ctrl & L2CAP_EXT_CTRL_FRAME_TYPE;
-        else
-                return ctrl & L2CAP_CTRL_FRAME_TYPE;
-}
-
-static inline __u32 __set_sframe(struct l2cap_chan *chan)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return L2CAP_EXT_CTRL_FRAME_TYPE;
-        else
-                return L2CAP_CTRL_FRAME_TYPE;
-}
-
-static inline __u8 __get_ctrl_sar(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (ctrl & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT;
-        else
-                return (ctrl & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT;
-}
-
-static inline __u32 __set_ctrl_sar(struct l2cap_chan *chan, __u32 sar)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (sar << L2CAP_EXT_CTRL_SAR_SHIFT) & L2CAP_EXT_CTRL_SAR;
-        else
-                return (sar << L2CAP_CTRL_SAR_SHIFT) & L2CAP_CTRL_SAR;
-}
-
-static inline bool __is_sar_start(struct l2cap_chan *chan, __u32 ctrl)
-{
-        return __get_ctrl_sar(chan, ctrl) == L2CAP_SAR_START;
-}
-
-static inline __u32 __get_sar_mask(struct l2cap_chan *chan)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return L2CAP_EXT_CTRL_SAR;
-        else
-                return L2CAP_CTRL_SAR;
-}
-
-static inline __u8 __get_ctrl_super(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (ctrl & L2CAP_EXT_CTRL_SUPERVISE) >>
-                                                L2CAP_EXT_CTRL_SUPER_SHIFT;
-        else
-                return (ctrl & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT;
-}
-
-static inline __u32 __set_ctrl_super(struct l2cap_chan *chan, __u32 super)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return (super << L2CAP_EXT_CTRL_SUPER_SHIFT) &
-                                                L2CAP_EXT_CTRL_SUPERVISE;
-        else
-                return (super << L2CAP_CTRL_SUPER_SHIFT) &
-                                                        L2CAP_CTRL_SUPERVISE;
-}
-
-static inline __u32 __set_ctrl_final(struct l2cap_chan *chan)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return L2CAP_EXT_CTRL_FINAL;
-        else
-                return L2CAP_CTRL_FINAL;
-}
-
-static inline bool __is_ctrl_final(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return ctrl & L2CAP_EXT_CTRL_FINAL;
-        else
-                return ctrl & L2CAP_CTRL_FINAL;
-}
-
-static inline __u32 __set_ctrl_poll(struct l2cap_chan *chan)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return L2CAP_EXT_CTRL_POLL;
-        else
-                return L2CAP_CTRL_POLL;
-}
-
-static inline bool __is_ctrl_poll(struct l2cap_chan *chan, __u32 ctrl)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return ctrl & L2CAP_EXT_CTRL_POLL;
-        else
-                return ctrl & L2CAP_CTRL_POLL;
-}
-
-static inline __u32 __get_control(struct l2cap_chan *chan, void *p)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return get_unaligned_le32(p);
-        else
-                return get_unaligned_le16(p);
-}
-
-static inline void __put_control(struct l2cap_chan *chan, __u32 control,
-                                                                void *p)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return put_unaligned_le32(control, p);
-        else
-                return put_unaligned_le16(control, p);
-}
-
-static inline __u8 __ctrl_size(struct l2cap_chan *chan)
-{
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                return L2CAP_EXT_HDR_SIZE - L2CAP_HDR_SIZE;
-        else
-                return L2CAP_ENH_HDR_SIZE - L2CAP_HDR_SIZE;
 }
 
 extern bool disable_ertm;
@@ -926,5 +774,8 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
+int l2cap_ertm_init(struct l2cap_chan *chan);
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void l2cap_chan_del(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 5e67020b170..b5da094468f 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -1945,6 +1945,11 @@ enum ieee80211_rate_control_changed {
  *      to also unregister the device. If it returns 1, then mac80211
  *      will also go through the regular complete restart on resume.
  *
+ * @set_wakeup: Enable or disable wakeup when WoWLAN configuration is
+ *      modified. The reason is that device_set_wakeup_enable() is
+ *      supposed to be called when the configuration changes, not only
+ *      in suspend().
+ *
  * @add_interface: Called when a netdevice attached to the hardware is
  *      enabled. Because it is not called for monitor mode devices, @start
  *      and @stop must be implemented.
@@ -2979,6 +2984,7 @@ __le16 ieee80211_ctstoself_duration(struct ieee80211_hw *hw,
  * ieee80211_generic_frame_duration - Calculate the duration field for a frame
  * @hw: pointer obtained from ieee80211_alloc_hw().
  * @vif: &struct ieee80211_vif pointer from the add_interface callback.
+ * @band: the band to calculate the frame duration on
  * @frame_len: the length of the frame.
  * @rate: the rate at which the frame is going to be transmitted.
  *
diff --git a/include/net/nfc/hci.h b/include/net/nfc/hci.h
index 4467c946085..e30e6a86971 100644
--- a/include/net/nfc/hci.h
+++ b/include/net/nfc/hci.h
@@ -31,7 +31,8 @@ struct nfc_hci_ops {
         void (*close) (struct nfc_hci_dev *hdev);
         int (*hci_ready) (struct nfc_hci_dev *hdev);
         int (*xmit) (struct nfc_hci_dev *hdev, struct sk_buff *skb);
-        int (*start_poll) (struct nfc_hci_dev *hdev, u32 protocols);
+        int (*start_poll) (struct nfc_hci_dev *hdev,
+                           u32 im_protocols, u32 tm_protocols);
         int (*target_from_gate) (struct nfc_hci_dev *hdev, u8 gate,
                                  struct nfc_target *target);
         int (*complete_target_discovered) (struct nfc_hci_dev *hdev, u8 gate,
diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index b7ca4a2a1d7..180964b954a 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -53,7 +53,8 @@ struct nfc_target;
 struct nfc_ops {
         int (*dev_up)(struct nfc_dev *dev);
         int (*dev_down)(struct nfc_dev *dev);
-        int (*start_poll)(struct nfc_dev *dev, u32 protocols);
+        int (*start_poll)(struct nfc_dev *dev,
+                          u32 im_protocols, u32 tm_protocols);
         void (*stop_poll)(struct nfc_dev *dev);
         int (*dep_link_up)(struct nfc_dev *dev, struct nfc_target *target,
                            u8 comm_mode, u8 *gb, size_t gb_len);
@@ -62,9 +63,10 @@ struct nfc_ops {
                                u32 protocol);
         void (*deactivate_target)(struct nfc_dev *dev,
                                   struct nfc_target *target);
-        int (*data_exchange)(struct nfc_dev *dev, struct nfc_target *target,
+        int (*im_transceive)(struct nfc_dev *dev, struct nfc_target *target,
                              struct sk_buff *skb, data_exchange_cb_t cb,
                              void *cb_context);
+        int (*tm_send)(struct nfc_dev *dev, struct sk_buff *skb);
         int (*check_presence)(struct nfc_dev *dev, struct nfc_target *target);
 };
 
@@ -99,10 +101,10 @@ struct nfc_dev {
         int targets_generation;
         struct device dev;
         bool dev_up;
+        u8 rf_mode;
         bool polling;
         struct nfc_target *active_target;
         bool dep_link_up;
-        u32 dep_rf_mode;
         struct nfc_genl_data genl_data;
         u32 supported_protocols;
 
@@ -188,6 +190,7 @@ struct sk_buff *nfc_alloc_recv_skb(unsigned int size, gfp_t gfp);
 
 int nfc_set_remote_general_bytes(struct nfc_dev *dev,
                                  u8 *gt, u8 gt_len);
+u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, size_t *gb_len);
 
 int nfc_targets_found(struct nfc_dev *dev,
                       struct nfc_target *targets, int ntargets);
@@ -196,4 +199,9 @@ int nfc_target_lost(struct nfc_dev *dev, u32 target_idx);
 int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
                        u8 comm_mode, u8 rf_mode);
 
+int nfc_tm_activated(struct nfc_dev *dev, u32 protocol, u8 comm_mode,
+                     u8 *gb, size_t gb_len);
+int nfc_tm_deactivated(struct nfc_dev *dev);
+int nfc_tm_data_received(struct nfc_dev *dev, struct sk_buff *skb);
+
 #endif /* __NET_NFC_H */
diff --git a/include/net/nfc/shdlc.h b/include/net/nfc/shdlc.h
index ab06afd462d..35e930d2f63 100644
--- a/include/net/nfc/shdlc.h
+++ b/include/net/nfc/shdlc.h
@@ -27,7 +27,8 @@ struct nfc_shdlc_ops {
         void (*close) (struct nfc_shdlc *shdlc);
         int (*hci_ready) (struct nfc_shdlc *shdlc);
         int (*xmit) (struct nfc_shdlc *shdlc, struct sk_buff *skb);
-        int (*start_poll) (struct nfc_shdlc *shdlc, u32 protocols);
+        int (*start_poll) (struct nfc_shdlc *shdlc,
+                           u32 im_protocols, u32 tm_protocols);
         int (*target_from_gate) (struct nfc_shdlc *shdlc, u8 gate,
                                  struct nfc_target *target);
         int (*complete_target_discovered) (struct nfc_shdlc *shdlc, u8 gate,
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 2dc5a5700f5..fa6d94a4602 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_BT_CMTP)	+= cmtp/
 obj-$(CONFIG_BT_HIDP)   += hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
-        hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o
+        hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
+        a2mp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 00000000000..fb93250b393
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,568 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 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.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+
+/* A2MP build & send command helper functions */
+static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
+{
+        struct a2mp_cmd *cmd;
+        int plen;
+
+        plen = sizeof(*cmd) + len;
+        cmd = kzalloc(plen, GFP_KERNEL);
+        if (!cmd)
+                return NULL;
+
+        cmd->code = code;
+        cmd->ident = ident;
+        cmd->len = cpu_to_le16(len);
+
+        memcpy(cmd->data, data, len);
+
+        return cmd;
+}
+
+static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+                      void *data)
+{
+        struct l2cap_chan *chan = mgr->a2mp_chan;
+        struct a2mp_cmd *cmd;
+        u16 total_len = len + sizeof(*cmd);
+        struct kvec iv;
+        struct msghdr msg;
+
+        cmd = __a2mp_build(code, ident, len, data);
+        if (!cmd)
+                return;
+
+        iv.iov_base = cmd;
+        iv.iov_len = total_len;
+
+        memset(&msg, 0, sizeof(msg));
+
+        msg.msg_iov = (struct iovec *) &iv;
+        msg.msg_iovlen = 1;
+
+        l2cap_chan_send(chan, &msg, total_len, 0);
+
+        kfree(cmd);
+}
+
+static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
+{
+        cl->id = 0;
+        cl->type = 0;
+        cl->status = 1;
+}
+
+/* hci_dev_list shall be locked */
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+{
+        int i = 0;
+        struct hci_dev *hdev;
+
+        __a2mp_cl_bredr(cl);
+
+        list_for_each_entry(hdev, &hci_dev_list, list) {
+                /* Iterate through AMP controllers */
+                if (hdev->id == HCI_BREDR_ID)
+                        continue;
+
+                /* Starting from second entry */
+                if (++i >= num_ctrl)
+                        return;
+
+                cl[i].id = hdev->id;
+                cl[i].type = hdev->amp_type;
+                cl[i].status = hdev->amp_status;
+        }
+}
+
+/* Processing A2MP messages */
+static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
+                            struct a2mp_cmd *hdr)
+{
+        struct a2mp_cmd_rej *rej = (void *) skb->data;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*rej))
+                return -EINVAL;
+
+        BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
+
+        skb_pull(skb, sizeof(*rej));
+
+        return 0;
+}
+
+static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                             struct a2mp_cmd *hdr)
+{
+        struct a2mp_discov_req *req = (void *) skb->data;
+        u16 len = le16_to_cpu(hdr->len);
+        struct a2mp_discov_rsp *rsp;
+        u16 ext_feat;
+        u8 num_ctrl;
+
+        if (len < sizeof(*req))
+                return -EINVAL;
+
+        skb_pull(skb, sizeof(*req));
+
+        ext_feat = le16_to_cpu(req->ext_feat);
+
+        BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
+
+        /* check that packet is not broken for now */
+        while (ext_feat & A2MP_FEAT_EXT) {
+                if (len < sizeof(ext_feat))
+                        return -EINVAL;
+
+                ext_feat = get_unaligned_le16(skb->data);
+                BT_DBG("efm 0x%4.4x", ext_feat);
+                len -= sizeof(ext_feat);
+                skb_pull(skb, sizeof(ext_feat));
+        }
+
+        read_lock(&hci_dev_list_lock);
+
+        num_ctrl = __hci_num_ctrl();
+        len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
+        rsp = kmalloc(len, GFP_ATOMIC);
+        if (!rsp) {
+                read_unlock(&hci_dev_list_lock);
+                return -ENOMEM;
+        }
+
+        rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+        rsp->ext_feat = 0;
+
+        __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+
+        read_unlock(&hci_dev_list_lock);
+
+        a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+        kfree(rsp);
+        return 0;
+}
+
+static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
+                              struct a2mp_cmd *hdr)
+{
+        struct a2mp_cl *cl = (void *) skb->data;
+
+        while (skb->len >= sizeof(*cl)) {
+                BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
+                       cl->status);
+                cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
+        }
+
+        /* TODO send A2MP_CHANGE_RSP */
+
+        return 0;
+}
+
+static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                            struct a2mp_cmd *hdr)
+{
+        struct a2mp_info_req *req  = (void *) skb->data;
+        struct a2mp_info_rsp rsp;
+        struct hci_dev *hdev;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*req))
+                return -EINVAL;
+
+        BT_DBG("id %d", req->id);
+
+        rsp.id = req->id;
+        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+        hdev = hci_dev_get(req->id);
+        if (hdev && hdev->amp_type != HCI_BREDR) {
+                rsp.status = 0;
+                rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+                rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+                rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+                rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+                rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+        }
+
+        if (hdev)
+                hci_dev_put(hdev);
+
+        a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+        skb_pull(skb, sizeof(*req));
+        return 0;
+}
+
+static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                                struct a2mp_cmd *hdr)
+{
+        struct a2mp_amp_assoc_req *req = (void *) skb->data;
+        struct hci_dev *hdev;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*req))
+                return -EINVAL;
+
+        BT_DBG("id %d", req->id);
+
+        hdev = hci_dev_get(req->id);
+        if (!hdev || hdev->amp_type == HCI_BREDR) {
+                struct a2mp_amp_assoc_rsp rsp;
+                rsp.id = req->id;
+                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+                a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
+                          &rsp);
+                goto clean;
+        }
+
+        /* Placeholder for HCI Read AMP Assoc */
+
+clean:
+        if (hdev)
+                hci_dev_put(hdev);
+
+        skb_pull(skb, sizeof(*req));
+        return 0;
+}
+
+static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                                   struct a2mp_cmd *hdr)
+{
+        struct a2mp_physlink_req *req = (void *) skb->data;
+
+        struct a2mp_physlink_rsp rsp;
+        struct hci_dev *hdev;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*req))
+                return -EINVAL;
+
+        BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
+
+        rsp.local_id = req->remote_id;
+        rsp.remote_id = req->local_id;
+
+        hdev = hci_dev_get(req->remote_id);
+        if (!hdev || hdev->amp_type != HCI_AMP) {
+                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+                goto send_rsp;
+        }
+
+        /* TODO process physlink create */
+
+        rsp.status = A2MP_STATUS_SUCCESS;
+
+send_rsp:
+        if (hdev)
+                hci_dev_put(hdev);
+
+        a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
+                  &rsp);
+
+        skb_pull(skb, le16_to_cpu(hdr->len));
+        return 0;
+}
+
+static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+                                 struct a2mp_cmd *hdr)
+{
+        struct a2mp_physlink_req *req = (void *) skb->data;
+        struct a2mp_physlink_rsp rsp;
+        struct hci_dev *hdev;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*req))
+                return -EINVAL;
+
+        BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
+
+        rsp.local_id = req->remote_id;
+        rsp.remote_id = req->local_id;
+        rsp.status = A2MP_STATUS_SUCCESS;
+
+        hdev = hci_dev_get(req->local_id);
+        if (!hdev) {
+                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+                goto send_rsp;
+        }
+
+        /* TODO Disconnect Phys Link here */
+
+        hci_dev_put(hdev);
+
+send_rsp:
+        a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+        skb_pull(skb, sizeof(*req));
+        return 0;
+}
+
+static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                               struct a2mp_cmd *hdr)
+{
+        BT_DBG("ident %d code %d", hdr->ident, hdr->code);
+
+        skb_pull(skb, le16_to_cpu(hdr->len));
+        return 0;
+}
+
+/* Handle A2MP signalling */
+static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
+{
+        struct a2mp_cmd *hdr = (void *) skb->data;
+        struct amp_mgr *mgr = chan->data;
+        int err = 0;
+
+        amp_mgr_get(mgr);
+
+        while (skb->len >= sizeof(*hdr)) {
+                struct a2mp_cmd *hdr = (void *) skb->data;
+                u16 len = le16_to_cpu(hdr->len);
+
+                BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
+
+                skb_pull(skb, sizeof(*hdr));
+
+                if (len > skb->len || !hdr->ident) {
+                        err = -EINVAL;
+                        break;
+                }
+
+                mgr->ident = hdr->ident;
+
+                switch (hdr->code) {
+                case A2MP_COMMAND_REJ:
+                        a2mp_command_rej(mgr, skb, hdr);
+                        break;
+
+                case A2MP_DISCOVER_REQ:
+                        err = a2mp_discover_req(mgr, skb, hdr);
+                        break;
+
+                case A2MP_CHANGE_NOTIFY:
+                        err = a2mp_change_notify(mgr, skb, hdr);
+                        break;
+
+                case A2MP_GETINFO_REQ:
+                        err = a2mp_getinfo_req(mgr, skb, hdr);
+                        break;
+
+                case A2MP_GETAMPASSOC_REQ:
+                        err = a2mp_getampassoc_req(mgr, skb, hdr);
+                        break;
+
+                case A2MP_CREATEPHYSLINK_REQ:
+                        err = a2mp_createphyslink_req(mgr, skb, hdr);
+                        break;
+
+                case A2MP_DISCONNPHYSLINK_REQ:
+                        err = a2mp_discphyslink_req(mgr, skb, hdr);
+                        break;
+
+                case A2MP_CHANGE_RSP:
+                case A2MP_DISCOVER_RSP:
+                case A2MP_GETINFO_RSP:
+                case A2MP_GETAMPASSOC_RSP:
+                case A2MP_CREATEPHYSLINK_RSP:
+                case A2MP_DISCONNPHYSLINK_RSP:
+                        err = a2mp_cmd_rsp(mgr, skb, hdr);
+                        break;
+
+                default:
+                        BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
+                        err = -EINVAL;
+                        break;
+                }
+        }
+
+        if (err) {
+                struct a2mp_cmd_rej rej;
+                rej.reason = __constant_cpu_to_le16(0);
+
+                BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
+
+                a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
+                          &rej);
+        }
+
+        /* Always free skb and return success error code to prevent
+           from sending L2CAP Disconnect over A2MP channel */
+        kfree_skb(skb);
+
+        amp_mgr_put(mgr);
+
+        return 0;
+}
+
+static void a2mp_chan_close_cb(struct l2cap_chan *chan)
+{
+        l2cap_chan_destroy(chan);
+}
+
+static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
+{
+        struct amp_mgr *mgr = chan->data;
+
+        if (!mgr)
+                return;
+
+        BT_DBG("chan %p state %s", chan, state_to_string(state));
+
+        chan->state = state;
+
+        switch (state) {
+        case BT_CLOSED:
+                if (mgr)
+                        amp_mgr_put(mgr);
+                break;
+        }
+}
+
+static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
+                                              unsigned long len, int nb)
+{
+        return bt_skb_alloc(len, GFP_KERNEL);
+}
+
+static struct l2cap_ops a2mp_chan_ops = {
+        .name = "L2CAP A2MP channel",
+        .recv = a2mp_chan_recv_cb,
+        .close = a2mp_chan_close_cb,
+        .state_change = a2mp_chan_state_change_cb,
+        .alloc_skb = a2mp_chan_alloc_skb_cb,
+
+        /* Not implemented for A2MP */
+        .new_connection = l2cap_chan_no_new_connection,
+        .teardown = l2cap_chan_no_teardown,
+        .ready = l2cap_chan_no_ready,
+};
+
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+{
+        struct l2cap_chan *chan;
+        int err;
+
+        chan = l2cap_chan_create();
+        if (!chan)
+                return NULL;
+
+        BT_DBG("chan %p", chan);
+
+        chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
+        chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
+
+        chan->ops = &a2mp_chan_ops;
+
+        l2cap_chan_set_defaults(chan);
+        chan->remote_max_tx = chan->max_tx;
+        chan->remote_tx_win = chan->tx_win;
+
+        chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
+        chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
+
+        skb_queue_head_init(&chan->tx_q);
+
+        chan->mode = L2CAP_MODE_ERTM;
+
+        err = l2cap_ertm_init(chan);
+        if (err < 0) {
+                l2cap_chan_del(chan, 0);
+                return NULL;
+        }
+
+        chan->conf_state = 0;
+
+        l2cap_chan_add(conn, chan);
+
+        chan->remote_mps = chan->omtu;
+        chan->mps = chan->omtu;
+
+        chan->state = BT_CONNECTED;
+
+        return chan;
+}
+
+/* AMP Manager functions */
+void amp_mgr_get(struct amp_mgr *mgr)
+{
+        BT_DBG("mgr %p", mgr);
+
+        kref_get(&mgr->kref);
+}
+
+static void amp_mgr_destroy(struct kref *kref)
+{
+        struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
+
+        BT_DBG("mgr %p", mgr);
+
+        kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+        BT_DBG("mgr %p", mgr);
+
+        return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+{
+        struct amp_mgr *mgr;
+        struct l2cap_chan *chan;
+
+        mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+        if (!mgr)
+                return NULL;
+
+        BT_DBG("conn %p mgr %p", conn, mgr);
+
+        mgr->l2cap_conn = conn;
+
+        chan = a2mp_chan_open(conn);
+        if (!chan) {
+                kfree(mgr);
+                return NULL;
+        }
+
+        mgr->a2mp_chan = chan;
+        chan->data = mgr;
+
+        conn->hcon->amp_mgr = mgr;
+
+        kref_init(&mgr->kref);
+
+        return mgr;
+}
+
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+                                       struct sk_buff *skb)
+{
+        struct amp_mgr *mgr;
+
+        mgr = amp_mgr_create(conn);
+        if (!mgr) {
+                BT_ERR("Could not create AMP manager");
+                return NULL;
+        }
+
+        BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+        return mgr->a2mp_chan;
+}
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index 3e18af4dadc..f7db5792ec6 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -25,18 +25,7 @@
 /* Bluetooth address family and sockets. */
 
 #include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/list.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/skbuff.h>
-#include <linux/init.h>
-#include <linux/poll.h>
-#include <net/sock.h>
 #include <asm/ioctls.h>
-#include <linux/kmod.h>
 
 #include <net/bluetooth/bluetooth.h>
 
@@ -418,7 +407,8 @@ static inline unsigned int bt_accept_poll(struct sock *parent)
         return 0;
 }
 
-unsigned int bt_sock_poll(struct file *file, struct socket *sock, poll_table *wait)
+unsigned int bt_sock_poll(struct file *file, struct socket *sock,
+                          poll_table *wait)
 {
         struct sock *sk = sock->sk;
         unsigned int mask = 0;
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index 031d7d65675..4a6620bc157 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -26,26 +26,9 @@
 */
 
 #include <linux/module.h>
-
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/signal.h>
-#include <linux/init.h>
-#include <linux/wait.h>
-#include <linux/freezer.h>
-#include <linux/errno.h>
-#include <linux/net.h>
-#include <linux/slab.h>
 #include <linux/kthread.h>
-#include <net/sock.h>
-
-#include <linux/socket.h>
 #include <linux/file.h>
-
-#include <linux/netdevice.h>
 #include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -306,7 +289,7 @@ static u8 __bnep_rx_hlen[] = {
         ETH_ALEN + 2  /* BNEP_COMPRESSED_DST_ONLY */
 };
 
-static inline int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
+static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb)
 {
         struct net_device *dev = s->dev;
         struct sk_buff *nskb;
@@ -404,7 +387,7 @@ static u8 __bnep_tx_types[] = {
         BNEP_COMPRESSED
 };
 
-static inline int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
+static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb)
 {
         struct ethhdr *eh = (void *) skb->data;
         struct socket *sock = s->sock;
diff --git a/net/bluetooth/bnep/netdev.c b/net/bluetooth/bnep/netdev.c
index bc4086480d9..98f86f91d47 100644
--- a/net/bluetooth/bnep/netdev.c
+++ b/net/bluetooth/bnep/netdev.c
@@ -25,16 +25,8 @@
    SOFTWARE IS DISCLAIMED.
 */
 
-#include <linux/module.h>
-#include <linux/slab.h>
-
-#include <linux/socket.h>
-#include <linux/netdevice.h>
+#include <linux/export.h>
 #include <linux/etherdevice.h>
-#include <linux/skbuff.h>
-#include <linux/wait.h>
-
-#include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -128,7 +120,7 @@ static void bnep_net_timeout(struct net_device *dev)
 }
 
 #ifdef CONFIG_BT_BNEP_MC_FILTER
-static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s)
+static int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s)
 {
         struct ethhdr *eh = (void *) skb->data;
 
@@ -140,7 +132,7 @@ static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s
 
 #ifdef CONFIG_BT_BNEP_PROTO_FILTER
 /* Determine ether protocol. Based on eth_type_trans. */
-static inline u16 bnep_net_eth_proto(struct sk_buff *skb)
+static u16 bnep_net_eth_proto(struct sk_buff *skb)
 {
         struct ethhdr *eh = (void *) skb->data;
         u16 proto = ntohs(eh->h_proto);
@@ -154,7 +146,7 @@ static inline u16 bnep_net_eth_proto(struct sk_buff *skb)
         return ETH_P_802_2;
 }
 
-static inline int bnep_net_proto_filter(struct sk_buff *skb, struct bnep_session *s)
+static int bnep_net_proto_filter(struct sk_buff *skb, struct bnep_session *s)
 {
         u16 proto = bnep_net_eth_proto(skb);
         struct bnep_proto_filter *f = s->proto_filter;
diff --git a/net/bluetooth/bnep/sock.c b/net/bluetooth/bnep/sock.c
index 180bfc45810..5e5f5b410e0 100644
--- a/net/bluetooth/bnep/sock.c
+++ b/net/bluetooth/bnep/sock.c
@@ -24,24 +24,8 @@
    SOFTWARE IS DISCLAIMED.
 */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/capability.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/skbuff.h>
-#include <linux/socket.h>
-#include <linux/ioctl.h>
+#include <linux/export.h>
 #include <linux/file.h>
-#include <linux/init.h>
-#include <linux/compat.h>
-#include <linux/gfp.h>
-#include <linux/uaccess.h>
-#include <net/sock.h>
-
 
 #include "bnep.h"
 
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 3f18a6ed973..2fcced377e5 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -24,24 +24,11 @@
 
 /* Bluetooth HCI connection handling. */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <net/sock.h>
-
-#include <linux/uaccess.h>
-#include <asm/unaligned.h>
+#include <linux/export.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 static void hci_le_connect(struct hci_conn *conn)
 {
@@ -54,15 +41,15 @@ static void hci_le_connect(struct hci_conn *conn)
         conn->sec_level = BT_SECURITY_LOW;
 
         memset(&cp, 0, sizeof(cp));
-        cp.scan_interval = cpu_to_le16(0x0060);
-        cp.scan_window = cpu_to_le16(0x0030);
+        cp.scan_interval = __constant_cpu_to_le16(0x0060);
+        cp.scan_window = __constant_cpu_to_le16(0x0030);
         bacpy(&cp.peer_addr, &conn->dst);
         cp.peer_addr_type = conn->dst_type;
-        cp.conn_interval_min = cpu_to_le16(0x0028);
-        cp.conn_interval_max = cpu_to_le16(0x0038);
-        cp.supervision_timeout = cpu_to_le16(0x002a);
-        cp.min_ce_len = cpu_to_le16(0x0000);
-        cp.max_ce_len = cpu_to_le16(0x0000);
+        cp.conn_interval_min = __constant_cpu_to_le16(0x0028);
+        cp.conn_interval_max = __constant_cpu_to_le16(0x0038);
+        cp.supervision_timeout = __constant_cpu_to_le16(0x002a);
+        cp.min_ce_len = __constant_cpu_to_le16(0x0000);
+        cp.max_ce_len = __constant_cpu_to_le16(0x0000);
 
         hci_send_cmd(hdev, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp);
 }
@@ -99,7 +86,7 @@ void hci_acl_connect(struct hci_conn *conn)
                         cp.pscan_rep_mode = ie->data.pscan_rep_mode;
                         cp.pscan_mode     = ie->data.pscan_mode;
                         cp.clock_offset   = ie->data.clock_offset |
-                                                        cpu_to_le16(0x8000);
+                                            __constant_cpu_to_le16(0x8000);
                 }
 
                 memcpy(conn->dev_class, ie->data.dev_class, 3);
@@ -175,9 +162,9 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
         cp.handle   = cpu_to_le16(handle);
         cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
-        cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
-        cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
-        cp.max_latency    = cpu_to_le16(0xffff);
+        cp.tx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
+        cp.rx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
+        cp.max_latency    = __constant_cpu_to_le16(0xffff);
         cp.voice_setting  = cpu_to_le16(hdev->voice_setting);
         cp.retrans_effort = 0xff;
 
@@ -185,7 +172,7 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
 }
 
 void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
-                                        u16 latency, u16 to_multiplier)
+                        u16 latency, u16 to_multiplier)
 {
         struct hci_cp_le_conn_update cp;
         struct hci_dev *hdev = conn->hdev;
@@ -197,15 +184,14 @@ void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
         cp.conn_interval_max    = cpu_to_le16(max);
         cp.conn_latency         = cpu_to_le16(latency);
         cp.supervision_timeout  = cpu_to_le16(to_multiplier);
-        cp.min_ce_len           = cpu_to_le16(0x0001);
-        cp.max_ce_len           = cpu_to_le16(0x0001);
+        cp.min_ce_len           = __constant_cpu_to_le16(0x0001);
+        cp.max_ce_len           = __constant_cpu_to_le16(0x0001);
 
         hci_send_cmd(hdev, HCI_OP_LE_CONN_UPDATE, sizeof(cp), &cp);
 }
-EXPORT_SYMBOL(hci_le_conn_update);
 
 void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
-                                                        __u8 ltk[16])
+                      __u8 ltk[16])
 {
         struct hci_dev *hdev = conn->hdev;
         struct hci_cp_le_start_enc cp;
@@ -221,7 +207,6 @@ void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
 
         hci_send_cmd(hdev, HCI_OP_LE_START_ENC, sizeof(cp), &cp);
 }
-EXPORT_SYMBOL(hci_le_start_enc);
 
 /* Device _must_ be locked */
 void hci_sco_setup(struct hci_conn *conn, __u8 status)
@@ -247,7 +232,7 @@ void hci_sco_setup(struct hci_conn *conn, __u8 status)
 static void hci_conn_timeout(struct work_struct *work)
 {
         struct hci_conn *conn = container_of(work, struct hci_conn,
-                                                        disc_work.work);
+                                             disc_work.work);
         __u8 reason;
 
         BT_DBG("conn %p state %s", conn, state_to_string(conn->state));
@@ -295,9 +280,9 @@ static void hci_conn_enter_sniff_mode(struct hci_conn *conn)
         if (lmp_sniffsubr_capable(hdev) && lmp_sniffsubr_capable(conn)) {
                 struct hci_cp_sniff_subrate cp;
                 cp.handle             = cpu_to_le16(conn->handle);
-                cp.max_latency        = cpu_to_le16(0);
-                cp.min_remote_timeout = cpu_to_le16(0);
-                cp.min_local_timeout  = cpu_to_le16(0);
+                cp.max_latency        = __constant_cpu_to_le16(0);
+                cp.min_remote_timeout = __constant_cpu_to_le16(0);
+                cp.min_local_timeout  = __constant_cpu_to_le16(0);
                 hci_send_cmd(hdev, HCI_OP_SNIFF_SUBRATE, sizeof(cp), &cp);
         }
 
@@ -306,8 +291,8 @@ static void hci_conn_enter_sniff_mode(struct hci_conn *conn)
                 cp.handle       = cpu_to_le16(conn->handle);
                 cp.max_interval = cpu_to_le16(hdev->sniff_max_interval);
                 cp.min_interval = cpu_to_le16(hdev->sniff_min_interval);
-                cp.attempt      = cpu_to_le16(4);
-                cp.timeout      = cpu_to_le16(1);
+                cp.attempt      = __constant_cpu_to_le16(4);
+                cp.timeout      = __constant_cpu_to_le16(1);
                 hci_send_cmd(hdev, HCI_OP_SNIFF_MODE, sizeof(cp), &cp);
         }
 }
@@ -327,7 +312,7 @@ static void hci_conn_auto_accept(unsigned long arg)
         struct hci_dev *hdev = conn->hdev;
 
         hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_REPLY, sizeof(conn->dst),
-                                                                &conn->dst);
+                     &conn->dst);
 }
 
 struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
@@ -376,7 +361,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
         INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout);
         setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn);
         setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept,
-                                                        (unsigned long) conn);
+                    (unsigned long) conn);
 
         atomic_set(&conn->refcnt, 0);
 
@@ -425,9 +410,11 @@ int hci_conn_del(struct hci_conn *conn)
                 }
         }
 
-
         hci_chan_list_flush(conn);
 
+        if (conn->amp_mgr)
+                amp_mgr_put(conn->amp_mgr);
+
         hci_conn_hash_del(hdev, conn);
         if (hdev->notify)
                 hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
@@ -454,7 +441,8 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
         read_lock(&hci_dev_list_lock);
 
         list_for_each_entry(d, &hci_dev_list, list) {
-                if (!test_bit(HCI_UP, &d->flags) || test_bit(HCI_RAW, &d->flags))
+                if (!test_bit(HCI_UP, &d->flags) ||
+                    test_bit(HCI_RAW, &d->flags))
                         continue;
 
                 /* Simple routing:
@@ -495,6 +483,11 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
         if (type == LE_LINK) {
                 le = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst);
                 if (!le) {
+                        le = hci_conn_hash_lookup_state(hdev, LE_LINK,
+                                                        BT_CONNECT);
+                        if (le)
+                                return ERR_PTR(-EBUSY);
+
                         le = hci_conn_add(hdev, LE_LINK, dst);
                         if (!le)
                                 return ERR_PTR(-ENOMEM);
@@ -545,7 +538,7 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
         hci_conn_hold(sco);
 
         if (acl->state == BT_CONNECTED &&
-                        (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
+            (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
                 set_bit(HCI_CONN_POWER_SAVE, &acl->flags);
                 hci_conn_enter_active_mode(acl, BT_POWER_FORCE_ACTIVE_ON);
 
@@ -560,7 +553,6 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
 
         return sco;
 }
-EXPORT_SYMBOL(hci_connect);
 
 /* Check link security requirement */
 int hci_conn_check_link_mode(struct hci_conn *conn)
@@ -572,7 +564,6 @@ int hci_conn_check_link_mode(struct hci_conn *conn)
 
         return 1;
 }
-EXPORT_SYMBOL(hci_conn_check_link_mode);
 
 /* Authenticate remote device */
 static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
@@ -600,7 +591,7 @@ static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
 
                 cp.handle = cpu_to_le16(conn->handle);
                 hci_send_cmd(conn->hdev, HCI_OP_AUTH_REQUESTED,
-                                                        sizeof(cp), &cp);
+                             sizeof(cp), &cp);
                 if (conn->key_type != 0xff)
                         set_bit(HCI_CONN_REAUTH_PEND, &conn->flags);
         }
@@ -618,7 +609,7 @@ static void hci_conn_encrypt(struct hci_conn *conn)
                 cp.handle  = cpu_to_le16(conn->handle);
                 cp.encrypt = 0x01;
                 hci_send_cmd(conn->hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp),
-                                                                        &cp);
+                             &cp);
         }
 }
 
@@ -648,8 +639,7 @@ int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
         /* An unauthenticated combination key has sufficient security for
            security level 1 and 2. */
         if (conn->key_type == HCI_LK_UNAUTH_COMBINATION &&
-                        (sec_level == BT_SECURITY_MEDIUM ||
-                        sec_level == BT_SECURITY_LOW))
+            (sec_level == BT_SECURITY_MEDIUM || sec_level == BT_SECURITY_LOW))
                 goto encrypt;
 
         /* A combination key has always sufficient security for the security
@@ -657,8 +647,7 @@ int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type)
            is generated using maximum PIN code length (16).
            For pre 2.1 units. */
         if (conn->key_type == HCI_LK_COMBINATION &&
-                        (sec_level != BT_SECURITY_HIGH ||
-                        conn->pin_length == 16))
+            (sec_level != BT_SECURITY_HIGH || conn->pin_length == 16))
                 goto encrypt;
 
 auth:
@@ -701,12 +690,11 @@ int hci_conn_change_link_key(struct hci_conn *conn)
                 struct hci_cp_change_conn_link_key cp;
                 cp.handle = cpu_to_le16(conn->handle);
                 hci_send_cmd(conn->hdev, HCI_OP_CHANGE_CONN_LINK_KEY,
-                                                        sizeof(cp), &cp);
+                             sizeof(cp), &cp);
         }
 
         return 0;
 }
-EXPORT_SYMBOL(hci_conn_change_link_key);
 
 /* Switch role */
 int hci_conn_switch_role(struct hci_conn *conn, __u8 role)
@@ -752,7 +740,7 @@ void hci_conn_enter_active_mode(struct hci_conn *conn, __u8 force_active)
 timer:
         if (hdev->idle_timeout > 0)
                 mod_timer(&conn->idle_timer,
-                        jiffies + msecs_to_jiffies(hdev->idle_timeout));
+                          jiffies + msecs_to_jiffies(hdev->idle_timeout));
 }
 
 /* Drop all connection on the device */
@@ -802,7 +790,7 @@ EXPORT_SYMBOL(hci_conn_put_device);
 
 int hci_get_conn_list(void __user *arg)
 {
-        register struct hci_conn *c;
+        struct hci_conn *c;
         struct hci_conn_list_req req, *cl;
         struct hci_conn_info *ci;
         struct hci_dev *hdev;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 411ace8e647..08994ecc3b6 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -25,28 +25,10 @@
 
 /* Bluetooth HCI core. */
 
-#include <linux/jiffies.h>
-#include <linux/module.h>
-#include <linux/kmod.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/skbuff.h>
-#include <linux/workqueue.h>
-#include <linux/interrupt.h>
-#include <linux/rfkill.h>
-#include <linux/timer.h>
-#include <linux/crypto.h>
-#include <net/sock.h>
+#include <linux/export.h>
+#include <linux/idr.h>
 
-#include <linux/uaccess.h>
-#include <asm/unaligned.h>
+#include <linux/rfkill.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -65,6 +47,9 @@ DEFINE_RWLOCK(hci_dev_list_lock);
 LIST_HEAD(hci_cb_list);
 DEFINE_RWLOCK(hci_cb_list_lock);
 
+/* HCI ID Numbering */
+static DEFINE_IDA(hci_index_ida);
+
 /* ---- HCI notifications ---- */
 
 static void hci_notify(struct hci_dev *hdev, int event)
@@ -124,8 +109,9 @@ static void hci_req_cancel(struct hci_dev *hdev, int err)
 }
 
 /* Execute request and wait for completion. */
-static int __hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt),
-                                        unsigned long opt, __u32 timeout)
+static int __hci_request(struct hci_dev *hdev,
+                         void (*req)(struct hci_dev *hdev, unsigned long opt),
+                         unsigned long opt, __u32 timeout)
 {
         DECLARE_WAITQUEUE(wait, current);
         int err = 0;
@@ -166,8 +152,9 @@ static int __hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev,
         return err;
 }
 
-static inline int hci_request(struct hci_dev *hdev, void (*req)(struct hci_dev *hdev, unsigned long opt),
-                                        unsigned long opt, __u32 timeout)
+static int hci_request(struct hci_dev *hdev,
+                       void (*req)(struct hci_dev *hdev, unsigned long opt),
+                       unsigned long opt, __u32 timeout)
 {
         int ret;
 
@@ -202,7 +189,7 @@ static void bredr_init(struct hci_dev *hdev)
         /* Mandatory initialization */
 
         /* Reset */
-        if (!test_bit(HCI_QUIRK_NO_RESET, &hdev->quirks)) {
+        if (!test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) {
                 set_bit(HCI_RESET, &hdev->flags);
                 hci_send_cmd(hdev, HCI_OP_RESET, 0, NULL);
         }
@@ -235,7 +222,7 @@ static void bredr_init(struct hci_dev *hdev)
         hci_send_cmd(hdev, HCI_OP_SET_EVENT_FLT, 1, &flt_type);
 
         /* Connection accept timeout ~20 secs */
-        param = cpu_to_le16(0x7d00);
+        param = __constant_cpu_to_le16(0x7d00);
         hci_send_cmd(hdev, HCI_OP_WRITE_CA_TIMEOUT, 2, &param);
 
         bacpy(&cp.bdaddr, BDADDR_ANY);
@@ -417,7 +404,8 @@ static void inquiry_cache_flush(struct hci_dev *hdev)
         INIT_LIST_HEAD(&cache->resolve);
 }
 
-struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr)
+struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev,
+                                               bdaddr_t *bdaddr)
 {
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
@@ -478,7 +466,7 @@ void hci_inquiry_cache_update_resolve(struct hci_dev *hdev,
 
         list_for_each_entry(p, &cache->resolve, list) {
                 if (p->name_state != NAME_PENDING &&
-                                abs(p->data.rssi) >= abs(ie->data.rssi))
+                    abs(p->data.rssi) >= abs(ie->data.rssi))
                         break;
                 pos = &p->list;
         }
@@ -503,7 +491,7 @@ bool hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data,
                         *ssp = true;
 
                 if (ie->name_state == NAME_NEEDED &&
-                                                data->rssi != ie->data.rssi) {
+                    data->rssi != ie->data.rssi) {
                         ie->data.rssi = data->rssi;
                         hci_inquiry_cache_update_resolve(hdev, ie);
                 }
@@ -527,7 +515,7 @@ bool hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data,
 
 update:
         if (name_known && ie->name_state != NAME_KNOWN &&
-                                        ie->name_state != NAME_PENDING) {
+            ie->name_state != NAME_PENDING) {
                 ie->name_state = NAME_KNOWN;
                 list_del(&ie->list);
         }
@@ -605,8 +593,7 @@ int hci_inquiry(void __user *arg)
 
         hci_dev_lock(hdev);
         if (inquiry_cache_age(hdev) > INQUIRY_CACHE_AGE_MAX ||
-                                inquiry_cache_empty(hdev) ||
-                                ir.flags & IREQ_CACHE_FLUSH) {
+            inquiry_cache_empty(hdev) || ir.flags & IREQ_CACHE_FLUSH) {
                 inquiry_cache_flush(hdev);
                 do_inquiry = 1;
         }
@@ -620,7 +607,9 @@ int hci_inquiry(void __user *arg)
                         goto done;
         }
 
-        /* for unlimited number of responses we will use buffer with 255 entries */
+        /* for unlimited number of responses we will use buffer with
+         * 255 entries
+         */
         max_rsp = (ir.num_rsp == 0) ? 255 : ir.num_rsp;
 
         /* cache_dump can't sleep. Therefore we allocate temp buffer and then
@@ -641,7 +630,7 @@ int hci_inquiry(void __user *arg)
         if (!copy_to_user(ptr, &ir, sizeof(ir))) {
                 ptr += sizeof(ir);
                 if (copy_to_user(ptr, buf, sizeof(struct inquiry_info) *
-                                        ir.num_rsp))
+                                 ir.num_rsp))
                         err = -EFAULT;
         } else
                 err = -EFAULT;
@@ -702,11 +691,11 @@ int hci_dev_open(__u16 dev)
                 hdev->init_last_cmd = 0;
 
                 ret = __hci_request(hdev, hci_init_req, 0,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                    msecs_to_jiffies(HCI_INIT_TIMEOUT));
 
                 if (lmp_host_le_capable(hdev))
                         ret = __hci_request(hdev, hci_le_init_req, 0,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                            msecs_to_jiffies(HCI_INIT_TIMEOUT));
 
                 clear_bit(HCI_INIT, &hdev->flags);
         }
@@ -791,10 +780,10 @@ static int hci_dev_do_close(struct hci_dev *hdev)
         skb_queue_purge(&hdev->cmd_q);
         atomic_set(&hdev->cmd_cnt, 1);
         if (!test_bit(HCI_RAW, &hdev->flags) &&
-                                test_bit(HCI_QUIRK_NO_RESET, &hdev->quirks)) {
+            test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) {
                 set_bit(HCI_INIT, &hdev->flags);
                 __hci_request(hdev, hci_reset_req, 0,
-                                        msecs_to_jiffies(250));
+                              msecs_to_jiffies(250));
                 clear_bit(HCI_INIT, &hdev->flags);
         }
 
@@ -884,7 +873,7 @@ int hci_dev_reset(__u16 dev)
 
         if (!test_bit(HCI_RAW, &hdev->flags))
                 ret = __hci_request(hdev, hci_reset_req, 0,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                    msecs_to_jiffies(HCI_INIT_TIMEOUT));
 
 done:
         hci_req_unlock(hdev);
@@ -924,7 +913,7 @@ int hci_dev_cmd(unsigned int cmd, void __user *arg)
         switch (cmd) {
         case HCISETAUTH:
                 err = hci_request(hdev, hci_auth_req, dr.dev_opt,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                  msecs_to_jiffies(HCI_INIT_TIMEOUT));
                 break;
 
         case HCISETENCRYPT:
@@ -936,23 +925,23 @@ int hci_dev_cmd(unsigned int cmd, void __user *arg)
                 if (!test_bit(HCI_AUTH, &hdev->flags)) {
                         /* Auth must be enabled first */
                         err = hci_request(hdev, hci_auth_req, dr.dev_opt,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                          msecs_to_jiffies(HCI_INIT_TIMEOUT));
                         if (err)
                                 break;
                 }
 
                 err = hci_request(hdev, hci_encrypt_req, dr.dev_opt,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                  msecs_to_jiffies(HCI_INIT_TIMEOUT));
                 break;
 
         case HCISETSCAN:
                 err = hci_request(hdev, hci_scan_req, dr.dev_opt,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                  msecs_to_jiffies(HCI_INIT_TIMEOUT));
                 break;
 
         case HCISETLINKPOL:
                 err = hci_request(hdev, hci_linkpol_req, dr.dev_opt,
-                                        msecs_to_jiffies(HCI_INIT_TIMEOUT));
+                                  msecs_to_jiffies(HCI_INIT_TIMEOUT));
                 break;
 
         case HCISETLINKMODE:
@@ -1103,7 +1092,7 @@ static void hci_power_on(struct work_struct *work)
 
         if (test_bit(HCI_AUTO_OFF, &hdev->dev_flags))
                 schedule_delayed_work(&hdev->power_off,
-                                        msecs_to_jiffies(AUTO_OFF_TIMEOUT));
+                                      msecs_to_jiffies(AUTO_OFF_TIMEOUT));
 
         if (test_and_clear_bit(HCI_SETUP, &hdev->dev_flags))
                 mgmt_index_added(hdev);
@@ -1112,7 +1101,7 @@ static void hci_power_on(struct work_struct *work)
 static void hci_power_off(struct work_struct *work)
 {
         struct hci_dev *hdev = container_of(work, struct hci_dev,
-                                                        power_off.work);
+                                            power_off.work);
 
         BT_DBG("%s", hdev->name);
 
@@ -1193,7 +1182,7 @@ struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
 }
 
 static bool hci_persistent_key(struct hci_dev *hdev, struct hci_conn *conn,
-                                                u8 key_type, u8 old_key_type)
+                               u8 key_type, u8 old_key_type)
 {
         /* Legacy key */
         if (key_type < 0x03)
@@ -1234,7 +1223,7 @@ struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, __le16 ediv, u8 rand[8])
 
         list_for_each_entry(k, &hdev->long_term_keys, list) {
                 if (k->ediv != ediv ||
-                                memcmp(rand, k->rand, sizeof(k->rand)))
+                    memcmp(rand, k->rand, sizeof(k->rand)))
                         continue;
 
                 return k;
@@ -1242,7 +1231,6 @@ struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, __le16 ediv, u8 rand[8])
 
         return NULL;
 }
-EXPORT_SYMBOL(hci_find_ltk);
 
 struct smp_ltk *hci_find_ltk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr,
                                      u8 addr_type)
@@ -1251,12 +1239,11 @@ struct smp_ltk *hci_find_ltk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr,
 
         list_for_each_entry(k, &hdev->long_term_keys, list)
                 if (addr_type == k->bdaddr_type &&
-                                        bacmp(bdaddr, &k->bdaddr) == 0)
+                    bacmp(bdaddr, &k->bdaddr) == 0)
                         return k;
 
         return NULL;
 }
-EXPORT_SYMBOL(hci_find_ltk_by_addr);
 
 int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
                      bdaddr_t *bdaddr, u8 *val, u8 type, u8 pin_len)
@@ -1283,15 +1270,14 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
          * combination key for legacy pairing even when there's no
          * previous key */
         if (type == HCI_LK_CHANGED_COMBINATION &&
-                                        (!conn || conn->remote_auth == 0xff) &&
-                                        old_key_type == 0xff) {
+            (!conn || conn->remote_auth == 0xff) && old_key_type == 0xff) {
                 type = HCI_LK_COMBINATION;
                 if (conn)
                         conn->key_type = type;
         }
 
         bacpy(&key->bdaddr, bdaddr);
-        memcpy(key->val, val, 16);
+        memcpy(key->val, val, HCI_LINK_KEY_SIZE);
         key->pin_len = pin_len;
 
         if (type == HCI_LK_CHANGED_COMBINATION)
@@ -1540,6 +1526,7 @@ static void le_scan_enable_req(struct hci_dev *hdev, unsigned long opt)
 
         memset(&cp, 0, sizeof(cp));
         cp.enable = 1;
+        cp.filter_dup = 1;
 
         hci_send_cmd(hdev, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
 }
@@ -1707,41 +1694,39 @@ EXPORT_SYMBOL(hci_free_dev);
 /* Register HCI device */
 int hci_register_dev(struct hci_dev *hdev)
 {
-        struct list_head *head, *p;
         int id, error;
 
         if (!hdev->open || !hdev->close)
                 return -EINVAL;
 
-        write_lock(&hci_dev_list_lock);
-
         /* Do not allow HCI_AMP devices to register at index 0,
          * so the index can be used as the AMP controller ID.
          */
-        id = (hdev->dev_type == HCI_BREDR) ? 0 : 1;
-        head = &hci_dev_list;
-
-        /* Find first available device id */
-        list_for_each(p, &hci_dev_list) {
-                int nid = list_entry(p, struct hci_dev, list)->id;
-                if (nid > id)
-                        break;
-                if (nid == id)
-                        id++;
-                head = p;
+        switch (hdev->dev_type) {
+        case HCI_BREDR:
+                id = ida_simple_get(&hci_index_ida, 0, 0, GFP_KERNEL);
+                break;
+        case HCI_AMP:
+                id = ida_simple_get(&hci_index_ida, 1, 0, GFP_KERNEL);
+                break;
+        default:
+                return -EINVAL;
         }
 
+        if (id < 0)
+                return id;
+
         sprintf(hdev->name, "hci%d", id);
         hdev->id = id;
 
         BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
 
-        list_add(&hdev->list, head);
-
+        write_lock(&hci_dev_list_lock);
+        list_add(&hdev->list, &hci_dev_list);
         write_unlock(&hci_dev_list_lock);
 
         hdev->workqueue = alloc_workqueue(hdev->name, WQ_HIGHPRI | WQ_UNBOUND |
-                                                        WQ_MEM_RECLAIM, 1);
+                                          WQ_MEM_RECLAIM, 1);
         if (!hdev->workqueue) {
                 error = -ENOMEM;
                 goto err;
@@ -1752,7 +1737,8 @@ int hci_register_dev(struct hci_dev *hdev)
                 goto err_wqueue;
 
         hdev->rfkill = rfkill_alloc(hdev->name, &hdev->dev,
-                                RFKILL_TYPE_BLUETOOTH, &hci_rfkill_ops, hdev);
+                                    RFKILL_TYPE_BLUETOOTH, &hci_rfkill_ops,
+                                    hdev);
         if (hdev->rfkill) {
                 if (rfkill_register(hdev->rfkill) < 0) {
                         rfkill_destroy(hdev->rfkill);
@@ -1772,6 +1758,7 @@ int hci_register_dev(struct hci_dev *hdev)
 err_wqueue:
         destroy_workqueue(hdev->workqueue);
 err:
+        ida_simple_remove(&hci_index_ida, hdev->id);
         write_lock(&hci_dev_list_lock);
         list_del(&hdev->list);
         write_unlock(&hci_dev_list_lock);
@@ -1783,12 +1770,14 @@ EXPORT_SYMBOL(hci_register_dev);
 /* Unregister HCI device */
 void hci_unregister_dev(struct hci_dev *hdev)
 {
-        int i;
+        int i, id;
 
         BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus);
 
         set_bit(HCI_UNREGISTER, &hdev->dev_flags);
 
+        id = hdev->id;
+
         write_lock(&hci_dev_list_lock);
         list_del(&hdev->list);
         write_unlock(&hci_dev_list_lock);
@@ -1799,7 +1788,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
                 kfree_skb(hdev->reassembly[i]);
 
         if (!test_bit(HCI_INIT, &hdev->flags) &&
-                                !test_bit(HCI_SETUP, &hdev->dev_flags)) {
+            !test_bit(HCI_SETUP, &hdev->dev_flags)) {
                 hci_dev_lock(hdev);
                 mgmt_index_removed(hdev);
                 hci_dev_unlock(hdev);
@@ -1829,6 +1818,8 @@ void hci_unregister_dev(struct hci_dev *hdev)
         hci_dev_unlock(hdev);
 
         hci_dev_put(hdev);
+
+        ida_simple_remove(&hci_index_ida, id);
 }
 EXPORT_SYMBOL(hci_unregister_dev);
 
@@ -1853,7 +1844,7 @@ int hci_recv_frame(struct sk_buff *skb)
 {
         struct hci_dev *hdev = (struct hci_dev *) skb->dev;
         if (!hdev || (!test_bit(HCI_UP, &hdev->flags)
-                                && !test_bit(HCI_INIT, &hdev->flags))) {
+                      && !test_bit(HCI_INIT, &hdev->flags))) {
                 kfree_skb(skb);
                 return -ENXIO;
         }
@@ -1872,7 +1863,7 @@ int hci_recv_frame(struct sk_buff *skb)
 EXPORT_SYMBOL(hci_recv_frame);
 
 static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
-                                                  int count, __u8 index)
+                          int count, __u8 index)
 {
         int len = 0;
         int hlen = 0;
@@ -1881,7 +1872,7 @@ static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
         struct bt_skb_cb *scb;
 
         if ((type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) ||
-                                index >= NUM_REASSEMBLY)
+            index >= NUM_REASSEMBLY)
                 return -EILSEQ;
 
         skb = hdev->reassembly[index];
@@ -2023,7 +2014,7 @@ int hci_recv_stream_fragment(struct hci_dev *hdev, void *data, int count)
                         type = bt_cb(skb)->pkt_type;
 
                 rem = hci_reassembly(hdev, type, data, count,
-                                                        STREAM_REASSEMBLY);
+                                     STREAM_REASSEMBLY);
                 if (rem < 0)
                         return rem;
 
@@ -2157,7 +2148,7 @@ static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
 }
 
 static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
-                                struct sk_buff *skb, __u16 flags)
+                          struct sk_buff *skb, __u16 flags)
 {
         struct hci_dev *hdev = conn->hdev;
         struct sk_buff *list;
@@ -2216,7 +2207,6 @@ void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
-EXPORT_SYMBOL(hci_send_acl);
 
 /* Send SCO data */
 void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb)
@@ -2239,12 +2229,12 @@ void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb)
         skb_queue_tail(&conn->data_q, skb);
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
-EXPORT_SYMBOL(hci_send_sco);
 
 /* ---- HCI TX task (outgoing data) ---- */
 
 /* HCI Connection scheduler */
-static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote)
+static struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type,
+                                     int *quote)
 {
         struct hci_conn_hash *h = &hdev->conn_hash;
         struct hci_conn *conn = NULL, *c;
@@ -2303,7 +2293,7 @@ static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int
         return conn;
 }
 
-static inline void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
+static void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
 {
         struct hci_conn_hash *h = &hdev->conn_hash;
         struct hci_conn *c;
@@ -2316,16 +2306,16 @@ static inline void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
         list_for_each_entry_rcu(c, &h->list, list) {
                 if (c->type == type && c->sent) {
                         BT_ERR("%s killing stalled connection %s",
-                                hdev->name, batostr(&c->dst));
-                        hci_acl_disconn(c, 0x13);
+                               hdev->name, batostr(&c->dst));
+                        hci_acl_disconn(c, HCI_ERROR_REMOTE_USER_TERM);
                 }
         }
 
         rcu_read_unlock();
 }
 
-static inline struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
-                                                int *quote)
+static struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
+                                      int *quote)
 {
         struct hci_conn_hash *h = &hdev->conn_hash;
         struct hci_chan *chan = NULL;
@@ -2442,7 +2432,7 @@ static void hci_prio_recalculate(struct hci_dev *hdev, __u8 type)
                         skb->priority = HCI_PRIO_MAX - 1;
 
                         BT_DBG("chan %p skb %p promoted to %d", chan, skb,
-                                                                skb->priority);
+                               skb->priority);
                 }
 
                 if (hci_conn_num(hdev, type) == num)
@@ -2459,18 +2449,18 @@ static inline int __get_blocks(struct hci_dev *hdev, struct sk_buff *skb)
         return DIV_ROUND_UP(skb->len - HCI_ACL_HDR_SIZE, hdev->block_len);
 }
 
-static inline void __check_timeout(struct hci_dev *hdev, unsigned int cnt)
+static void __check_timeout(struct hci_dev *hdev, unsigned int cnt)
 {
         if (!test_bit(HCI_RAW, &hdev->flags)) {
                 /* ACL tx timeout must be longer than maximum
                  * link supervision timeout (40.9 seconds) */
                 if (!cnt && time_after(jiffies, hdev->acl_last_tx +
-                                        msecs_to_jiffies(HCI_ACL_TX_TIMEOUT)))
+                                       msecs_to_jiffies(HCI_ACL_TX_TIMEOUT)))
                         hci_link_tx_to(hdev, ACL_LINK);
         }
 }
 
-static inline void hci_sched_acl_pkt(struct hci_dev *hdev)
+static void hci_sched_acl_pkt(struct hci_dev *hdev)
 {
         unsigned int cnt = hdev->acl_cnt;
         struct hci_chan *chan;
@@ -2480,11 +2470,11 @@ static inline void hci_sched_acl_pkt(struct hci_dev *hdev)
         __check_timeout(hdev, cnt);
 
         while (hdev->acl_cnt &&
-                        (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
+               (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
                 u32 priority = (skb_peek(&chan->data_q))->priority;
                 while (quote-- && (skb = skb_peek(&chan->data_q))) {
                         BT_DBG("chan %p skb %p len %d priority %u", chan, skb,
-                                        skb->len, skb->priority);
+                               skb->len, skb->priority);
 
                         /* Stop if priority has changed */
                         if (skb->priority < priority)
@@ -2508,7 +2498,7 @@ static inline void hci_sched_acl_pkt(struct hci_dev *hdev)
                 hci_prio_recalculate(hdev, ACL_LINK);
 }
 
-static inline void hci_sched_acl_blk(struct hci_dev *hdev)
+static void hci_sched_acl_blk(struct hci_dev *hdev)
 {
         unsigned int cnt = hdev->block_cnt;
         struct hci_chan *chan;
@@ -2518,13 +2508,13 @@ static inline void hci_sched_acl_blk(struct hci_dev *hdev)
         __check_timeout(hdev, cnt);
 
         while (hdev->block_cnt > 0 &&
-                        (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
+               (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
                 u32 priority = (skb_peek(&chan->data_q))->priority;
                 while (quote > 0 && (skb = skb_peek(&chan->data_q))) {
                         int blocks;
 
                         BT_DBG("chan %p skb %p len %d priority %u", chan, skb,
-                                                skb->len, skb->priority);
+                               skb->len, skb->priority);
 
                         /* Stop if priority has changed */
                         if (skb->priority < priority)
@@ -2537,7 +2527,7 @@ static inline void hci_sched_acl_blk(struct hci_dev *hdev)
                                 return;
 
                         hci_conn_enter_active_mode(chan->conn,
-                                                bt_cb(skb)->force_active);
+                                                   bt_cb(skb)->force_active);
 
                         hci_send_frame(skb);
                         hdev->acl_last_tx = jiffies;
@@ -2554,7 +2544,7 @@ static inline void hci_sched_acl_blk(struct hci_dev *hdev)
                 hci_prio_recalculate(hdev, ACL_LINK);
 }
 
-static inline void hci_sched_acl(struct hci_dev *hdev)
+static void hci_sched_acl(struct hci_dev *hdev)
 {
         BT_DBG("%s", hdev->name);
 
@@ -2573,7 +2563,7 @@ static inline void hci_sched_acl(struct hci_dev *hdev)
 }
 
 /* Schedule SCO */
-static inline void hci_sched_sco(struct hci_dev *hdev)
+static void hci_sched_sco(struct hci_dev *hdev)
 {
         struct hci_conn *conn;
         struct sk_buff *skb;
@@ -2596,7 +2586,7 @@ static inline void hci_sched_sco(struct hci_dev *hdev)
         }
 }
 
-static inline void hci_sched_esco(struct hci_dev *hdev)
+static void hci_sched_esco(struct hci_dev *hdev)
 {
         struct hci_conn *conn;
         struct sk_buff *skb;
@@ -2607,7 +2597,8 @@ static inline void hci_sched_esco(struct hci_dev *hdev)
         if (!hci_conn_num(hdev, ESCO_LINK))
                 return;
 
-        while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK, &quote))) {
+        while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK,
+                                                     &quote))) {
                 while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                         BT_DBG("skb %p len %d", skb, skb->len);
                         hci_send_frame(skb);
@@ -2619,7 +2610,7 @@ static inline void hci_sched_esco(struct hci_dev *hdev)
         }
 }
 
-static inline void hci_sched_le(struct hci_dev *hdev)
+static void hci_sched_le(struct hci_dev *hdev)
 {
         struct hci_chan *chan;
         struct sk_buff *skb;
@@ -2634,7 +2625,7 @@ static inline void hci_sched_le(struct hci_dev *hdev)
                 /* LE tx timeout must be longer than maximum
                  * link supervision timeout (40.9 seconds) */
                 if (!hdev->le_cnt && hdev->le_pkts &&
-                                time_after(jiffies, hdev->le_last_tx + HZ * 45))
+                    time_after(jiffies, hdev->le_last_tx + HZ * 45))
                         hci_link_tx_to(hdev, LE_LINK);
         }
 
@@ -2644,7 +2635,7 @@ static inline void hci_sched_le(struct hci_dev *hdev)
                 u32 priority = (skb_peek(&chan->data_q))->priority;
                 while (quote-- && (skb = skb_peek(&chan->data_q))) {
                         BT_DBG("chan %p skb %p len %d priority %u", chan, skb,
-                                        skb->len, skb->priority);
+                               skb->len, skb->priority);
 
                         /* Stop if priority has changed */
                         if (skb->priority < priority)
@@ -2676,7 +2667,7 @@ static void hci_tx_work(struct work_struct *work)
         struct sk_buff *skb;
 
         BT_DBG("%s acl %d sco %d le %d", hdev->name, hdev->acl_cnt,
-                hdev->sco_cnt, hdev->le_cnt);
+               hdev->sco_cnt, hdev->le_cnt);
 
         /* Schedule queues and send stuff to HCI driver */
 
@@ -2696,7 +2687,7 @@ static void hci_tx_work(struct work_struct *work)
 /* ----- HCI RX task (incoming data processing) ----- */
 
 /* ACL data packet */
-static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_acl_hdr *hdr = (void *) skb->data;
         struct hci_conn *conn;
@@ -2708,7 +2699,8 @@ static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
         flags  = hci_flags(handle);
         handle = hci_handle(handle);
 
-        BT_DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len, handle, flags);
+        BT_DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len,
+               handle, flags);
 
         hdev->stat.acl_rx++;
 
@@ -2732,14 +2724,14 @@ static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 return;
         } else {
                 BT_ERR("%s ACL packet for unknown connection handle %d",
-                        hdev->name, handle);
+                       hdev->name, handle);
         }
 
         kfree_skb(skb);
 }
 
 /* SCO data packet */
-static inline void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_sco_hdr *hdr = (void *) skb->data;
         struct hci_conn *conn;
@@ -2763,7 +2755,7 @@ static inline void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 return;
         } else {
                 BT_ERR("%s SCO packet for unknown connection handle %d",
-                        hdev->name, handle);
+                       hdev->name, handle);
         }
 
         kfree_skb(skb);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 4eefb7f65cf..1ba929c05d0 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -24,20 +24,7 @@
 
 /* Bluetooth HCI event handling. */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <net/sock.h>
-
-#include <linux/uaccess.h>
+#include <linux/export.h>
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -95,7 +82,8 @@ static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb)
         hci_conn_check_pending(hdev);
 }
 
-static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev,
+                                          struct sk_buff *skb)
 {
         BT_DBG("%s", hdev->name);
 }
@@ -166,7 +154,8 @@ static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb)
         hci_dev_unlock(hdev);
 }
 
-static void hci_cc_read_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_read_def_link_policy(struct hci_dev *hdev,
+                                        struct sk_buff *skb)
 {
         struct hci_rp_read_def_link_policy *rp = (void *) skb->data;
 
@@ -178,7 +167,8 @@ static void hci_cc_read_def_link_policy(struct hci_dev *hdev, struct sk_buff *sk
         hdev->link_policy = __le16_to_cpu(rp->policy);
 }
 
-static void hci_cc_write_def_link_policy(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_write_def_link_policy(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
         void *sent;
@@ -329,7 +319,7 @@ static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb)
                 if (hdev->discov_timeout > 0) {
                         int to = msecs_to_jiffies(hdev->discov_timeout * 1000);
                         queue_delayed_work(hdev->workqueue, &hdev->discov_off,
-                                                                        to);
+                                           to);
                 }
         } else if (old_iscan)
                 mgmt_discoverable(hdev, 0);
@@ -358,7 +348,7 @@ static void hci_cc_read_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb)
         memcpy(hdev->dev_class, rp->dev_class, 3);
 
         BT_DBG("%s class 0x%.2x%.2x%.2x", hdev->name,
-                hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]);
+               hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]);
 }
 
 static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb)
@@ -406,7 +396,8 @@ static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb)
                 hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING);
 }
 
-static void hci_cc_write_voice_setting(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_write_voice_setting(struct hci_dev *hdev,
+                                       struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
         __u16 setting;
@@ -473,7 +464,7 @@ static u8 hci_get_inquiry_mode(struct hci_dev *hdev)
                 return 1;
 
         if (hdev->manufacturer == 11 && hdev->hci_rev == 0x00 &&
-                                                hdev->lmp_subver == 0x0757)
+            hdev->lmp_subver == 0x0757)
                 return 1;
 
         if (hdev->manufacturer == 15) {
@@ -486,7 +477,7 @@ static u8 hci_get_inquiry_mode(struct hci_dev *hdev)
         }
 
         if (hdev->manufacturer == 31 && hdev->hci_rev == 0x2005 &&
-                                                hdev->lmp_subver == 0x1805)
+            hdev->lmp_subver == 0x1805)
                 return 1;
 
         return 0;
@@ -566,7 +557,7 @@ static void hci_setup(struct hci_dev *hdev)
         if (hdev->hci_ver > BLUETOOTH_VER_1_1)
                 hci_send_cmd(hdev, HCI_OP_READ_LOCAL_COMMANDS, 0, NULL);
 
-        if (hdev->features[6] & LMP_SIMPLE_PAIR) {
+        if (lmp_ssp_capable(hdev)) {
                 if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) {
                         u8 mode = 0x01;
                         hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE,
@@ -618,8 +609,7 @@ static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb)
         hdev->lmp_subver = __le16_to_cpu(rp->lmp_subver);
 
         BT_DBG("%s manufacturer %d hci ver %d:%d", hdev->name,
-                                        hdev->manufacturer,
-                                        hdev->hci_ver, hdev->hci_rev);
+               hdev->manufacturer, hdev->hci_ver, hdev->hci_rev);
 
         if (test_bit(HCI_INIT, &hdev->flags))
                 hci_setup(hdev);
@@ -646,7 +636,8 @@ static void hci_setup_link_policy(struct hci_dev *hdev)
         hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, sizeof(cp), &cp);
 }
 
-static void hci_cc_read_local_commands(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_read_local_commands(struct hci_dev *hdev,
+                                       struct sk_buff *skb)
 {
         struct hci_rp_read_local_commands *rp = (void *) skb->data;
 
@@ -664,7 +655,8 @@ done:
         hci_req_complete(hdev, HCI_OP_READ_LOCAL_COMMANDS, rp->status);
 }
 
-static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cc_read_local_features(struct hci_dev *hdev,
+                                       struct sk_buff *skb)
 {
         struct hci_rp_read_local_features *rp = (void *) skb->data;
 
@@ -713,10 +705,10 @@ static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb
                 hdev->esco_type |= (ESCO_2EV5 | ESCO_3EV5);
 
         BT_DBG("%s features 0x%.2x%.2x%.2x%.2x%.2x%.2x%.2x%.2x", hdev->name,
-                                        hdev->features[0], hdev->features[1],
-                                        hdev->features[2], hdev->features[3],
-                                        hdev->features[4], hdev->features[5],
-                                        hdev->features[6], hdev->features[7]);
+               hdev->features[0], hdev->features[1],
+               hdev->features[2], hdev->features[3],
+               hdev->features[4], hdev->features[5],
+               hdev->features[6], hdev->features[7]);
 }
 
 static void hci_set_le_support(struct hci_dev *hdev)
@@ -736,7 +728,7 @@ static void hci_set_le_support(struct hci_dev *hdev)
 }
 
 static void hci_cc_read_local_ext_features(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                           struct sk_buff *skb)
 {
         struct hci_rp_read_local_ext_features *rp = (void *) skb->data;
 
@@ -762,7 +754,7 @@ done:
 }
 
 static void hci_cc_read_flow_control_mode(struct hci_dev *hdev,
-                                                struct sk_buff *skb)
+                                          struct sk_buff *skb)
 {
         struct hci_rp_read_flow_control_mode *rp = (void *) skb->data;
 
@@ -798,9 +790,8 @@ static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb)
         hdev->acl_cnt = hdev->acl_pkts;
         hdev->sco_cnt = hdev->sco_pkts;
 
-        BT_DBG("%s acl mtu %d:%d sco mtu %d:%d", hdev->name,
-                                        hdev->acl_mtu, hdev->acl_pkts,
-                                        hdev->sco_mtu, hdev->sco_pkts);
+        BT_DBG("%s acl mtu %d:%d sco mtu %d:%d", hdev->name, hdev->acl_mtu,
+               hdev->acl_pkts, hdev->sco_mtu, hdev->sco_pkts);
 }
 
 static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb)
@@ -816,7 +807,7 @@ static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_data_block_size(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                        struct sk_buff *skb)
 {
         struct hci_rp_read_data_block_size *rp = (void *) skb->data;
 
@@ -832,7 +823,7 @@ static void hci_cc_read_data_block_size(struct hci_dev *hdev,
         hdev->block_cnt = hdev->num_blocks;
 
         BT_DBG("%s blk mtu %d cnt %d len %d", hdev->name, hdev->block_mtu,
-                                        hdev->block_cnt, hdev->block_len);
+               hdev->block_cnt, hdev->block_len);
 
         hci_req_complete(hdev, HCI_OP_READ_DATA_BLOCK_SIZE, rp->status);
 }
@@ -847,7 +838,7 @@ static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
-                struct sk_buff *skb)
+                                       struct sk_buff *skb)
 {
         struct hci_rp_read_local_amp_info *rp = (void *) skb->data;
 
@@ -871,7 +862,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                          struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
 
@@ -890,7 +881,7 @@ static void hci_cc_set_event_mask(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_write_inquiry_mode(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                      struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
 
@@ -900,7 +891,7 @@ static void hci_cc_write_inquiry_mode(struct hci_dev *hdev,
 }
 
 static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                         struct sk_buff *skb)
 {
         struct hci_rp_read_inq_rsp_tx_power *rp = (void *) skb->data;
 
@@ -959,7 +950,7 @@ static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb)
 
         if (test_bit(HCI_MGMT, &hdev->dev_flags))
                 mgmt_pin_code_neg_reply_complete(hdev, &rp->bdaddr,
-                                                                rp->status);
+                                                 rp->status);
 
         hci_dev_unlock(hdev);
 }
@@ -1000,7 +991,7 @@ static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                          struct sk_buff *skb)
 {
         struct hci_rp_user_confirm_reply *rp = (void *) skb->data;
 
@@ -1031,7 +1022,7 @@ static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                          struct sk_buff *skb)
 {
         struct hci_rp_user_confirm_reply *rp = (void *) skb->data;
 
@@ -1047,7 +1038,7 @@ static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev,
 }
 
 static void hci_cc_read_local_oob_data_reply(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+                                             struct sk_buff *skb)
 {
         struct hci_rp_read_local_oob_data *rp = (void *) skb->data;
 
@@ -1076,7 +1067,7 @@ static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_le_set_scan_enable(struct hci_dev *hdev,
-                                        struct sk_buff *skb)
+                                      struct sk_buff *skb)
 {
         struct hci_cp_le_set_scan_enable *cp;
         __u8 status = *((__u8 *) skb->data);
@@ -1156,8 +1147,8 @@ static void hci_cc_le_ltk_neg_reply(struct hci_dev *hdev, struct sk_buff *skb)
         hci_req_complete(hdev, HCI_OP_LE_LTK_NEG_REPLY, rp->status);
 }
 
-static inline void hci_cc_write_le_host_supported(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
+                                           struct sk_buff *skb)
 {
         struct hci_cp_write_le_host_supported *sent;
         __u8 status = *((__u8 *) skb->data);
@@ -1176,13 +1167,13 @@ static inline void hci_cc_write_le_host_supported(struct hci_dev *hdev,
         }
 
         if (test_bit(HCI_MGMT, &hdev->dev_flags) &&
-                                        !test_bit(HCI_INIT, &hdev->flags))
+            !test_bit(HCI_INIT, &hdev->flags))
                 mgmt_le_enable_complete(hdev, sent->le, status);
 
         hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
-static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
+static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
         BT_DBG("%s status 0x%x", hdev->name, status);
 
@@ -1203,7 +1194,7 @@ static inline void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
+static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
 {
         struct hci_cp_create_conn *cp;
         struct hci_conn *conn;
@@ -1333,7 +1324,7 @@ static void hci_cs_set_conn_encrypt(struct hci_dev *hdev, __u8 status)
 }
 
 static int hci_outgoing_auth_needed(struct hci_dev *hdev,
-                                                        struct hci_conn *conn)
+                                    struct hci_conn *conn)
 {
         if (conn->state != BT_CONFIG || !conn->out)
                 return 0;
@@ -1343,15 +1334,14 @@ static int hci_outgoing_auth_needed(struct hci_dev *hdev,
 
         /* Only request authentication for SSP connections or non-SSP
          * devices with sec_level HIGH or if MITM protection is requested */
-        if (!hci_conn_ssp_enabled(conn) &&
-                                conn->pending_sec_level != BT_SECURITY_HIGH &&
-                                !(conn->auth_type & 0x01))
+        if (!hci_conn_ssp_enabled(conn) && !(conn->auth_type & 0x01) &&
+            conn->pending_sec_level != BT_SECURITY_HIGH)
                 return 0;
 
         return 1;
 }
 
-static inline int hci_resolve_name(struct hci_dev *hdev,
+static int hci_resolve_name(struct hci_dev *hdev,
                                    struct inquiry_entry *e)
 {
         struct hci_cp_remote_name_req cp;
@@ -1638,7 +1628,7 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
         conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->peer_addr);
 
         BT_DBG("%s bdaddr %s conn %p", hdev->name, batostr(&cp->peer_addr),
-                conn);
+               conn);
 
         if (status) {
                 if (conn && conn->state == BT_CONNECT) {
@@ -1668,7 +1658,7 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
         BT_DBG("%s status 0x%x", hdev->name, status);
 }
 
-static inline void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
         struct discovery_state *discov = &hdev->discovery;
@@ -1708,7 +1698,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct inquiry_data data;
         struct inquiry_info *info = (void *) (skb->data + 1);
@@ -1745,7 +1735,7 @@ static inline void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_conn_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -1823,18 +1813,18 @@ unlock:
         hci_conn_check_pending(hdev);
 }
 
-static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_conn_request *ev = (void *) skb->data;
         int mask = hdev->link_mode;
 
-        BT_DBG("%s bdaddr %s type 0x%x", hdev->name,
-                                        batostr(&ev->bdaddr), ev->link_type);
+        BT_DBG("%s bdaddr %s type 0x%x", hdev->name, batostr(&ev->bdaddr),
+               ev->link_type);
 
         mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type);
 
         if ((mask & HCI_LM_ACCEPT) &&
-                        !hci_blacklist_lookup(hdev, &ev->bdaddr)) {
+            !hci_blacklist_lookup(hdev, &ev->bdaddr)) {
                 /* Connection accepted */
                 struct inquiry_entry *ie;
                 struct hci_conn *conn;
@@ -1845,7 +1835,8 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
                 if (ie)
                         memcpy(ie->data.dev_class, ev->dev_class, 3);
 
-                conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
+                conn = hci_conn_hash_lookup_ba(hdev, ev->link_type,
+                                               &ev->bdaddr);
                 if (!conn) {
                         conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr);
                         if (!conn) {
@@ -1878,9 +1869,9 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
                         bacpy(&cp.bdaddr, &ev->bdaddr);
                         cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
-                        cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
-                        cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
-                        cp.max_latency    = cpu_to_le16(0xffff);
+                        cp.tx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
+                        cp.rx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
+                        cp.max_latency    = __constant_cpu_to_le16(0xffff);
                         cp.content_format = cpu_to_le16(hdev->voice_setting);
                         cp.retrans_effort = 0xff;
 
@@ -1897,7 +1888,7 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
         }
 }
 
-static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_disconn_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -1914,10 +1905,10 @@ static inline void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff
                 conn->state = BT_CLOSED;
 
         if (test_and_clear_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags) &&
-                        (conn->type == ACL_LINK || conn->type == LE_LINK)) {
+            (conn->type == ACL_LINK || conn->type == LE_LINK)) {
                 if (ev->status != 0)
                         mgmt_disconnect_failed(hdev, &conn->dst, conn->type,
-                                                conn->dst_type, ev->status);
+                                               conn->dst_type, ev->status);
                 else
                         mgmt_device_disconnected(hdev, &conn->dst, conn->type,
                                                  conn->dst_type);
@@ -1934,7 +1925,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_auth_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -1949,7 +1940,7 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
 
         if (!ev->status) {
                 if (!hci_conn_ssp_enabled(conn) &&
-                                test_bit(HCI_CONN_REAUTH_PEND, &conn->flags)) {
+                    test_bit(HCI_CONN_REAUTH_PEND, &conn->flags)) {
                         BT_INFO("re-auth of legacy device is not possible.");
                 } else {
                         conn->link_mode |= HCI_LM_AUTH;
@@ -1969,7 +1960,7 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                         cp.handle  = ev->handle;
                         cp.encrypt = 0x01;
                         hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp),
-                                                                        &cp);
+                                     &cp);
                 } else {
                         conn->state = BT_CONNECTED;
                         hci_proto_connect_cfm(conn, ev->status);
@@ -1989,7 +1980,7 @@ static inline void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                         cp.handle  = ev->handle;
                         cp.encrypt = 0x01;
                         hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp),
-                                                                        &cp);
+                                     &cp);
                 } else {
                         clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags);
                         hci_encrypt_cfm(conn, ev->status, 0x00);
@@ -2000,7 +1991,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_remote_name *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2039,7 +2030,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_encrypt_change *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2082,7 +2073,8 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_change_link_key_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_change_link_key_complete_evt(struct hci_dev *hdev,
+                                             struct sk_buff *skb)
 {
         struct hci_ev_change_link_key_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2104,7 +2096,8 @@ static inline void hci_change_link_key_complete_evt(struct hci_dev *hdev, struct
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_remote_features_evt(struct hci_dev *hdev,
+                                    struct sk_buff *skb)
 {
         struct hci_ev_remote_features *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2128,7 +2121,7 @@ static inline void hci_remote_features_evt(struct hci_dev *hdev, struct sk_buff
                 cp.handle = ev->handle;
                 cp.page = 0x01;
                 hci_send_cmd(hdev, HCI_OP_READ_REMOTE_EXT_FEATURES,
-                                                        sizeof(cp), &cp);
+                             sizeof(cp), &cp);
                 goto unlock;
         }
 
@@ -2153,17 +2146,18 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_version_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_remote_version_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         BT_DBG("%s", hdev->name);
 }
 
-static inline void hci_qos_setup_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_qos_setup_complete_evt(struct hci_dev *hdev,
+                                       struct sk_buff *skb)
 {
         BT_DBG("%s", hdev->name);
 }
 
-static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_cmd_complete *ev = (void *) skb->data;
         __u16 opcode;
@@ -2384,7 +2378,7 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
         }
 }
 
-static inline void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_cmd_status *ev = (void *) skb->data;
         __u16 opcode;
@@ -2465,7 +2459,7 @@ static inline void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
         }
 }
 
-static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_role_change *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2491,7 +2485,7 @@ static inline void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_num_comp_pkts *ev = (void *) skb->data;
         int i;
@@ -2502,7 +2496,7 @@ static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *s
         }
 
         if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) +
-                        ev->num_hndl * sizeof(struct hci_comp_pkts_info)) {
+            ev->num_hndl * sizeof(struct hci_comp_pkts_info)) {
                 BT_DBG("%s bad parameters", hdev->name);
                 return;
         }
@@ -2557,8 +2551,7 @@ static inline void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *s
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
 
-static inline void hci_num_comp_blocks_evt(struct hci_dev *hdev,
-                                           struct sk_buff *skb)
+static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_num_comp_blocks *ev = (void *) skb->data;
         int i;
@@ -2569,13 +2562,13 @@ static inline void hci_num_comp_blocks_evt(struct hci_dev *hdev,
         }
 
         if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) +
-                        ev->num_hndl * sizeof(struct hci_comp_blocks_info)) {
+            ev->num_hndl * sizeof(struct hci_comp_blocks_info)) {
                 BT_DBG("%s bad parameters", hdev->name);
                 return;
         }
 
         BT_DBG("%s num_blocks %d num_hndl %d", hdev->name, ev->num_blocks,
-                                                                ev->num_hndl);
+               ev->num_hndl);
 
         for (i = 0; i < ev->num_hndl; i++) {
                 struct hci_comp_blocks_info *info = &ev->handles[i];
@@ -2607,7 +2600,7 @@ static inline void hci_num_comp_blocks_evt(struct hci_dev *hdev,
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
 
-static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_mode_change *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2621,7 +2614,8 @@ static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb
                 conn->mode = ev->mode;
                 conn->interval = __le16_to_cpu(ev->interval);
 
-                if (!test_and_clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) {
+                if (!test_and_clear_bit(HCI_CONN_MODE_CHANGE_PEND,
+                                        &conn->flags)) {
                         if (conn->mode == HCI_CM_ACTIVE)
                                 set_bit(HCI_CONN_POWER_SAVE, &conn->flags);
                         else
@@ -2635,7 +2629,7 @@ static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_pin_code_req *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2656,7 +2650,7 @@ static inline void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff
 
         if (!test_bit(HCI_PAIRABLE, &hdev->dev_flags))
                 hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY,
-                                        sizeof(ev->bdaddr), &ev->bdaddr);
+                             sizeof(ev->bdaddr), &ev->bdaddr);
         else if (test_bit(HCI_MGMT, &hdev->dev_flags)) {
                 u8 secure;
 
@@ -2672,7 +2666,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_link_key_req *ev = (void *) skb->data;
         struct hci_cp_link_key_reply cp;
@@ -2689,15 +2683,15 @@ static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff
         key = hci_find_link_key(hdev, &ev->bdaddr);
         if (!key) {
                 BT_DBG("%s link key not found for %s", hdev->name,
-                                                        batostr(&ev->bdaddr));
+                       batostr(&ev->bdaddr));
                 goto not_found;
         }
 
         BT_DBG("%s found key type %u for %s", hdev->name, key->type,
-                                                        batostr(&ev->bdaddr));
+               batostr(&ev->bdaddr));
 
         if (!test_bit(HCI_DEBUG_KEYS, &hdev->dev_flags) &&
-                                key->type == HCI_LK_DEBUG_COMBINATION) {
+            key->type == HCI_LK_DEBUG_COMBINATION) {
                 BT_DBG("%s ignoring debug key", hdev->name);
                 goto not_found;
         }
@@ -2705,16 +2699,15 @@ static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff
         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr);
         if (conn) {
                 if (key->type == HCI_LK_UNAUTH_COMBINATION &&
-                                conn->auth_type != 0xff &&
-                                (conn->auth_type & 0x01)) {
+                    conn->auth_type != 0xff && (conn->auth_type & 0x01)) {
                         BT_DBG("%s ignoring unauthenticated key", hdev->name);
                         goto not_found;
                 }
 
                 if (key->type == HCI_LK_COMBINATION && key->pin_len < 16 &&
-                                conn->pending_sec_level == BT_SECURITY_HIGH) {
-                        BT_DBG("%s ignoring key unauthenticated for high \
-                                                        security", hdev->name);
+                    conn->pending_sec_level == BT_SECURITY_HIGH) {
+                        BT_DBG("%s ignoring key unauthenticated for high security",
+                               hdev->name);
                         goto not_found;
                 }
 
@@ -2723,7 +2716,7 @@ static inline void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff
         }
 
         bacpy(&cp.bdaddr, &ev->bdaddr);
-        memcpy(cp.link_key, key->val, 16);
+        memcpy(cp.link_key, key->val, HCI_LINK_KEY_SIZE);
 
         hci_send_cmd(hdev, HCI_OP_LINK_KEY_REPLY, sizeof(cp), &cp);
 
@@ -2736,7 +2729,7 @@ not_found:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_link_key_notify *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2760,12 +2753,12 @@ static inline void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff
 
         if (test_bit(HCI_LINK_KEYS, &hdev->dev_flags))
                 hci_add_link_key(hdev, conn, 1, &ev->bdaddr, ev->link_key,
-                                                        ev->key_type, pin_len);
+                                 ev->key_type, pin_len);
 
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_clock_offset *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2788,7 +2781,7 @@ static inline void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *sk
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_pkt_type_change *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2804,7 +2797,7 @@ static inline void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_pscan_rep_mode *ev = (void *) skb->data;
         struct inquiry_entry *ie;
@@ -2822,7 +2815,8 @@ static inline void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev,
+                                             struct sk_buff *skb)
 {
         struct inquiry_data data;
         int num_rsp = *((__u8 *) skb->data);
@@ -2881,7 +2875,8 @@ static inline void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, struct
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_ext_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_remote_ext_features_evt(struct hci_dev *hdev,
+                                        struct sk_buff *skb)
 {
         struct hci_ev_remote_ext_features *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2929,7 +2924,8 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_sync_conn_complete_evt(struct hci_dev *hdev,
+                                       struct sk_buff *skb)
 {
         struct hci_ev_sync_conn_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -2984,19 +2980,20 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_sync_conn_changed_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_sync_conn_changed_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         BT_DBG("%s", hdev->name);
 }
 
-static inline void hci_sniff_subrate_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_sniff_subrate_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_sniff_subrate *ev = (void *) skb->data;
 
         BT_DBG("%s status %d", hdev->name, ev->status);
 }
 
-static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_extended_inquiry_result_evt(struct hci_dev *hdev,
+                                            struct sk_buff *skb)
 {
         struct inquiry_data data;
         struct extended_inquiry_info *info = (void *) (skb->data + 1);
@@ -3043,7 +3040,51 @@ static inline void hci_extended_inquiry_result_evt(struct hci_dev *hdev, struct
         hci_dev_unlock(hdev);
 }
 
-static inline u8 hci_get_auth_req(struct hci_conn *conn)
+static void hci_key_refresh_complete_evt(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
+{
+        struct hci_ev_key_refresh_complete *ev = (void *) skb->data;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %u handle %u", hdev->name, ev->status,
+               __le16_to_cpu(ev->handle));
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle));
+        if (!conn)
+                goto unlock;
+
+        if (!ev->status)
+                conn->sec_level = conn->pending_sec_level;
+
+        clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags);
+
+        if (ev->status && conn->state == BT_CONNECTED) {
+                hci_acl_disconn(conn, HCI_ERROR_AUTH_FAILURE);
+                hci_conn_put(conn);
+                goto unlock;
+        }
+
+        if (conn->state == BT_CONFIG) {
+                if (!ev->status)
+                        conn->state = BT_CONNECTED;
+
+                hci_proto_connect_cfm(conn, ev->status);
+                hci_conn_put(conn);
+        } else {
+                hci_auth_cfm(conn, ev->status);
+
+                hci_conn_hold(conn);
+                conn->disc_timeout = HCI_DISCONN_TIMEOUT;
+                hci_conn_put(conn);
+        }
+
+unlock:
+        hci_dev_unlock(hdev);
+}
+
+static u8 hci_get_auth_req(struct hci_conn *conn)
 {
         /* If remote requests dedicated bonding follow that lead */
         if (conn->remote_auth == 0x02 || conn->remote_auth == 0x03) {
@@ -3062,7 +3103,7 @@ static inline u8 hci_get_auth_req(struct hci_conn *conn)
         return conn->auth_type;
 }
 
-static inline void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_io_capa_request *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -3081,7 +3122,7 @@ static inline void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff
                 goto unlock;
 
         if (test_bit(HCI_PAIRABLE, &hdev->dev_flags) ||
-                        (conn->remote_auth & ~0x01) == HCI_AT_NO_BONDING) {
+            (conn->remote_auth & ~0x01) == HCI_AT_NO_BONDING) {
                 struct hci_cp_io_capability_reply cp;
 
                 bacpy(&cp.bdaddr, &ev->bdaddr);
@@ -3092,14 +3133,14 @@ static inline void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff
                 conn->auth_type = hci_get_auth_req(conn);
                 cp.authentication = conn->auth_type;
 
-                if ((conn->out || test_bit(HCI_CONN_REMOTE_OOB, &conn->flags)) &&
-                                hci_find_remote_oob_data(hdev, &conn->dst))
+                if (hci_find_remote_oob_data(hdev, &conn->dst) &&
+                    (conn->out || test_bit(HCI_CONN_REMOTE_OOB, &conn->flags)))
                         cp.oob_data = 0x01;
                 else
                         cp.oob_data = 0x00;
 
                 hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_REPLY,
-                                                        sizeof(cp), &cp);
+                             sizeof(cp), &cp);
         } else {
                 struct hci_cp_io_capability_neg_reply cp;
 
@@ -3107,14 +3148,14 @@ static inline void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff
                 cp.reason = HCI_ERROR_PAIRING_NOT_ALLOWED;
 
                 hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_NEG_REPLY,
-                                                        sizeof(cp), &cp);
+                             sizeof(cp), &cp);
         }
 
 unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_io_capa_reply *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -3136,8 +3177,8 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_user_confirm_request_evt(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+static void hci_user_confirm_request_evt(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
 {
         struct hci_ev_user_confirm_req *ev = (void *) skb->data;
         int loc_mitm, rem_mitm, confirm_hint = 0;
@@ -3165,13 +3206,13 @@ static inline void hci_user_confirm_request_evt(struct hci_dev *hdev,
         if (!conn->connect_cfm_cb && loc_mitm && conn->remote_cap == 0x03) {
                 BT_DBG("Rejecting request: remote device can't provide MITM");
                 hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_NEG_REPLY,
-                                        sizeof(ev->bdaddr), &ev->bdaddr);
+                             sizeof(ev->bdaddr), &ev->bdaddr);
                 goto unlock;
         }
 
         /* If no side requires MITM protection; auto-accept */
         if ((!loc_mitm || conn->remote_cap == 0x03) &&
-                                (!rem_mitm || conn->io_capability == 0x03)) {
+            (!rem_mitm || conn->io_capability == 0x03)) {
 
                 /* If we're not the initiators request authorization to
                  * proceed from user space (mgmt_user_confirm with
@@ -3183,7 +3224,7 @@ static inline void hci_user_confirm_request_evt(struct hci_dev *hdev,
                 }
 
                 BT_DBG("Auto-accept of user confirmation with %ums delay",
-                                                hdev->auto_accept_delay);
+                       hdev->auto_accept_delay);
 
                 if (hdev->auto_accept_delay > 0) {
                         int delay = msecs_to_jiffies(hdev->auto_accept_delay);
@@ -3192,7 +3233,7 @@ static inline void hci_user_confirm_request_evt(struct hci_dev *hdev,
                 }
 
                 hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_REPLY,
-                                                sizeof(ev->bdaddr), &ev->bdaddr);
+                             sizeof(ev->bdaddr), &ev->bdaddr);
                 goto unlock;
         }
 
@@ -3204,8 +3245,8 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_user_passkey_request_evt(struct hci_dev *hdev,
-                                                        struct sk_buff *skb)
+static void hci_user_passkey_request_evt(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
 {
         struct hci_ev_user_passkey_req *ev = (void *) skb->data;
 
@@ -3219,7 +3260,8 @@ static inline void hci_user_passkey_request_evt(struct hci_dev *hdev,
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_simple_pair_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_simple_pair_complete_evt(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
 {
         struct hci_ev_simple_pair_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -3247,7 +3289,8 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_host_features_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_remote_host_features_evt(struct hci_dev *hdev,
+                                         struct sk_buff *skb)
 {
         struct hci_ev_remote_host_features *ev = (void *) skb->data;
         struct inquiry_entry *ie;
@@ -3263,8 +3306,8 @@ static inline void hci_remote_host_features_evt(struct hci_dev *hdev, struct sk_
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_remote_oob_data_request_evt(struct hci_dev *hdev,
-                                                   struct sk_buff *skb)
+static void hci_remote_oob_data_request_evt(struct hci_dev *hdev,
+                                            struct sk_buff *skb)
 {
         struct hci_ev_remote_oob_data_request *ev = (void *) skb->data;
         struct oob_data *data;
@@ -3285,20 +3328,20 @@ static inline void hci_remote_oob_data_request_evt(struct hci_dev *hdev,
                 memcpy(cp.randomizer, data->randomizer, sizeof(cp.randomizer));
 
                 hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_REPLY, sizeof(cp),
-                                                                        &cp);
+                             &cp);
         } else {
                 struct hci_cp_remote_oob_data_neg_reply cp;
 
                 bacpy(&cp.bdaddr, &ev->bdaddr);
                 hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_NEG_REPLY, sizeof(cp),
-                                                                        &cp);
+                             &cp);
         }
 
 unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_le_conn_complete *ev = (void *) skb->data;
         struct hci_conn *conn;
@@ -3307,6 +3350,19 @@ static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff
 
         hci_dev_lock(hdev);
 
+        if (ev->status) {
+                conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT);
+                if (!conn)
+                        goto unlock;
+
+                mgmt_connect_failed(hdev, &conn->dst, conn->type,
+                                    conn->dst_type, ev->status);
+                hci_proto_connect_cfm(conn, ev->status);
+                conn->state = BT_CLOSED;
+                hci_conn_del(conn);
+                goto unlock;
+        }
+
         conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &ev->bdaddr);
         if (!conn) {
                 conn = hci_conn_add(hdev, LE_LINK, &ev->bdaddr);
@@ -3319,15 +3375,6 @@ static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff
                 conn->dst_type = ev->bdaddr_type;
         }
 
-        if (ev->status) {
-                mgmt_connect_failed(hdev, &ev->bdaddr, conn->type,
-                                                conn->dst_type, ev->status);
-                hci_proto_connect_cfm(conn, ev->status);
-                conn->state = BT_CLOSED;
-                hci_conn_del(conn);
-                goto unlock;
-        }
-
         if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags))
                 mgmt_device_connected(hdev, &ev->bdaddr, conn->type,
                                       conn->dst_type, 0, NULL, 0, NULL);
@@ -3345,8 +3392,7 @@ unlock:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_le_adv_report_evt(struct hci_dev *hdev,
-                                                struct sk_buff *skb)
+static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         u8 num_reports = skb->data[0];
         void *ptr = &skb->data[1];
@@ -3367,8 +3413,7 @@ static inline void hci_le_adv_report_evt(struct hci_dev *hdev,
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_le_ltk_request_evt(struct hci_dev *hdev,
-                                                struct sk_buff *skb)
+static void hci_le_ltk_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_le_ltk_req *ev = (void *) skb->data;
         struct hci_cp_le_ltk_reply cp;
@@ -3411,7 +3456,7 @@ not_found:
         hci_dev_unlock(hdev);
 }
 
-static inline void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
+static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_le_meta *le_ev = (void *) skb->data;
 
@@ -3559,6 +3604,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_extended_inquiry_result_evt(hdev, skb);
                 break;
 
+        case HCI_EV_KEY_REFRESH_COMPLETE:
+                hci_key_refresh_complete_evt(hdev, skb);
+                break;
+
         case HCI_EV_IO_CAPA_REQUEST:
                 hci_io_capa_request_evt(hdev, skb);
                 break;
diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index 5914623f426..a7f04de03d7 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -24,25 +24,7 @@
 
 /* Bluetooth HCI sockets. */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/capability.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/skbuff.h>
-#include <linux/workqueue.h>
-#include <linux/interrupt.h>
-#include <linux/compat.h>
-#include <linux/socket.h>
-#include <linux/ioctl.h>
-#include <net/sock.h>
-
-#include <linux/uaccess.h>
+#include <linux/export.h>
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -113,11 +95,12 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
                 flt = &hci_pi(sk)->filter;
 
                 if (!test_bit((bt_cb(skb)->pkt_type == HCI_VENDOR_PKT) ?
-                                0 : (bt_cb(skb)->pkt_type & HCI_FLT_TYPE_BITS), &flt->type_mask))
+                              0 : (bt_cb(skb)->pkt_type & HCI_FLT_TYPE_BITS),
+                              &flt->type_mask))
                         continue;
 
                 if (bt_cb(skb)->pkt_type == HCI_EVENT_PKT) {
-                        register int evt = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS);
+                        int evt = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS);
 
                         if (!hci_test_bit(evt, &flt->event_mask))
                                 continue;
@@ -240,7 +223,8 @@ void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb)
                         struct hci_mon_hdr *hdr;
 
                         /* Create a private copy with headroom */
-                        skb_copy = __pskb_copy(skb, HCI_MON_HDR_SIZE, GFP_ATOMIC);
+                        skb_copy = __pskb_copy(skb, HCI_MON_HDR_SIZE,
+                                               GFP_ATOMIC);
                         if (!skb_copy)
                                 continue;
 
@@ -495,7 +479,8 @@ static int hci_sock_blacklist_del(struct hci_dev *hdev, void __user *arg)
 }
 
 /* Ioctls that require bound socket */
-static inline int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, unsigned long arg)
+static int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd,
+                                unsigned long arg)
 {
         struct hci_dev *hdev = hci_pi(sk)->hdev;
 
@@ -540,7 +525,8 @@ static inline int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, unsign
         }
 }
 
-static int hci_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg)
+static int hci_sock_ioctl(struct socket *sock, unsigned int cmd,
+                          unsigned long arg)
 {
         struct sock *sk = sock->sk;
         void __user *argp = (void __user *) arg;
@@ -601,7 +587,8 @@ static int hci_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long a
         }
 }
 
-static int hci_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len)
+static int hci_sock_bind(struct socket *sock, struct sockaddr *addr,
+                         int addr_len)
 {
         struct sockaddr_hci haddr;
         struct sock *sk = sock->sk;
@@ -690,7 +677,8 @@ done:
         return err;
 }
 
-static int hci_sock_getname(struct socket *sock, struct sockaddr *addr, int *addr_len, int peer)
+static int hci_sock_getname(struct socket *sock, struct sockaddr *addr,
+                            int *addr_len, int peer)
 {
         struct sockaddr_hci *haddr = (struct sockaddr_hci *) addr;
         struct sock *sk = sock->sk;
@@ -711,13 +699,15 @@ static int hci_sock_getname(struct socket *sock, struct sockaddr *addr, int *add
         return 0;
 }
 
-static inline void hci_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_buff *skb)
+static void hci_sock_cmsg(struct sock *sk, struct msghdr *msg,
+                          struct sk_buff *skb)
 {
         __u32 mask = hci_pi(sk)->cmsg_mask;
 
         if (mask & HCI_CMSG_DIR) {
                 int incoming = bt_cb(skb)->incoming;
-                put_cmsg(msg, SOL_HCI, HCI_CMSG_DIR, sizeof(incoming), &incoming);
+                put_cmsg(msg, SOL_HCI, HCI_CMSG_DIR, sizeof(incoming),
+                         &incoming);
         }
 
         if (mask & HCI_CMSG_TSTAMP) {
@@ -747,7 +737,7 @@ static inline void hci_sock_cmsg(struct sock *sk, struct msghdr *msg, struct sk_
 }
 
 static int hci_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
-                                struct msghdr *msg, size_t len, int flags)
+                            struct msghdr *msg, size_t len, int flags)
 {
         int noblock = flags & MSG_DONTWAIT;
         struct sock *sk = sock->sk;
@@ -857,8 +847,9 @@ static int hci_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
                 u16 ocf = hci_opcode_ocf(opcode);
 
                 if (((ogf > HCI_SFLT_MAX_OGF) ||
-                                !hci_test_bit(ocf & HCI_FLT_OCF_BITS, &hci_sec_filter.ocf_mask[ogf])) &&
-                                        !capable(CAP_NET_RAW)) {
+                     !hci_test_bit(ocf & HCI_FLT_OCF_BITS,
+                                   &hci_sec_filter.ocf_mask[ogf])) &&
+                    !capable(CAP_NET_RAW)) {
                         err = -EPERM;
                         goto drop;
                 }
@@ -891,7 +882,8 @@ drop:
         goto done;
 }
 
-static int hci_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int len)
+static int hci_sock_setsockopt(struct socket *sock, int level, int optname,
+                               char __user *optval, unsigned int len)
 {
         struct hci_ufilter uf = { .opcode = 0 };
         struct sock *sk = sock->sk;
@@ -973,7 +965,8 @@ done:
         return err;
 }
 
-static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
+static int hci_sock_getsockopt(struct socket *sock, int level, int optname,
+                               char __user *optval, int __user *optlen)
 {
         struct hci_ufilter uf;
         struct sock *sk = sock->sk;
diff --git a/net/bluetooth/hci_sysfs.c b/net/bluetooth/hci_sysfs.c
index 937f3187eaf..a20e61c3653 100644
--- a/net/bluetooth/hci_sysfs.c
+++ b/net/bluetooth/hci_sysfs.c
@@ -1,10 +1,6 @@
 /* Bluetooth HCI driver model support. */
 
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/init.h>
 #include <linux/debugfs.h>
-#include <linux/seq_file.h>
 #include <linux/module.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -31,27 +27,30 @@ static inline char *link_typetostr(int type)
         }
 }
 
-static ssize_t show_link_type(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_link_type(struct device *dev,
+                              struct device_attribute *attr, char *buf)
 {
         struct hci_conn *conn = to_hci_conn(dev);
         return sprintf(buf, "%s\n", link_typetostr(conn->type));
 }
 
-static ssize_t show_link_address(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_link_address(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
 {
         struct hci_conn *conn = to_hci_conn(dev);
         return sprintf(buf, "%s\n", batostr(&conn->dst));
 }
 
-static ssize_t show_link_features(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_link_features(struct device *dev,
+                                  struct device_attribute *attr, char *buf)
 {
         struct hci_conn *conn = to_hci_conn(dev);
 
         return sprintf(buf, "0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
-                                conn->features[0], conn->features[1],
-                                conn->features[2], conn->features[3],
-                                conn->features[4], conn->features[5],
-                                conn->features[6], conn->features[7]);
+                       conn->features[0], conn->features[1],
+                       conn->features[2], conn->features[3],
+                       conn->features[4], conn->features[5],
+                       conn->features[6], conn->features[7]);
 }
 
 #define LINK_ATTR(_name, _mode, _show, _store) \
@@ -185,19 +184,22 @@ static inline char *host_typetostr(int type)
         }
 }
 
-static ssize_t show_bus(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_bus(struct device *dev,
+                        struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%s\n", host_bustostr(hdev->bus));
 }
 
-static ssize_t show_type(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_type(struct device *dev,
+                         struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%s\n", host_typetostr(hdev->dev_type));
 }
 
-static ssize_t show_name(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_name(struct device *dev,
+                         struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         char name[HCI_MAX_NAME_LENGTH + 1];
@@ -210,55 +212,64 @@ static ssize_t show_name(struct device *dev, struct device_attribute *attr, char
         return sprintf(buf, "%s\n", name);
 }
 
-static ssize_t show_class(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_class(struct device *dev,
+                          struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
-        return sprintf(buf, "0x%.2x%.2x%.2x\n",
-                        hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]);
+        return sprintf(buf, "0x%.2x%.2x%.2x\n", hdev->dev_class[2],
+                       hdev->dev_class[1], hdev->dev_class[0]);
 }
 
-static ssize_t show_address(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_address(struct device *dev,
+                            struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%s\n", batostr(&hdev->bdaddr));
 }
 
-static ssize_t show_features(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_features(struct device *dev,
+                             struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
 
         return sprintf(buf, "0x%02x%02x%02x%02x%02x%02x%02x%02x\n",
-                                hdev->features[0], hdev->features[1],
-                                hdev->features[2], hdev->features[3],
-                                hdev->features[4], hdev->features[5],
-                                hdev->features[6], hdev->features[7]);
+                       hdev->features[0], hdev->features[1],
+                       hdev->features[2], hdev->features[3],
+                       hdev->features[4], hdev->features[5],
+                       hdev->features[6], hdev->features[7]);
 }
 
-static ssize_t show_manufacturer(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_manufacturer(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->manufacturer);
 }
 
-static ssize_t show_hci_version(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_hci_version(struct device *dev,
+                                struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->hci_ver);
 }
 
-static ssize_t show_hci_revision(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_hci_revision(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->hci_rev);
 }
 
-static ssize_t show_idle_timeout(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_idle_timeout(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->idle_timeout);
 }
 
-static ssize_t store_idle_timeout(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+static ssize_t store_idle_timeout(struct device *dev,
+                                  struct device_attribute *attr,
+                                  const char *buf, size_t count)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         unsigned int val;
@@ -276,13 +287,16 @@ static ssize_t store_idle_timeout(struct device *dev, struct device_attribute *a
         return count;
 }
 
-static ssize_t show_sniff_max_interval(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_sniff_max_interval(struct device *dev,
+                                       struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->sniff_max_interval);
 }
 
-static ssize_t store_sniff_max_interval(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+static ssize_t store_sniff_max_interval(struct device *dev,
+                                        struct device_attribute *attr,
+                                        const char *buf, size_t count)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         u16 val;
@@ -300,13 +314,16 @@ static ssize_t store_sniff_max_interval(struct device *dev, struct device_attrib
         return count;
 }
 
-static ssize_t show_sniff_min_interval(struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_sniff_min_interval(struct device *dev,
+                                       struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         return sprintf(buf, "%d\n", hdev->sniff_min_interval);
 }
 
-static ssize_t store_sniff_min_interval(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+static ssize_t store_sniff_min_interval(struct device *dev,
+                                        struct device_attribute *attr,
+                                        const char *buf, size_t count)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
         u16 val;
@@ -335,11 +352,11 @@ static DEVICE_ATTR(hci_version, S_IRUGO, show_hci_version, NULL);
 static DEVICE_ATTR(hci_revision, S_IRUGO, show_hci_revision, NULL);
 
 static DEVICE_ATTR(idle_timeout, S_IRUGO | S_IWUSR,
-                                show_idle_timeout, store_idle_timeout);
+                   show_idle_timeout, store_idle_timeout);
 static DEVICE_ATTR(sniff_max_interval, S_IRUGO | S_IWUSR,
-                                show_sniff_max_interval, store_sniff_max_interval);
+                   show_sniff_max_interval, store_sniff_max_interval);
 static DEVICE_ATTR(sniff_min_interval, S_IRUGO | S_IWUSR,
-                                show_sniff_min_interval, store_sniff_min_interval);
+                   show_sniff_min_interval, store_sniff_min_interval);
 
 static struct attribute *bt_host_attrs[] = {
         &dev_attr_bus.attr,
@@ -455,8 +472,8 @@ static void print_bt_uuid(struct seq_file *f, u8 *uuid)
         memcpy(&data5, &uuid[14], 2);
 
         seq_printf(f, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x\n",
-                                ntohl(data0), ntohs(data1), ntohs(data2),
-                                ntohs(data3), ntohl(data4), ntohs(data5));
+                   ntohl(data0), ntohs(data1), ntohs(data2), ntohs(data3),
+                   ntohl(data4), ntohs(data5));
 }
 
 static int uuids_show(struct seq_file *f, void *p)
@@ -513,7 +530,7 @@ static int auto_accept_delay_get(void *data, u64 *val)
 }
 
 DEFINE_SIMPLE_ATTRIBUTE(auto_accept_delay_fops, auto_accept_delay_get,
-                                        auto_accept_delay_set, "%llu\n");
+                        auto_accept_delay_set, "%llu\n");
 
 void hci_init_sysfs(struct hci_dev *hdev)
 {
@@ -547,15 +564,15 @@ int hci_add_sysfs(struct hci_dev *hdev)
                 return 0;
 
         debugfs_create_file("inquiry_cache", 0444, hdev->debugfs,
-                                                hdev, &inquiry_cache_fops);
+                            hdev, &inquiry_cache_fops);
 
         debugfs_create_file("blacklist", 0444, hdev->debugfs,
-                                                hdev, &blacklist_fops);
+                            hdev, &blacklist_fops);
 
         debugfs_create_file("uuids", 0444, hdev->debugfs, hdev, &uuids_fops);
 
         debugfs_create_file("auto_accept_delay", 0444, hdev->debugfs, hdev,
-                                                &auto_accept_delay_fops);
+                            &auto_accept_delay_fops);
         return 0;
 }
 
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index 2c20d765b39..ccd985da651 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -21,27 +21,8 @@
 */
 
 #include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/freezer.h>
-#include <linux/fcntl.h>
-#include <linux/skbuff.h>
-#include <linux/socket.h>
-#include <linux/ioctl.h>
 #include <linux/file.h>
-#include <linux/init.h>
-#include <linux/wait.h>
-#include <linux/mutex.h>
 #include <linux/kthread.h>
-#include <net/sock.h>
-
-#include <linux/input.h>
-#include <linux/hid.h>
 #include <linux/hidraw.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -244,7 +225,8 @@ static void hidp_input_report(struct hidp_session *session, struct sk_buff *skb)
 }
 
 static int __hidp_send_ctrl_message(struct hidp_session *session,
-                        unsigned char hdr, unsigned char *data, int size)
+                                    unsigned char hdr, unsigned char *data,
+                                    int size)
 {
         struct sk_buff *skb;
 
@@ -268,7 +250,7 @@ static int __hidp_send_ctrl_message(struct hidp_session *session,
         return 0;
 }
 
-static inline int hidp_send_ctrl_message(struct hidp_session *session,
+static int hidp_send_ctrl_message(struct hidp_session *session,
                         unsigned char hdr, unsigned char *data, int size)
 {
         int err;
@@ -471,7 +453,7 @@ static void hidp_set_timer(struct hidp_session *session)
                 mod_timer(&session->timer, jiffies + HZ * session->idle_to);
 }
 
-static inline void hidp_del_timer(struct hidp_session *session)
+static void hidp_del_timer(struct hidp_session *session)
 {
         if (session->idle_to > 0)
                 del_timer(&session->timer);
diff --git a/net/bluetooth/hidp/sock.c b/net/bluetooth/hidp/sock.c
index 73a32d705c1..18b3f6892a3 100644
--- a/net/bluetooth/hidp/sock.c
+++ b/net/bluetooth/hidp/sock.c
@@ -20,22 +20,8 @@
    SOFTWARE IS DISCLAIMED.
 */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/capability.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/skbuff.h>
-#include <linux/socket.h>
-#include <linux/ioctl.h>
+#include <linux/export.h>
 #include <linux/file.h>
-#include <linux/init.h>
-#include <linux/compat.h>
-#include <linux/gfp.h>
-#include <net/sock.h>
 
 #include "hidp.h"
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 24f144b72a9..4ca88247b7c 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -30,32 +30,14 @@
 
 #include <linux/module.h>
 
-#include <linux/types.h>
-#include <linux/capability.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/socket.h>
-#include <linux/skbuff.h>
-#include <linux/list.h>
-#include <linux/device.h>
 #include <linux/debugfs.h>
-#include <linux/seq_file.h>
-#include <linux/uaccess.h>
 #include <linux/crc16.h>
-#include <net/sock.h>
-
-#include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 bool disable_ertm;
 
@@ -73,6 +55,9 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
 static void l2cap_send_disconn_req(struct l2cap_conn *conn,
                                    struct l2cap_chan *chan, int err);
 
+static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
+                    struct sk_buff_head *skbs, u8 event);
+
 /* ---- L2CAP channels ---- */
 
 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
@@ -196,7 +181,7 @@ static void __l2cap_state_change(struct l2cap_chan *chan, int state)
                                                 state_to_string(state));
 
         chan->state = state;
-        chan->ops->state_change(chan->data, state);
+        chan->ops->state_change(chan, state);
 }
 
 static void l2cap_state_change(struct l2cap_chan *chan, int state)
@@ -224,6 +209,37 @@ static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err)
         release_sock(sk);
 }
 
+static void __set_retrans_timer(struct l2cap_chan *chan)
+{
+        if (!delayed_work_pending(&chan->monitor_timer) &&
+            chan->retrans_timeout) {
+                l2cap_set_timer(chan, &chan->retrans_timer,
+                                msecs_to_jiffies(chan->retrans_timeout));
+        }
+}
+
+static void __set_monitor_timer(struct l2cap_chan *chan)
+{
+        __clear_retrans_timer(chan);
+        if (chan->monitor_timeout) {
+                l2cap_set_timer(chan, &chan->monitor_timer,
+                                msecs_to_jiffies(chan->monitor_timeout));
+        }
+}
+
+static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head,
+                                               u16 seq)
+{
+        struct sk_buff *skb;
+
+        skb_queue_walk(head, skb) {
+                if (bt_cb(skb)->control.txseq == seq)
+                        return skb;
+        }
+
+        return NULL;
+}
+
 /* ---- L2CAP sequence number lists ---- */
 
 /* For ERTM, ordered lists of sequence numbers must be tracked for
@@ -366,7 +382,7 @@ static void l2cap_chan_timeout(struct work_struct *work)
 
         l2cap_chan_unlock(chan);
 
-        chan->ops->close(chan->data);
+        chan->ops->close(chan);
         mutex_unlock(&conn->chan_lock);
 
         l2cap_chan_put(chan);
@@ -392,6 +408,9 @@ struct l2cap_chan *l2cap_chan_create(void)
 
         atomic_set(&chan->refcnt, 1);
 
+        /* This flag is cleared in l2cap_chan_ready() */
+        set_bit(CONF_NOT_COMPLETE, &chan->conf_state);
+
         BT_DBG("chan %p", chan);
 
         return chan;
@@ -430,7 +449,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
         case L2CAP_CHAN_CONN_ORIENTED:
                 if (conn->hcon->type == LE_LINK) {
                         /* LE connection */
-                        chan->omtu = L2CAP_LE_DEFAULT_MTU;
+                        chan->omtu = L2CAP_DEFAULT_MTU;
                         chan->scid = L2CAP_CID_LE_DATA;
                         chan->dcid = L2CAP_CID_LE_DATA;
                 } else {
@@ -447,6 +466,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
                 chan->omtu = L2CAP_DEFAULT_MTU;
                 break;
 
+        case L2CAP_CHAN_CONN_FIX_A2MP:
+                chan->scid = L2CAP_CID_A2MP;
+                chan->dcid = L2CAP_CID_A2MP;
+                chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+                chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+                break;
+
         default:
                 /* Raw socket can send/recv signalling messages only */
                 chan->scid = L2CAP_CID_SIGNALING;
@@ -466,18 +492,16 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
         list_add(&chan->list, &conn->chan_l);
 }
 
-static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
         mutex_lock(&conn->chan_lock);
         __l2cap_chan_add(conn, chan);
         mutex_unlock(&conn->chan_lock);
 }
 
-static void l2cap_chan_del(struct l2cap_chan *chan, int err)
+void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
-        struct sock *sk = chan->sk;
         struct l2cap_conn *conn = chan->conn;
-        struct sock *parent = bt_sk(sk)->parent;
 
         __clear_chan_timer(chan);
 
@@ -490,34 +514,22 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
                 l2cap_chan_put(chan);
 
                 chan->conn = NULL;
-                hci_conn_put(conn->hcon);
-        }
-
-        lock_sock(sk);
-
-        __l2cap_state_change(chan, BT_CLOSED);
-        sock_set_flag(sk, SOCK_ZAPPED);
 
-        if (err)
-                __l2cap_chan_set_err(chan, err);
+                if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
+                        hci_conn_put(conn->hcon);
+        }
 
-        if (parent) {
-                bt_accept_unlink(sk);
-                parent->sk_data_ready(parent, 0);
-        } else
-                sk->sk_state_change(sk);
+        if (chan->ops->teardown)
+                chan->ops->teardown(chan, err);
 
-        release_sock(sk);
-
-        if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
-                        test_bit(CONF_INPUT_DONE, &chan->conf_state)))
+        if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
                 return;
 
-        skb_queue_purge(&chan->tx_q);
-
-        if (chan->mode == L2CAP_MODE_ERTM) {
-                struct srej_list *l, *tmp;
+        switch(chan->mode) {
+        case L2CAP_MODE_BASIC:
+                break;
 
+        case L2CAP_MODE_ERTM:
                 __clear_retrans_timer(chan);
                 __clear_monitor_timer(chan);
                 __clear_ack_timer(chan);
@@ -526,30 +538,15 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
                 l2cap_seq_list_free(&chan->srej_list);
                 l2cap_seq_list_free(&chan->retrans_list);
-                list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
-                        list_del(&l->list);
-                        kfree(l);
-                }
-        }
-}
-
-static void l2cap_chan_cleanup_listen(struct sock *parent)
-{
-        struct sock *sk;
-
-        BT_DBG("parent %p", parent);
 
-        /* Close not yet accepted channels */
-        while ((sk = bt_accept_dequeue(parent, NULL))) {
-                struct l2cap_chan *chan = l2cap_pi(sk)->chan;
-
-                l2cap_chan_lock(chan);
-                __clear_chan_timer(chan);
-                l2cap_chan_close(chan, ECONNRESET);
-                l2cap_chan_unlock(chan);
+                /* fall through */
 
-                chan->ops->close(chan->data);
+        case L2CAP_MODE_STREAMING:
+                skb_queue_purge(&chan->tx_q);
+                break;
         }
+
+        return;
 }
 
 void l2cap_chan_close(struct l2cap_chan *chan, int reason)
@@ -562,12 +559,8 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 
         switch (chan->state) {
         case BT_LISTEN:
-                lock_sock(sk);
-                l2cap_chan_cleanup_listen(sk);
-
-                __l2cap_state_change(chan, BT_CLOSED);
-                sock_set_flag(sk, SOCK_ZAPPED);
-                release_sock(sk);
+                if (chan->ops->teardown)
+                        chan->ops->teardown(chan, 0);
                 break;
 
         case BT_CONNECTED:
@@ -595,7 +588,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                         rsp.scid   = cpu_to_le16(chan->dcid);
                         rsp.dcid   = cpu_to_le16(chan->scid);
                         rsp.result = cpu_to_le16(result);
-                        rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
+                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
                                                         sizeof(rsp), &rsp);
                 }
@@ -609,9 +602,8 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                 break;
 
         default:
-                lock_sock(sk);
-                sock_set_flag(sk, SOCK_ZAPPED);
-                release_sock(sk);
+                if (chan->ops->teardown)
+                        chan->ops->teardown(chan, 0);
                 break;
         }
 }
@@ -627,7 +619,7 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
                 default:
                         return HCI_AT_NO_BONDING;
                 }
-        } else if (chan->psm == cpu_to_le16(0x0001)) {
+        } else if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
                 if (chan->sec_level == BT_SECURITY_LOW)
                         chan->sec_level = BT_SECURITY_SDP;
 
@@ -773,9 +765,11 @@ static inline void __unpack_control(struct l2cap_chan *chan,
         if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
                 __unpack_extended_control(get_unaligned_le32(skb->data),
                                           &bt_cb(skb)->control);
+                skb_pull(skb, L2CAP_EXT_CTRL_SIZE);
         } else {
                 __unpack_enhanced_control(get_unaligned_le16(skb->data),
                                           &bt_cb(skb)->control);
+                skb_pull(skb, L2CAP_ENH_CTRL_SIZE);
         }
 }
 
@@ -830,66 +824,102 @@ static inline void __pack_control(struct l2cap_chan *chan,
         }
 }
 
-static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
+static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan)
 {
-        struct sk_buff *skb;
-        struct l2cap_hdr *lh;
-        struct l2cap_conn *conn = chan->conn;
-        int count, hlen;
-
-        if (chan->state != BT_CONNECTED)
-                return;
-
         if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                hlen = L2CAP_EXT_HDR_SIZE;
+                return L2CAP_EXT_HDR_SIZE;
         else
-                hlen = L2CAP_ENH_HDR_SIZE;
+                return L2CAP_ENH_HDR_SIZE;
+}
+
+static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan,
+                                               u32 control)
+{
+        struct sk_buff *skb;
+        struct l2cap_hdr *lh;
+        int hlen = __ertm_hdr_size(chan);
 
         if (chan->fcs == L2CAP_FCS_CRC16)
                 hlen += L2CAP_FCS_SIZE;
 
-        BT_DBG("chan %p, control 0x%8.8x", chan, control);
-
-        count = min_t(unsigned int, conn->mtu, hlen);
-
-        control |= __set_sframe(chan);
+        skb = bt_skb_alloc(hlen, GFP_KERNEL);
 
-        if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
-                control |= __set_ctrl_final(chan);
-
-        if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
-                control |= __set_ctrl_poll(chan);
-
-        skb = bt_skb_alloc(count, GFP_ATOMIC);
         if (!skb)
-                return;
+                return ERR_PTR(-ENOMEM);
 
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
         lh->cid = cpu_to_le16(chan->dcid);
 
-        __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
+        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+                put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
+        else
+                put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
 
         if (chan->fcs == L2CAP_FCS_CRC16) {
-                u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
+                u16 fcs = crc16(0, (u8 *)skb->data, skb->len);
                 put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
         }
 
         skb->priority = HCI_PRIO_MAX;
-        l2cap_do_send(chan, skb);
+        return skb;
 }
 
-static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
+static void l2cap_send_sframe(struct l2cap_chan *chan,
+                              struct l2cap_ctrl *control)
 {
-        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-                control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
+        struct sk_buff *skb;
+        u32 control_field;
+
+        BT_DBG("chan %p, control %p", chan, control);
+
+        if (!control->sframe)
+                return;
+
+        if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) &&
+            !control->poll)
+                control->final = 1;
+
+        if (control->super == L2CAP_SUPER_RR)
+                clear_bit(CONN_RNR_SENT, &chan->conn_state);
+        else if (control->super == L2CAP_SUPER_RNR)
                 set_bit(CONN_RNR_SENT, &chan->conn_state);
-        } else
-                control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
 
-        control |= __set_reqseq(chan, chan->buffer_seq);
+        if (control->super != L2CAP_SUPER_SREJ) {
+                chan->last_acked_seq = control->reqseq;
+                __clear_ack_timer(chan);
+        }
+
+        BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq,
+               control->final, control->poll, control->super);
+
+        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+                control_field = __pack_extended_control(control);
+        else
+                control_field = __pack_enhanced_control(control);
+
+        skb = l2cap_create_sframe_pdu(chan, control_field);
+        if (!IS_ERR(skb))
+                l2cap_do_send(chan, skb);
+}
+
+static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll)
+{
+        struct l2cap_ctrl control;
+
+        BT_DBG("chan %p, poll %d", chan, poll);
+
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
+        control.poll = poll;
 
-        l2cap_send_sframe(chan, control);
+        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
+                control.super = L2CAP_SUPER_RNR;
+        else
+                control.super = L2CAP_SUPER_RR;
+
+        control.reqseq = chan->buffer_seq;
+        l2cap_send_sframe(chan, &control);
 }
 
 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
@@ -914,25 +944,13 @@ static void l2cap_send_conn_req(struct l2cap_chan *chan)
 
 static void l2cap_chan_ready(struct l2cap_chan *chan)
 {
-        struct sock *sk = chan->sk;
-        struct sock *parent;
-
-        lock_sock(sk);
-
-        parent = bt_sk(sk)->parent;
-
-        BT_DBG("sk %p, parent %p", sk, parent);
-
+        /* This clears all conf flags, including CONF_NOT_COMPLETE */
         chan->conf_state = 0;
         __clear_chan_timer(chan);
 
-        __l2cap_state_change(chan, BT_CONNECTED);
-        sk->sk_state_change(sk);
-
-        if (parent)
-                parent->sk_data_ready(parent, 0);
+        chan->state = BT_CONNECTED;
 
-        release_sock(sk);
+        chan->ops->ready(chan);
 }
 
 static void l2cap_do_start(struct l2cap_chan *chan)
@@ -953,7 +971,7 @@ static void l2cap_do_start(struct l2cap_chan *chan)
                         l2cap_send_conn_req(chan);
         } else {
                 struct l2cap_info_req req;
-                req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
+                req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
 
                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
                 conn->info_ident = l2cap_get_ident(conn);
@@ -995,6 +1013,11 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
                 __clear_ack_timer(chan);
         }
 
+        if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+                __l2cap_state_change(chan, BT_DISCONN);
+                return;
+        }
+
         req.dcid = cpu_to_le16(chan->dcid);
         req.scid = cpu_to_le16(chan->scid);
         l2cap_send_cmd(conn, l2cap_get_ident(conn),
@@ -1053,20 +1076,20 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                                 if (test_bit(BT_SK_DEFER_SETUP,
                                              &bt_sk(sk)->flags)) {
                                         struct sock *parent = bt_sk(sk)->parent;
-                                        rsp.result = cpu_to_le16(L2CAP_CR_PEND);
-                                        rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
+                                        rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
+                                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
                                         if (parent)
                                                 parent->sk_data_ready(parent, 0);
 
                                 } else {
                                         __l2cap_state_change(chan, BT_CONFIG);
-                                        rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
-                                        rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
+                                        rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
+                                        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
                                 }
                                 release_sock(sk);
                         } else {
-                                rsp.result = cpu_to_le16(L2CAP_CR_PEND);
-                                rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
+                                rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
+                                rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHEN_PEND);
                         }
 
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
@@ -1150,13 +1173,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
         lock_sock(parent);
 
-        /* Check for backlog size */
-        if (sk_acceptq_is_full(parent)) {
-                BT_DBG("backlog full %d", parent->sk_ack_backlog);
-                goto clean;
-        }
-
-        chan = pchan->ops->new_connection(pchan->data);
+        chan = pchan->ops->new_connection(pchan);
         if (!chan)
                 goto clean;
 
@@ -1171,10 +1188,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
         l2cap_chan_add(conn, chan);
 
-        __set_chan_timer(chan, sk->sk_sndtimeo);
-
-        __l2cap_state_change(chan, BT_CONNECTED);
-        parent->sk_data_ready(parent, 0);
+        l2cap_chan_ready(chan);
 
 clean:
         release_sock(parent);
@@ -1198,6 +1212,11 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
 
                 l2cap_chan_lock(chan);
 
+                if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+                        l2cap_chan_unlock(chan);
+                        continue;
+                }
+
                 if (conn->hcon->type == LE_LINK) {
                         if (smp_conn_security(conn, chan->sec_level))
                                 l2cap_chan_ready(chan);
@@ -1270,7 +1289,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 
                 l2cap_chan_unlock(chan);
 
-                chan->ops->close(chan->data);
+                chan->ops->close(chan);
                 l2cap_chan_put(chan);
         }
 
@@ -1295,7 +1314,12 @@ static void security_timeout(struct work_struct *work)
         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
                                                 security_timer.work);
 
-        l2cap_conn_del(conn->hcon, ETIMEDOUT);
+        BT_DBG("conn %p", conn);
+
+        if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) {
+                smp_chan_destroy(conn);
+                l2cap_conn_del(conn->hcon, ETIMEDOUT);
+        }
 }
 
 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
@@ -1439,21 +1463,17 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
                 goto done;
         }
 
-        lock_sock(sk);
-
-        switch (sk->sk_state) {
+        switch (chan->state) {
         case BT_CONNECT:
         case BT_CONNECT2:
         case BT_CONFIG:
                 /* Already connecting */
                 err = 0;
-                release_sock(sk);
                 goto done;
 
         case BT_CONNECTED:
                 /* Already connected */
                 err = -EISCONN;
-                release_sock(sk);
                 goto done;
 
         case BT_OPEN:
@@ -1463,13 +1483,12 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
 
         default:
                 err = -EBADFD;
-                release_sock(sk);
                 goto done;
         }
 
         /* Set destination address and psm */
+        lock_sock(sk);
         bacpy(&bt_sk(sk)->dst, dst);
-
         release_sock(sk);
 
         chan->psm = psm;
@@ -1571,23 +1590,20 @@ int __l2cap_wait_ack(struct sock *sk)
 static void l2cap_monitor_timeout(struct work_struct *work)
 {
         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
-                                                        monitor_timer.work);
+                                               monitor_timer.work);
 
         BT_DBG("chan %p", chan);
 
         l2cap_chan_lock(chan);
 
-        if (chan->retry_count >= chan->remote_max_tx) {
-                l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
+        if (!chan->conn) {
                 l2cap_chan_unlock(chan);
                 l2cap_chan_put(chan);
                 return;
         }
 
-        chan->retry_count++;
-        __set_monitor_timer(chan);
+        l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO);
 
-        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
         l2cap_chan_unlock(chan);
         l2cap_chan_put(chan);
 }
@@ -1595,234 +1611,293 @@ static void l2cap_monitor_timeout(struct work_struct *work)
 static void l2cap_retrans_timeout(struct work_struct *work)
 {
         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
-                                                        retrans_timer.work);
+                                               retrans_timer.work);
 
         BT_DBG("chan %p", chan);
 
         l2cap_chan_lock(chan);
 
-        chan->retry_count = 1;
-        __set_monitor_timer(chan);
-
-        set_bit(CONN_WAIT_F, &chan->conn_state);
-
-        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
+        if (!chan->conn) {
+                l2cap_chan_unlock(chan);
+                l2cap_chan_put(chan);
+                return;
+        }
 
+        l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO);
         l2cap_chan_unlock(chan);
         l2cap_chan_put(chan);
 }
 
-static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
+static void l2cap_streaming_send(struct l2cap_chan *chan,
+                                 struct sk_buff_head *skbs)
 {
         struct sk_buff *skb;
+        struct l2cap_ctrl *control;
 
-        while ((skb = skb_peek(&chan->tx_q)) &&
-                        chan->unacked_frames) {
-                if (bt_cb(skb)->control.txseq == chan->expected_ack_seq)
-                        break;
+        BT_DBG("chan %p, skbs %p", chan, skbs);
 
-                skb = skb_dequeue(&chan->tx_q);
-                kfree_skb(skb);
+        skb_queue_splice_tail_init(skbs, &chan->tx_q);
 
-                chan->unacked_frames--;
-        }
+        while (!skb_queue_empty(&chan->tx_q)) {
 
-        if (!chan->unacked_frames)
-                __clear_retrans_timer(chan);
-}
+                skb = skb_dequeue(&chan->tx_q);
 
-static void l2cap_streaming_send(struct l2cap_chan *chan)
-{
-        struct sk_buff *skb;
-        u32 control;
-        u16 fcs;
+                bt_cb(skb)->control.retries = 1;
+                control = &bt_cb(skb)->control;
+
+                control->reqseq = 0;
+                control->txseq = chan->next_tx_seq;
 
-        while ((skb = skb_dequeue(&chan->tx_q))) {
-                control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
-                control |= __set_txseq(chan, chan->next_tx_seq);
-                control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
-                __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
+                __pack_control(chan, control, skb);
 
                 if (chan->fcs == L2CAP_FCS_CRC16) {
-                        fcs = crc16(0, (u8 *)skb->data,
-                                                skb->len - L2CAP_FCS_SIZE);
-                        put_unaligned_le16(fcs,
-                                        skb->data + skb->len - L2CAP_FCS_SIZE);
+                        u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
+                        put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
                 }
 
                 l2cap_do_send(chan, skb);
 
+                BT_DBG("Sent txseq %d", (int)control->txseq);
+
                 chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
+                chan->frames_sent++;
         }
 }
 
-static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
+static int l2cap_ertm_send(struct l2cap_chan *chan)
 {
         struct sk_buff *skb, *tx_skb;
-        u16 fcs;
-        u32 control;
+        struct l2cap_ctrl *control;
+        int sent = 0;
 
-        skb = skb_peek(&chan->tx_q);
-        if (!skb)
-                return;
+        BT_DBG("chan %p", chan);
 
-        while (bt_cb(skb)->control.txseq != tx_seq) {
-                if (skb_queue_is_last(&chan->tx_q, skb))
-                        return;
+        if (chan->state != BT_CONNECTED)
+                return -ENOTCONN;
 
-                skb = skb_queue_next(&chan->tx_q, skb);
-        }
+        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
+                return 0;
 
-        if (bt_cb(skb)->control.retries == chan->remote_max_tx &&
-            chan->remote_max_tx) {
-                l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
-                return;
-        }
+        while (chan->tx_send_head &&
+               chan->unacked_frames < chan->remote_tx_win &&
+               chan->tx_state == L2CAP_TX_STATE_XMIT) {
 
-        tx_skb = skb_clone(skb, GFP_ATOMIC);
-        bt_cb(skb)->control.retries++;
+                skb = chan->tx_send_head;
 
-        control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
-        control &= __get_sar_mask(chan);
+                bt_cb(skb)->control.retries = 1;
+                control = &bt_cb(skb)->control;
 
-        if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
-                control |= __set_ctrl_final(chan);
+                if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
+                        control->final = 1;
 
-        control |= __set_reqseq(chan, chan->buffer_seq);
-        control |= __set_txseq(chan, tx_seq);
+                control->reqseq = chan->buffer_seq;
+                chan->last_acked_seq = chan->buffer_seq;
+                control->txseq = chan->next_tx_seq;
 
-        __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
+                __pack_control(chan, control, skb);
 
-        if (chan->fcs == L2CAP_FCS_CRC16) {
-                fcs = crc16(0, (u8 *)tx_skb->data,
-                                                tx_skb->len - L2CAP_FCS_SIZE);
-                put_unaligned_le16(fcs,
-                                tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
+                if (chan->fcs == L2CAP_FCS_CRC16) {
+                        u16 fcs = crc16(0, (u8 *) skb->data, skb->len);
+                        put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
+                }
+
+                /* Clone after data has been modified. Data is assumed to be
+                   read-only (for locking purposes) on cloned sk_buffs.
+                 */
+                tx_skb = skb_clone(skb, GFP_KERNEL);
+
+                if (!tx_skb)
+                        break;
+
+                __set_retrans_timer(chan);
+
+                chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
+                chan->unacked_frames++;
+                chan->frames_sent++;
+                sent++;
+
+                if (skb_queue_is_last(&chan->tx_q, skb))
+                        chan->tx_send_head = NULL;
+                else
+                        chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
+
+                l2cap_do_send(chan, tx_skb);
+                BT_DBG("Sent txseq %d", (int)control->txseq);
         }
 
-        l2cap_do_send(chan, tx_skb);
+        BT_DBG("Sent %d, %d unacked, %d in ERTM queue", sent,
+               (int) chan->unacked_frames, skb_queue_len(&chan->tx_q));
+
+        return sent;
 }
 
-static int l2cap_ertm_send(struct l2cap_chan *chan)
+static void l2cap_ertm_resend(struct l2cap_chan *chan)
 {
-        struct sk_buff *skb, *tx_skb;
-        u16 fcs;
-        u32 control;
-        int nsent = 0;
+        struct l2cap_ctrl control;
+        struct sk_buff *skb;
+        struct sk_buff *tx_skb;
+        u16 seq;
 
-        if (chan->state != BT_CONNECTED)
-                return -ENOTCONN;
+        BT_DBG("chan %p", chan);
 
         if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
-                return 0;
+                return;
 
-        while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
+        while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) {
+                seq = l2cap_seq_list_pop(&chan->retrans_list);
 
-                if (bt_cb(skb)->control.retries == chan->remote_max_tx &&
-                    chan->remote_max_tx) {
-                        l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
-                        break;
+                skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
+                if (!skb) {
+                        BT_DBG("Error: Can't retransmit seq %d, frame missing",
+                                seq);
+                        continue;
                 }
 
-                tx_skb = skb_clone(skb, GFP_ATOMIC);
-
                 bt_cb(skb)->control.retries++;
+                control = bt_cb(skb)->control;
 
-                control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
-                control &= __get_sar_mask(chan);
+                if (chan->max_tx != 0 &&
+                    bt_cb(skb)->control.retries > chan->max_tx) {
+                        BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
+                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                        l2cap_seq_list_clear(&chan->retrans_list);
+                        break;
+                }
 
+                control.reqseq = chan->buffer_seq;
                 if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
-                        control |= __set_ctrl_final(chan);
+                        control.final = 1;
+                else
+                        control.final = 0;
 
-                control |= __set_reqseq(chan, chan->buffer_seq);
-                control |= __set_txseq(chan, chan->next_tx_seq);
-                control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
+                if (skb_cloned(skb)) {
+                        /* Cloned sk_buffs are read-only, so we need a
+                         * writeable copy
+                         */
+                        tx_skb = skb_copy(skb, GFP_ATOMIC);
+                } else {
+                        tx_skb = skb_clone(skb, GFP_ATOMIC);
+                }
 
-                __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
+                if (!tx_skb) {
+                        l2cap_seq_list_clear(&chan->retrans_list);
+                        break;
+                }
+
+                /* Update skb contents */
+                if (test_bit(FLAG_EXT_CTRL, &chan->flags)) {
+                        put_unaligned_le32(__pack_extended_control(&control),
+                                           tx_skb->data + L2CAP_HDR_SIZE);
+                } else {
+                        put_unaligned_le16(__pack_enhanced_control(&control),
+                                           tx_skb->data + L2CAP_HDR_SIZE);
+                }
 
                 if (chan->fcs == L2CAP_FCS_CRC16) {
-                        fcs = crc16(0, (u8 *)skb->data,
-                                                tx_skb->len - L2CAP_FCS_SIZE);
-                        put_unaligned_le16(fcs, skb->data +
-                                                tx_skb->len - L2CAP_FCS_SIZE);
+                        u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len);
+                        put_unaligned_le16(fcs, skb_put(tx_skb,
+                                                        L2CAP_FCS_SIZE));
                 }
 
                 l2cap_do_send(chan, tx_skb);
 
-                __set_retrans_timer(chan);
-
-                bt_cb(skb)->control.txseq = chan->next_tx_seq;
-
-                chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
-
-                if (bt_cb(skb)->control.retries == 1) {
-                        chan->unacked_frames++;
-
-                        if (!nsent++)
-                                __clear_ack_timer(chan);
-                }
-
-                chan->frames_sent++;
+                BT_DBG("Resent txseq %d", control.txseq);
 
-                if (skb_queue_is_last(&chan->tx_q, skb))
-                        chan->tx_send_head = NULL;
-                else
-                        chan->tx_send_head = skb_queue_next(&chan->tx_q, skb);
+                chan->last_acked_seq = chan->buffer_seq;
         }
-
-        return nsent;
 }
 
-static int l2cap_retransmit_frames(struct l2cap_chan *chan)
+static void l2cap_retransmit(struct l2cap_chan *chan,
+                             struct l2cap_ctrl *control)
 {
-        int ret;
-
-        if (!skb_queue_empty(&chan->tx_q))
-                chan->tx_send_head = chan->tx_q.next;
+        BT_DBG("chan %p, control %p", chan, control);
 
-        chan->next_tx_seq = chan->expected_ack_seq;
-        ret = l2cap_ertm_send(chan);
-        return ret;
+        l2cap_seq_list_append(&chan->retrans_list, control->reqseq);
+        l2cap_ertm_resend(chan);
 }
 
-static void __l2cap_send_ack(struct l2cap_chan *chan)
+static void l2cap_retransmit_all(struct l2cap_chan *chan,
+                                 struct l2cap_ctrl *control)
 {
-        u32 control = 0;
+        struct sk_buff *skb;
 
-        control |= __set_reqseq(chan, chan->buffer_seq);
+        BT_DBG("chan %p, control %p", chan, control);
 
-        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-                control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
-                set_bit(CONN_RNR_SENT, &chan->conn_state);
-                l2cap_send_sframe(chan, control);
-                return;
-        }
+        if (control->poll)
+                set_bit(CONN_SEND_FBIT, &chan->conn_state);
 
-        if (l2cap_ertm_send(chan) > 0)
+        l2cap_seq_list_clear(&chan->retrans_list);
+
+        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
                 return;
 
-        control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
-        l2cap_send_sframe(chan, control);
+        if (chan->unacked_frames) {
+                skb_queue_walk(&chan->tx_q, skb) {
+                        if (bt_cb(skb)->control.txseq == control->reqseq ||
+                                skb == chan->tx_send_head)
+                                break;
+                }
+
+                skb_queue_walk_from(&chan->tx_q, skb) {
+                        if (skb == chan->tx_send_head)
+                                break;
+
+                        l2cap_seq_list_append(&chan->retrans_list,
+                                              bt_cb(skb)->control.txseq);
+                }
+
+                l2cap_ertm_resend(chan);
+        }
 }
 
 static void l2cap_send_ack(struct l2cap_chan *chan)
 {
-        __clear_ack_timer(chan);
-        __l2cap_send_ack(chan);
-}
+        struct l2cap_ctrl control;
+        u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq,
+                                         chan->last_acked_seq);
+        int threshold;
 
-static void l2cap_send_srejtail(struct l2cap_chan *chan)
-{
-        struct srej_list *tail;
-        u32 control;
+        BT_DBG("chan %p last_acked_seq %d buffer_seq %d",
+               chan, chan->last_acked_seq, chan->buffer_seq);
 
-        control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
-        control |= __set_ctrl_final(chan);
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
 
-        tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
-        control |= __set_reqseq(chan, tail->tx_seq);
+        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
+            chan->rx_state == L2CAP_RX_STATE_RECV) {
+                __clear_ack_timer(chan);
+                control.super = L2CAP_SUPER_RNR;
+                control.reqseq = chan->buffer_seq;
+                l2cap_send_sframe(chan, &control);
+        } else {
+                if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) {
+                        l2cap_ertm_send(chan);
+                        /* If any i-frames were sent, they included an ack */
+                        if (chan->buffer_seq == chan->last_acked_seq)
+                                frames_to_ack = 0;
+                }
 
-        l2cap_send_sframe(chan, control);
+                /* Ack now if the tx window is 3/4ths full.
+                 * Calculate without mul or div
+                 */
+                threshold = chan->tx_win;
+                threshold += threshold << 1;
+                threshold >>= 2;
+
+                BT_DBG("frames_to_ack %d, threshold %d", (int)frames_to_ack,
+                       threshold);
+
+                if (frames_to_ack >= threshold) {
+                        __clear_ack_timer(chan);
+                        control.super = L2CAP_SUPER_RR;
+                        control.reqseq = chan->buffer_seq;
+                        l2cap_send_sframe(chan, &control);
+                        frames_to_ack = 0;
+                }
+
+                if (frames_to_ack)
+                        __set_ack_timer(chan);
+        }
 }
 
 static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan,
@@ -1951,10 +2026,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
         if (!conn)
                 return ERR_PTR(-ENOTCONN);
 
-        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
-                hlen = L2CAP_EXT_HDR_SIZE;
-        else
-                hlen = L2CAP_ENH_HDR_SIZE;
+        hlen = __ertm_hdr_size(chan);
 
         if (sdulen)
                 hlen += L2CAP_SDULEN_SIZE;
@@ -1974,7 +2046,11 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
         lh->cid = cpu_to_le16(chan->dcid);
         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
 
-        __put_control(chan, 0, skb_put(skb, __ctrl_size(chan)));
+        /* Control header is populated later */
+        if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+                put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE));
+        else
+                put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE));
 
         if (sdulen)
                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
@@ -1985,9 +2061,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
                 return ERR_PTR(err);
         }
 
-        if (chan->fcs == L2CAP_FCS_CRC16)
-                put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
-
+        bt_cb(skb)->control.fcs = chan->fcs;
         bt_cb(skb)->control.retries = 0;
         return skb;
 }
@@ -1999,7 +2073,6 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan,
         struct sk_buff *skb;
         u16 sdu_len;
         size_t pdu_len;
-        int err = 0;
         u8 sar;
 
         BT_DBG("chan %p, msg %p, len %d", chan, msg, (int)len);
@@ -2015,7 +2088,10 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan,
         pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
 
         /* Adjust for largest possible L2CAP overhead. */
-        pdu_len -= L2CAP_EXT_HDR_SIZE + L2CAP_FCS_SIZE;
+        if (chan->fcs)
+                pdu_len -= L2CAP_FCS_SIZE;
+
+        pdu_len -= __ertm_hdr_size(chan);
 
         /* Remote device may have requested smaller PDUs */
         pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
@@ -2055,7 +2131,7 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan,
                 }
         }
 
-        return err;
+        return 0;
 }
 
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
@@ -2117,17 +2193,12 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
                 if (err)
                         break;
 
-                if (chan->mode == L2CAP_MODE_ERTM && chan->tx_send_head == NULL)
-                        chan->tx_send_head = seg_queue.next;
-                skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
-
                 if (chan->mode == L2CAP_MODE_ERTM)
-                        err = l2cap_ertm_send(chan);
+                        l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST);
                 else
-                        l2cap_streaming_send(chan);
+                        l2cap_streaming_send(chan, &seg_queue);
 
-                if (err >= 0)
-                        err = len;
+                err = len;
 
                 /* If the skbs were not queued for sending, they'll still be in
                  * seg_queue and need to be purged.
@@ -2143,6 +2214,296 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
         return err;
 }
 
+static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq)
+{
+        struct l2cap_ctrl control;
+        u16 seq;
+
+        BT_DBG("chan %p, txseq %d", chan, txseq);
+
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
+        control.super = L2CAP_SUPER_SREJ;
+
+        for (seq = chan->expected_tx_seq; seq != txseq;
+             seq = __next_seq(chan, seq)) {
+                if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) {
+                        control.reqseq = seq;
+                        l2cap_send_sframe(chan, &control);
+                        l2cap_seq_list_append(&chan->srej_list, seq);
+                }
+        }
+
+        chan->expected_tx_seq = __next_seq(chan, txseq);
+}
+
+static void l2cap_send_srej_tail(struct l2cap_chan *chan)
+{
+        struct l2cap_ctrl control;
+
+        BT_DBG("chan %p", chan);
+
+        if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR)
+                return;
+
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
+        control.super = L2CAP_SUPER_SREJ;
+        control.reqseq = chan->srej_list.tail;
+        l2cap_send_sframe(chan, &control);
+}
+
+static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq)
+{
+        struct l2cap_ctrl control;
+        u16 initial_head;
+        u16 seq;
+
+        BT_DBG("chan %p, txseq %d", chan, txseq);
+
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
+        control.super = L2CAP_SUPER_SREJ;
+
+        /* Capture initial list head to allow only one pass through the list. */
+        initial_head = chan->srej_list.head;
+
+        do {
+                seq = l2cap_seq_list_pop(&chan->srej_list);
+                if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR)
+                        break;
+
+                control.reqseq = seq;
+                l2cap_send_sframe(chan, &control);
+                l2cap_seq_list_append(&chan->srej_list, seq);
+        } while (chan->srej_list.head != initial_head);
+}
+
+static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq)
+{
+        struct sk_buff *acked_skb;
+        u16 ackseq;
+
+        BT_DBG("chan %p, reqseq %d", chan, reqseq);
+
+        if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq)
+                return;
+
+        BT_DBG("expected_ack_seq %d, unacked_frames %d",
+               chan->expected_ack_seq, chan->unacked_frames);
+
+        for (ackseq = chan->expected_ack_seq; ackseq != reqseq;
+             ackseq = __next_seq(chan, ackseq)) {
+
+                acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq);
+                if (acked_skb) {
+                        skb_unlink(acked_skb, &chan->tx_q);
+                        kfree_skb(acked_skb);
+                        chan->unacked_frames--;
+                }
+        }
+
+        chan->expected_ack_seq = reqseq;
+
+        if (chan->unacked_frames == 0)
+                __clear_retrans_timer(chan);
+
+        BT_DBG("unacked_frames %d", (int) chan->unacked_frames);
+}
+
+static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan)
+{
+        BT_DBG("chan %p", chan);
+
+        chan->expected_tx_seq = chan->buffer_seq;
+        l2cap_seq_list_clear(&chan->srej_list);
+        skb_queue_purge(&chan->srej_q);
+        chan->rx_state = L2CAP_RX_STATE_RECV;
+}
+
+static void l2cap_tx_state_xmit(struct l2cap_chan *chan,
+                                struct l2cap_ctrl *control,
+                                struct sk_buff_head *skbs, u8 event)
+{
+        BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
+               event);
+
+        switch (event) {
+        case L2CAP_EV_DATA_REQUEST:
+                if (chan->tx_send_head == NULL)
+                        chan->tx_send_head = skb_peek(skbs);
+
+                skb_queue_splice_tail_init(skbs, &chan->tx_q);
+                l2cap_ertm_send(chan);
+                break;
+        case L2CAP_EV_LOCAL_BUSY_DETECTED:
+                BT_DBG("Enter LOCAL_BUSY");
+                set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+
+                if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
+                        /* The SREJ_SENT state must be aborted if we are to
+                         * enter the LOCAL_BUSY state.
+                         */
+                        l2cap_abort_rx_srej_sent(chan);
+                }
+
+                l2cap_send_ack(chan);
+
+                break;
+        case L2CAP_EV_LOCAL_BUSY_CLEAR:
+                BT_DBG("Exit LOCAL_BUSY");
+                clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+
+                if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
+                        struct l2cap_ctrl local_control;
+
+                        memset(&local_control, 0, sizeof(local_control));
+                        local_control.sframe = 1;
+                        local_control.super = L2CAP_SUPER_RR;
+                        local_control.poll = 1;
+                        local_control.reqseq = chan->buffer_seq;
+                        l2cap_send_sframe(chan, &local_control);
+
+                        chan->retry_count = 1;
+                        __set_monitor_timer(chan);
+                        chan->tx_state = L2CAP_TX_STATE_WAIT_F;
+                }
+                break;
+        case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
+                l2cap_process_reqseq(chan, control->reqseq);
+                break;
+        case L2CAP_EV_EXPLICIT_POLL:
+                l2cap_send_rr_or_rnr(chan, 1);
+                chan->retry_count = 1;
+                __set_monitor_timer(chan);
+                __clear_ack_timer(chan);
+                chan->tx_state = L2CAP_TX_STATE_WAIT_F;
+                break;
+        case L2CAP_EV_RETRANS_TO:
+                l2cap_send_rr_or_rnr(chan, 1);
+                chan->retry_count = 1;
+                __set_monitor_timer(chan);
+                chan->tx_state = L2CAP_TX_STATE_WAIT_F;
+                break;
+        case L2CAP_EV_RECV_FBIT:
+                /* Nothing to process */
+                break;
+        default:
+                break;
+        }
+}
+
+static void l2cap_tx_state_wait_f(struct l2cap_chan *chan,
+                                  struct l2cap_ctrl *control,
+                                  struct sk_buff_head *skbs, u8 event)
+{
+        BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs,
+               event);
+
+        switch (event) {
+        case L2CAP_EV_DATA_REQUEST:
+                if (chan->tx_send_head == NULL)
+                        chan->tx_send_head = skb_peek(skbs);
+                /* Queue data, but don't send. */
+                skb_queue_splice_tail_init(skbs, &chan->tx_q);
+                break;
+        case L2CAP_EV_LOCAL_BUSY_DETECTED:
+                BT_DBG("Enter LOCAL_BUSY");
+                set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+
+                if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
+                        /* The SREJ_SENT state must be aborted if we are to
+                         * enter the LOCAL_BUSY state.
+                         */
+                        l2cap_abort_rx_srej_sent(chan);
+                }
+
+                l2cap_send_ack(chan);
+
+                break;
+        case L2CAP_EV_LOCAL_BUSY_CLEAR:
+                BT_DBG("Exit LOCAL_BUSY");
+                clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+
+                if (test_bit(CONN_RNR_SENT, &chan->conn_state)) {
+                        struct l2cap_ctrl local_control;
+                        memset(&local_control, 0, sizeof(local_control));
+                        local_control.sframe = 1;
+                        local_control.super = L2CAP_SUPER_RR;
+                        local_control.poll = 1;
+                        local_control.reqseq = chan->buffer_seq;
+                        l2cap_send_sframe(chan, &local_control);
+
+                        chan->retry_count = 1;
+                        __set_monitor_timer(chan);
+                        chan->tx_state = L2CAP_TX_STATE_WAIT_F;
+                }
+                break;
+        case L2CAP_EV_RECV_REQSEQ_AND_FBIT:
+                l2cap_process_reqseq(chan, control->reqseq);
+
+                /* Fall through */
+
+        case L2CAP_EV_RECV_FBIT:
+                if (control && control->final) {
+                        __clear_monitor_timer(chan);
+                        if (chan->unacked_frames > 0)
+                                __set_retrans_timer(chan);
+                        chan->retry_count = 0;
+                        chan->tx_state = L2CAP_TX_STATE_XMIT;
+                        BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state);
+                }
+                break;
+        case L2CAP_EV_EXPLICIT_POLL:
+                /* Ignore */
+                break;
+        case L2CAP_EV_MONITOR_TO:
+                if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) {
+                        l2cap_send_rr_or_rnr(chan, 1);
+                        __set_monitor_timer(chan);
+                        chan->retry_count++;
+                } else {
+                        l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
+                }
+                break;
+        default:
+                break;
+        }
+}
+
+static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
+                     struct sk_buff_head *skbs, u8 event)
+{
+        BT_DBG("chan %p, control %p, skbs %p, event %d, state %d",
+               chan, control, skbs, event, chan->tx_state);
+
+        switch (chan->tx_state) {
+        case L2CAP_TX_STATE_XMIT:
+                l2cap_tx_state_xmit(chan, control, skbs, event);
+                break;
+        case L2CAP_TX_STATE_WAIT_F:
+                l2cap_tx_state_wait_f(chan, control, skbs, event);
+                break;
+        default:
+                /* Ignore event */
+                break;
+        }
+}
+
+static void l2cap_pass_to_tx(struct l2cap_chan *chan,
+                             struct l2cap_ctrl *control)
+{
+        BT_DBG("chan %p, control %p", chan, control);
+        l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT);
+}
+
+static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan,
+                                  struct l2cap_ctrl *control)
+{
+        BT_DBG("chan %p, control %p", chan, control);
+        l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT);
+}
+
 /* Copy frame to all raw sockets on that connection */
 static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
 {
@@ -2165,7 +2526,7 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
                 if (!nskb)
                         continue;
 
-                if (chan->ops->recv(chan->data, nskb))
+                if (chan->ops->recv(chan, nskb))
                         kfree_skb(nskb);
         }
 
@@ -2195,9 +2556,9 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
         lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen);
 
         if (conn->hcon->type == LE_LINK)
-                lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING);
+                lh->cid = __constant_cpu_to_le16(L2CAP_CID_LE_SIGNALING);
         else
-                lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING);
+                lh->cid = __constant_cpu_to_le16(L2CAP_CID_SIGNALING);
 
         cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE);
         cmd->code  = code;
@@ -2309,8 +2670,8 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
                 efs.stype       = chan->local_stype;
                 efs.msdu        = cpu_to_le16(chan->local_msdu);
                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
-                efs.acc_lat     = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
-                efs.flush_to    = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
+                efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
+                efs.flush_to    = __constant_cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
                 break;
 
         case L2CAP_MODE_STREAMING:
@@ -2333,20 +2694,24 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
 static void l2cap_ack_timeout(struct work_struct *work)
 {
         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
-                                                        ack_timer.work);
+                                               ack_timer.work);
+        u16 frames_to_ack;
 
         BT_DBG("chan %p", chan);
 
         l2cap_chan_lock(chan);
 
-        __l2cap_send_ack(chan);
+        frames_to_ack = __seq_offset(chan, chan->buffer_seq,
+                                     chan->last_acked_seq);
 
-        l2cap_chan_unlock(chan);
+        if (frames_to_ack)
+                l2cap_send_rr_or_rnr(chan, 0);
 
+        l2cap_chan_unlock(chan);
         l2cap_chan_put(chan);
 }
 
-static inline int l2cap_ertm_init(struct l2cap_chan *chan)
+int l2cap_ertm_init(struct l2cap_chan *chan)
 {
         int err;
 
@@ -2355,7 +2720,6 @@ static inline int l2cap_ertm_init(struct l2cap_chan *chan)
         chan->expected_ack_seq = 0;
         chan->unacked_frames = 0;
         chan->buffer_seq = 0;
-        chan->num_acked = 0;
         chan->frames_sent = 0;
         chan->last_acked_seq = 0;
         chan->sdu = NULL;
@@ -2376,12 +2740,15 @@ static inline int l2cap_ertm_init(struct l2cap_chan *chan)
 
         skb_queue_head_init(&chan->srej_q);
 
-        INIT_LIST_HEAD(&chan->srej_l);
         err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win);
         if (err < 0)
                 return err;
 
-        return l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
+        err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win);
+        if (err < 0)
+                l2cap_seq_list_free(&chan->srej_list);
+
+        return err;
 }
 
 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
@@ -2507,6 +2874,7 @@ done:
                 break;
 
         case L2CAP_MODE_STREAMING:
+                l2cap_txwin_setup(chan);
                 rfc.mode            = L2CAP_MODE_STREAMING;
                 rfc.txwin_size      = 0;
                 rfc.max_transmit    = 0;
@@ -2537,7 +2905,7 @@ done:
         }
 
         req->dcid  = cpu_to_le16(chan->dcid);
-        req->flags = cpu_to_le16(0);
+        req->flags = __constant_cpu_to_le16(0);
 
         return ptr - data;
 }
@@ -2757,7 +3125,7 @@ done:
         }
         rsp->scid   = cpu_to_le16(chan->dcid);
         rsp->result = cpu_to_le16(result);
-        rsp->flags  = cpu_to_le16(0x0000);
+        rsp->flags  = __constant_cpu_to_le16(0);
 
         return ptr - data;
 }
@@ -2856,7 +3224,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
         }
 
         req->dcid   = cpu_to_le16(chan->dcid);
-        req->flags  = cpu_to_le16(0x0000);
+        req->flags  = __constant_cpu_to_le16(0);
 
         return ptr - data;
 }
@@ -2883,8 +3251,8 @@ void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
 
         rsp.scid   = cpu_to_le16(chan->dcid);
         rsp.dcid   = cpu_to_le16(chan->scid);
-        rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
-        rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
+        rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
+        rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
         l2cap_send_cmd(conn, chan->ident,
                                 L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
@@ -2922,8 +3290,8 @@ static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
          * did not send an RFC option.
          */
         rfc.mode = chan->mode;
-        rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
-        rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
+        rfc.retrans_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO);
+        rfc.monitor_timeout = __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO);
         rfc.max_pdu_size = cpu_to_le16(chan->imtu);
 
         BT_ERR("Expected RFC option was not found, using defaults");
@@ -2986,7 +3354,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
         lock_sock(parent);
 
         /* Check if the ACL is secure enough (if not SDP) */
-        if (psm != cpu_to_le16(0x0001) &&
+        if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
                                 !hci_conn_check_link_mode(conn->hcon)) {
                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
                 result = L2CAP_CR_SEC_BLOCK;
@@ -2995,25 +3363,16 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
         result = L2CAP_CR_NO_MEM;
 
-        /* Check for backlog size */
-        if (sk_acceptq_is_full(parent)) {
-                BT_DBG("backlog full %d", parent->sk_ack_backlog);
+        /* Check if we already have channel with that dcid */
+        if (__l2cap_get_chan_by_dcid(conn, scid))
                 goto response;
-        }
 
-        chan = pchan->ops->new_connection(pchan->data);
+        chan = pchan->ops->new_connection(pchan);
         if (!chan)
                 goto response;
 
         sk = chan->sk;
 
-        /* Check if we already have channel with that dcid */
-        if (__l2cap_get_chan_by_dcid(conn, scid)) {
-                sock_set_flag(sk, SOCK_ZAPPED);
-                chan->ops->close(chan->data);
-                goto response;
-        }
-
         hci_conn_hold(conn->hcon);
 
         bacpy(&bt_sk(sk)->src, conn->src);
@@ -3067,7 +3426,7 @@ sendresp:
 
         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
                 struct l2cap_info_req info;
-                info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
+                info.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
 
                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
                 conn->info_ident = l2cap_get_ident(conn);
@@ -3189,7 +3548,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
                 struct l2cap_cmd_rej_cid rej;
 
-                rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
+                rej.reason = __constant_cpu_to_le16(L2CAP_REJ_INVALID_CID);
                 rej.scid = cpu_to_le16(chan->scid);
                 rej.dcid = cpu_to_le16(chan->dcid);
 
@@ -3211,11 +3570,11 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         memcpy(chan->conf_req + chan->conf_len, req->data, len);
         chan->conf_len += len;
 
-        if (flags & 0x0001) {
+        if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
                 /* Incomplete config. Send empty response. */
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
                                 l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_SUCCESS, 0x0001), rsp);
+                                        L2CAP_CONF_SUCCESS, flags), rsp);
                 goto unlock;
         }
 
@@ -3238,8 +3597,6 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
                 set_default_fcs(chan);
 
-                l2cap_state_change(chan, BT_CONNECTED);
-
                 if (chan->mode == L2CAP_MODE_ERTM ||
                     chan->mode == L2CAP_MODE_STREAMING)
                         err = l2cap_ertm_init(chan);
@@ -3271,7 +3628,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
                                         l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_SUCCESS, 0x0000), rsp);
+                                        L2CAP_CONF_SUCCESS, flags), rsp);
         }
 
 unlock:
@@ -3362,7 +3719,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 goto done;
         }
 
-        if (flags & 0x01)
+        if (flags & L2CAP_CONF_FLAG_CONTINUATION)
                 goto done;
 
         set_bit(CONF_INPUT_DONE, &chan->conf_state);
@@ -3370,7 +3727,6 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
                 set_default_fcs(chan);
 
-                l2cap_state_change(chan, BT_CONNECTED);
                 if (chan->mode == L2CAP_MODE_ERTM ||
                     chan->mode == L2CAP_MODE_STREAMING)
                         err = l2cap_ertm_init(chan);
@@ -3424,7 +3780,7 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 
         l2cap_chan_unlock(chan);
 
-        chan->ops->close(chan->data);
+        chan->ops->close(chan);
         l2cap_chan_put(chan);
 
         mutex_unlock(&conn->chan_lock);
@@ -3458,7 +3814,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
 
         l2cap_chan_unlock(chan);
 
-        chan->ops->close(chan->data);
+        chan->ops->close(chan);
         l2cap_chan_put(chan);
 
         mutex_unlock(&conn->chan_lock);
@@ -3479,8 +3835,8 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 u8 buf[8];
                 u32 feat_mask = l2cap_feat_mask;
                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
-                rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
-                rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
+                rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
+                rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
                 if (!disable_ertm)
                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
                                                          | L2CAP_FEAT_FCS;
@@ -3500,15 +3856,15 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 else
                         l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
 
-                rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
-                rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
+                rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
+                rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
                 l2cap_send_cmd(conn, cmd->ident,
                                         L2CAP_INFO_RSP, sizeof(buf), buf);
         } else {
                 struct l2cap_info_rsp rsp;
                 rsp.type   = cpu_to_le16(type);
-                rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP);
+                rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
                 l2cap_send_cmd(conn, cmd->ident,
                                         L2CAP_INFO_RSP, sizeof(rsp), &rsp);
         }
@@ -3548,7 +3904,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 
                 if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) {
                         struct l2cap_info_req req;
-                        req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
+                        req.type = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
 
                         conn->info_ident = l2cap_get_ident(conn);
 
@@ -3783,9 +4139,9 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
 
         err = l2cap_check_conn_param(min, max, latency, to_multiplier);
         if (err)
-                rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
+                rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_REJECTED);
         else
-                rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
+                rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
 
         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
                                                         sizeof(rsp), &rsp);
@@ -3933,7 +4289,7 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
                         BT_ERR("Wrong link type (%d)", err);
 
                         /* FIXME: Map err to a valid reason */
-                        rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
+                        rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
                         l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
                 }
 
@@ -3965,65 +4321,38 @@ static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
         return 0;
 }
 
-static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
+static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
 {
-        u32 control = 0;
+        struct l2cap_ctrl control;
 
-        chan->frames_sent = 0;
+        BT_DBG("chan %p", chan);
 
-        control |= __set_reqseq(chan, chan->buffer_seq);
+        memset(&control, 0, sizeof(control));
+        control.sframe = 1;
+        control.final = 1;
+        control.reqseq = chan->buffer_seq;
+        set_bit(CONN_SEND_FBIT, &chan->conn_state);
 
         if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-                control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
-                l2cap_send_sframe(chan, control);
-                set_bit(CONN_RNR_SENT, &chan->conn_state);
+                control.super = L2CAP_SUPER_RNR;
+                l2cap_send_sframe(chan, &control);
         }
 
-        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
-                l2cap_retransmit_frames(chan);
+        if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
+            chan->unacked_frames > 0)
+                __set_retrans_timer(chan);
 
+        /* Send pending iframes */
         l2cap_ertm_send(chan);
 
         if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
-                        chan->frames_sent == 0) {
-                control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
-                l2cap_send_sframe(chan, control);
-        }
-}
-
-static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
-{
-        struct sk_buff *next_skb;
-        int tx_seq_offset, next_tx_seq_offset;
-
-        bt_cb(skb)->control.txseq = tx_seq;
-        bt_cb(skb)->control.sar = sar;
-
-        next_skb = skb_peek(&chan->srej_q);
-
-        tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
-
-        while (next_skb) {
-                if (bt_cb(next_skb)->control.txseq == tx_seq)
-                        return -EINVAL;
-
-                next_tx_seq_offset = __seq_offset(chan,
-                        bt_cb(next_skb)->control.txseq, chan->buffer_seq);
-
-                if (next_tx_seq_offset > tx_seq_offset) {
-                        __skb_queue_before(&chan->srej_q, next_skb, skb);
-                        return 0;
-                }
-
-                if (skb_queue_is_last(&chan->srej_q, next_skb))
-                        next_skb = NULL;
-                else
-                        next_skb = skb_queue_next(&chan->srej_q, next_skb);
+            test_bit(CONN_SEND_FBIT, &chan->conn_state)) {
+                /* F-bit wasn't sent in an s-frame or i-frame yet, so
+                 * send it now.
+                 */
+                control.super = L2CAP_SUPER_RR;
+                l2cap_send_sframe(chan, &control);
         }
-
-        __skb_queue_tail(&chan->srej_q, skb);
-
-        return 0;
 }
 
 static void append_skb_frag(struct sk_buff *skb,
@@ -4045,16 +4374,17 @@ static void append_skb_frag(struct sk_buff *skb,
         skb->truesize += new_frag->truesize;
 }
 
-static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
+static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb,
+                                struct l2cap_ctrl *control)
 {
         int err = -EINVAL;
 
-        switch (__get_ctrl_sar(chan, control)) {
+        switch (control->sar) {
         case L2CAP_SAR_UNSEGMENTED:
                 if (chan->sdu)
                         break;
 
-                err = chan->ops->recv(chan->data, skb);
+                err = chan->ops->recv(chan, skb);
                 break;
 
         case L2CAP_SAR_START:
@@ -4104,7 +4434,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u3
                 if (chan->sdu->len != chan->sdu_len)
                         break;
 
-                err = chan->ops->recv(chan->data, chan->sdu);
+                err = chan->ops->recv(chan, chan->sdu);
 
                 if (!err) {
                         /* Reassembly complete */
@@ -4126,448 +4456,609 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u3
         return err;
 }
 
-static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
+void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
 {
-        BT_DBG("chan %p, Enter local busy", chan);
+        u8 event;
 
-        set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
-        l2cap_seq_list_clear(&chan->srej_list);
+        if (chan->mode != L2CAP_MODE_ERTM)
+                return;
 
-        __set_ack_timer(chan);
+        event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR;
+        l2cap_tx(chan, NULL, NULL, event);
 }
 
-static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
+static int l2cap_rx_queued_iframes(struct l2cap_chan *chan)
 {
-        u32 control;
-
-        if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
-                goto done;
+        int err = 0;
+        /* Pass sequential frames to l2cap_reassemble_sdu()
+         * until a gap is encountered.
+         */
 
-        control = __set_reqseq(chan, chan->buffer_seq);
-        control |= __set_ctrl_poll(chan);
-        control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
-        l2cap_send_sframe(chan, control);
-        chan->retry_count = 1;
+        BT_DBG("chan %p", chan);
 
-        __clear_retrans_timer(chan);
-        __set_monitor_timer(chan);
+        while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+                struct sk_buff *skb;
+                BT_DBG("Searching for skb with txseq %d (queue len %d)",
+                       chan->buffer_seq, skb_queue_len(&chan->srej_q));
 
-        set_bit(CONN_WAIT_F, &chan->conn_state);
+                skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq);
 
-done:
-        clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
-        clear_bit(CONN_RNR_SENT, &chan->conn_state);
+                if (!skb)
+                        break;
 
-        BT_DBG("chan %p, Exit local busy", chan);
-}
+                skb_unlink(skb, &chan->srej_q);
+                chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
+                err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control);
+                if (err)
+                        break;
+        }
 
-void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
-{
-        if (chan->mode == L2CAP_MODE_ERTM) {
-                if (busy)
-                        l2cap_ertm_enter_local_busy(chan);
-                else
-                        l2cap_ertm_exit_local_busy(chan);
+        if (skb_queue_empty(&chan->srej_q)) {
+                chan->rx_state = L2CAP_RX_STATE_RECV;
+                l2cap_send_ack(chan);
         }
+
+        return err;
 }
 
-static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
+static void l2cap_handle_srej(struct l2cap_chan *chan,
+                              struct l2cap_ctrl *control)
 {
         struct sk_buff *skb;
-        u32 control;
 
-        while ((skb = skb_peek(&chan->srej_q)) &&
-                        !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-                int err;
+        BT_DBG("chan %p, control %p", chan, control);
 
-                if (bt_cb(skb)->control.txseq != tx_seq)
-                        break;
+        if (control->reqseq == chan->next_tx_seq) {
+                BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
+                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                return;
+        }
 
-                skb = skb_dequeue(&chan->srej_q);
-                control = __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
-                err = l2cap_reassemble_sdu(chan, skb, control);
+        skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
 
-                if (err < 0) {
-                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
-                        break;
-                }
+        if (skb == NULL) {
+                BT_DBG("Seq %d not available for retransmission",
+                       control->reqseq);
+                return;
+        }
 
-                chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
-                tx_seq = __next_seq(chan, tx_seq);
+        if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) {
+                BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
+                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                return;
         }
-}
 
-static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
-{
-        struct srej_list *l, *tmp;
-        u32 control;
+        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-        list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
-                if (l->tx_seq == tx_seq) {
-                        list_del(&l->list);
-                        kfree(l);
-                        return;
+        if (control->poll) {
+                l2cap_pass_to_tx(chan, control);
+
+                set_bit(CONN_SEND_FBIT, &chan->conn_state);
+                l2cap_retransmit(chan, control);
+                l2cap_ertm_send(chan);
+
+                if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
+                        set_bit(CONN_SREJ_ACT, &chan->conn_state);
+                        chan->srej_save_reqseq = control->reqseq;
+                }
+        } else {
+                l2cap_pass_to_tx_fbit(chan, control);
+
+                if (control->final) {
+                        if (chan->srej_save_reqseq != control->reqseq ||
+                            !test_and_clear_bit(CONN_SREJ_ACT,
+                                                &chan->conn_state))
+                                l2cap_retransmit(chan, control);
+                } else {
+                        l2cap_retransmit(chan, control);
+                        if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) {
+                                set_bit(CONN_SREJ_ACT, &chan->conn_state);
+                                chan->srej_save_reqseq = control->reqseq;
+                        }
                 }
-                control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
-                control |= __set_reqseq(chan, l->tx_seq);
-                l2cap_send_sframe(chan, control);
-                list_del(&l->list);
-                list_add_tail(&l->list, &chan->srej_l);
         }
 }
 
-static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
+static void l2cap_handle_rej(struct l2cap_chan *chan,
+                             struct l2cap_ctrl *control)
 {
-        struct srej_list *new;
-        u32 control;
-
-        while (tx_seq != chan->expected_tx_seq) {
-                control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
-                control |= __set_reqseq(chan, chan->expected_tx_seq);
-                l2cap_seq_list_append(&chan->srej_list, chan->expected_tx_seq);
-                l2cap_send_sframe(chan, control);
+        struct sk_buff *skb;
 
-                new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
-                if (!new)
-                        return -ENOMEM;
+        BT_DBG("chan %p, control %p", chan, control);
 
-                new->tx_seq = chan->expected_tx_seq;
+        if (control->reqseq == chan->next_tx_seq) {
+                BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq);
+                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                return;
+        }
 
-                chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
+        skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq);
 
-                list_add_tail(&new->list, &chan->srej_l);
+        if (chan->max_tx && skb &&
+            bt_cb(skb)->control.retries >= chan->max_tx) {
+                BT_DBG("Retry limit exceeded (%d)", chan->max_tx);
+                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                return;
         }
 
-        chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
+        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-        return 0;
+        l2cap_pass_to_tx(chan, control);
+
+        if (control->final) {
+                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
+                        l2cap_retransmit_all(chan, control);
+        } else {
+                l2cap_retransmit_all(chan, control);
+                l2cap_ertm_send(chan);
+                if (chan->tx_state == L2CAP_TX_STATE_WAIT_F)
+                        set_bit(CONN_REJ_ACT, &chan->conn_state);
+        }
 }
 
-static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
+static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
 {
-        u16 tx_seq = __get_txseq(chan, rx_control);
-        u16 req_seq = __get_reqseq(chan, rx_control);
-        u8 sar = __get_ctrl_sar(chan, rx_control);
-        int tx_seq_offset, expected_tx_seq_offset;
-        int num_to_ack = (chan->tx_win/6) + 1;
-        int err = 0;
+        BT_DBG("chan %p, txseq %d", chan, txseq);
 
-        BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
-                                                        tx_seq, rx_control);
+        BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq,
+               chan->expected_tx_seq);
 
-        if (__is_ctrl_final(chan, rx_control) &&
-                        test_bit(CONN_WAIT_F, &chan->conn_state)) {
-                __clear_monitor_timer(chan);
-                if (chan->unacked_frames > 0)
-                        __set_retrans_timer(chan);
-                clear_bit(CONN_WAIT_F, &chan->conn_state);
-        }
+        if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
+                if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
+                                                                chan->tx_win) {
+                        /* See notes below regarding "double poll" and
+                         * invalid packets.
+                         */
+                        if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
+                                BT_DBG("Invalid/Ignore - after SREJ");
+                                return L2CAP_TXSEQ_INVALID_IGNORE;
+                        } else {
+                                BT_DBG("Invalid - in window after SREJ sent");
+                                return L2CAP_TXSEQ_INVALID;
+                        }
+                }
 
-        chan->expected_ack_seq = req_seq;
-        l2cap_drop_acked_frames(chan);
+                if (chan->srej_list.head == txseq) {
+                        BT_DBG("Expected SREJ");
+                        return L2CAP_TXSEQ_EXPECTED_SREJ;
+                }
 
-        tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
+                if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) {
+                        BT_DBG("Duplicate SREJ - txseq already stored");
+                        return L2CAP_TXSEQ_DUPLICATE_SREJ;
+                }
 
-        /* invalid tx_seq */
-        if (tx_seq_offset >= chan->tx_win) {
-                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
-                goto drop;
+                if (l2cap_seq_list_contains(&chan->srej_list, txseq)) {
+                        BT_DBG("Unexpected SREJ - not requested");
+                        return L2CAP_TXSEQ_UNEXPECTED_SREJ;
+                }
         }
 
-        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-                if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
-                        l2cap_send_ack(chan);
-                goto drop;
+        if (chan->expected_tx_seq == txseq) {
+                if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
+                    chan->tx_win) {
+                        BT_DBG("Invalid - txseq outside tx window");
+                        return L2CAP_TXSEQ_INVALID;
+                } else {
+                        BT_DBG("Expected");
+                        return L2CAP_TXSEQ_EXPECTED;
+                }
         }
 
-        if (tx_seq == chan->expected_tx_seq)
-                goto expected;
+        if (__seq_offset(chan, txseq, chan->last_acked_seq) <
+                __seq_offset(chan, chan->expected_tx_seq,
+                             chan->last_acked_seq)){
+                BT_DBG("Duplicate - expected_tx_seq later than txseq");
+                return L2CAP_TXSEQ_DUPLICATE;
+        }
+
+        if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) {
+                /* A source of invalid packets is a "double poll" condition,
+                 * where delays cause us to send multiple poll packets.  If
+                 * the remote stack receives and processes both polls,
+                 * sequence numbers can wrap around in such a way that a
+                 * resent frame has a sequence number that looks like new data
+                 * with a sequence gap.  This would trigger an erroneous SREJ
+                 * request.
+                 *
+                 * Fortunately, this is impossible with a tx window that's
+                 * less than half of the maximum sequence number, which allows
+                 * invalid frames to be safely ignored.
+                 *
+                 * With tx window sizes greater than half of the tx window
+                 * maximum, the frame is invalid and cannot be ignored.  This
+                 * causes a disconnect.
+                 */
+
+                if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) {
+                        BT_DBG("Invalid/Ignore - txseq outside tx window");
+                        return L2CAP_TXSEQ_INVALID_IGNORE;
+                } else {
+                        BT_DBG("Invalid - txseq outside tx window");
+                        return L2CAP_TXSEQ_INVALID;
+                }
+        } else {
+                BT_DBG("Unexpected - txseq indicates missing frames");
+                return L2CAP_TXSEQ_UNEXPECTED;
+        }
+}
 
-        if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
-                struct srej_list *first;
+static int l2cap_rx_state_recv(struct l2cap_chan *chan,
+                               struct l2cap_ctrl *control,
+                               struct sk_buff *skb, u8 event)
+{
+        int err = 0;
+        bool skb_in_use = 0;
 
-                first = list_first_entry(&chan->srej_l,
-                                struct srej_list, list);
-                if (tx_seq == first->tx_seq) {
-                        l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
-                        l2cap_check_srej_gap(chan, tx_seq);
+        BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
+               event);
 
-                        list_del(&first->list);
-                        kfree(first);
+        switch (event) {
+        case L2CAP_EV_RECV_IFRAME:
+                switch (l2cap_classify_txseq(chan, control->txseq)) {
+                case L2CAP_TXSEQ_EXPECTED:
+                        l2cap_pass_to_tx(chan, control);
 
-                        if (list_empty(&chan->srej_l)) {
-                                chan->buffer_seq = chan->buffer_seq_srej;
-                                clear_bit(CONN_SREJ_SENT, &chan->conn_state);
-                                l2cap_send_ack(chan);
-                                BT_DBG("chan %p, Exit SREJ_SENT", chan);
+                        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+                                BT_DBG("Busy, discarding expected seq %d",
+                                       control->txseq);
+                                break;
                         }
-                } else {
-                        struct srej_list *l;
 
-                        /* duplicated tx_seq */
-                        if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0)
-                                goto drop;
+                        chan->expected_tx_seq = __next_seq(chan,
+                                                           control->txseq);
+
+                        chan->buffer_seq = chan->expected_tx_seq;
+                        skb_in_use = 1;
 
-                        list_for_each_entry(l, &chan->srej_l, list) {
-                                if (l->tx_seq == tx_seq) {
-                                        l2cap_resend_srejframe(chan, tx_seq);
-                                        return 0;
+                        err = l2cap_reassemble_sdu(chan, skb, control);
+                        if (err)
+                                break;
+
+                        if (control->final) {
+                                if (!test_and_clear_bit(CONN_REJ_ACT,
+                                                        &chan->conn_state)) {
+                                        control->final = 0;
+                                        l2cap_retransmit_all(chan, control);
+                                        l2cap_ertm_send(chan);
                                 }
                         }
 
-                        err = l2cap_send_srejframe(chan, tx_seq);
-                        if (err < 0) {
-                                l2cap_send_disconn_req(chan->conn, chan, -err);
-                                return err;
+                        if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
+                                l2cap_send_ack(chan);
+                        break;
+                case L2CAP_TXSEQ_UNEXPECTED:
+                        l2cap_pass_to_tx(chan, control);
+
+                        /* Can't issue SREJ frames in the local busy state.
+                         * Drop this frame, it will be seen as missing
+                         * when local busy is exited.
+                         */
+                        if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+                                BT_DBG("Busy, discarding unexpected seq %d",
+                                       control->txseq);
+                                break;
                         }
-                }
-        } else {
-                expected_tx_seq_offset = __seq_offset(chan,
-                                chan->expected_tx_seq, chan->buffer_seq);
 
-                /* duplicated tx_seq */
-                if (tx_seq_offset < expected_tx_seq_offset)
-                        goto drop;
-
-                set_bit(CONN_SREJ_SENT, &chan->conn_state);
+                        /* There was a gap in the sequence, so an SREJ
+                         * must be sent for each missing frame.  The
+                         * current frame is stored for later use.
+                         */
+                        skb_queue_tail(&chan->srej_q, skb);
+                        skb_in_use = 1;
+                        BT_DBG("Queued %p (queue len %d)", skb,
+                               skb_queue_len(&chan->srej_q));
 
-                BT_DBG("chan %p, Enter SREJ", chan);
+                        clear_bit(CONN_SREJ_ACT, &chan->conn_state);
+                        l2cap_seq_list_clear(&chan->srej_list);
+                        l2cap_send_srej(chan, control->txseq);
 
-                INIT_LIST_HEAD(&chan->srej_l);
-                chan->buffer_seq_srej = chan->buffer_seq;
+                        chan->rx_state = L2CAP_RX_STATE_SREJ_SENT;
+                        break;
+                case L2CAP_TXSEQ_DUPLICATE:
+                        l2cap_pass_to_tx(chan, control);
+                        break;
+                case L2CAP_TXSEQ_INVALID_IGNORE:
+                        break;
+                case L2CAP_TXSEQ_INVALID:
+                default:
+                        l2cap_send_disconn_req(chan->conn, chan,
+                                               ECONNRESET);
+                        break;
+                }
+                break;
+        case L2CAP_EV_RECV_RR:
+                l2cap_pass_to_tx(chan, control);
+                if (control->final) {
+                        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-                __skb_queue_head_init(&chan->srej_q);
-                l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
+                        if (!test_and_clear_bit(CONN_REJ_ACT,
+                                                &chan->conn_state)) {
+                                control->final = 0;
+                                l2cap_retransmit_all(chan, control);
+                        }
 
-                /* Set P-bit only if there are some I-frames to ack. */
-                if (__clear_ack_timer(chan))
-                        set_bit(CONN_SEND_PBIT, &chan->conn_state);
+                        l2cap_ertm_send(chan);
+                } else if (control->poll) {
+                        l2cap_send_i_or_rr_or_rnr(chan);
+                } else {
+                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
+                                               &chan->conn_state) &&
+                            chan->unacked_frames)
+                                __set_retrans_timer(chan);
 
-                err = l2cap_send_srejframe(chan, tx_seq);
-                if (err < 0) {
-                        l2cap_send_disconn_req(chan->conn, chan, -err);
-                        return err;
+                        l2cap_ertm_send(chan);
                 }
+                break;
+        case L2CAP_EV_RECV_RNR:
+                set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
+                l2cap_pass_to_tx(chan, control);
+                if (control && control->poll) {
+                        set_bit(CONN_SEND_FBIT, &chan->conn_state);
+                        l2cap_send_rr_or_rnr(chan, 0);
+                }
+                __clear_retrans_timer(chan);
+                l2cap_seq_list_clear(&chan->retrans_list);
+                break;
+        case L2CAP_EV_RECV_REJ:
+                l2cap_handle_rej(chan, control);
+                break;
+        case L2CAP_EV_RECV_SREJ:
+                l2cap_handle_srej(chan, control);
+                break;
+        default:
+                break;
         }
-        return 0;
-
-expected:
-        chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
-
-        if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
-                bt_cb(skb)->control.txseq = tx_seq;
-                bt_cb(skb)->control.sar = sar;
-                __skb_queue_tail(&chan->srej_q, skb);
-                return 0;
-        }
-
-        err = l2cap_reassemble_sdu(chan, skb, rx_control);
-        chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
 
-        if (err < 0) {
-                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
-                return err;
+        if (skb && !skb_in_use) {
+                BT_DBG("Freeing %p", skb);
+                kfree_skb(skb);
         }
 
-        if (__is_ctrl_final(chan, rx_control)) {
-                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
-                        l2cap_retransmit_frames(chan);
-        }
+        return err;
+}
 
+static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan,
+                                    struct l2cap_ctrl *control,
+                                    struct sk_buff *skb, u8 event)
+{
+        int err = 0;
+        u16 txseq = control->txseq;
+        bool skb_in_use = 0;
+
+        BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb,
+               event);
+
+        switch (event) {
+        case L2CAP_EV_RECV_IFRAME:
+                switch (l2cap_classify_txseq(chan, txseq)) {
+                case L2CAP_TXSEQ_EXPECTED:
+                        /* Keep frame for reassembly later */
+                        l2cap_pass_to_tx(chan, control);
+                        skb_queue_tail(&chan->srej_q, skb);
+                        skb_in_use = 1;
+                        BT_DBG("Queued %p (queue len %d)", skb,
+                               skb_queue_len(&chan->srej_q));
+
+                        chan->expected_tx_seq = __next_seq(chan, txseq);
+                        break;
+                case L2CAP_TXSEQ_EXPECTED_SREJ:
+                        l2cap_seq_list_pop(&chan->srej_list);
 
-        chan->num_acked = (chan->num_acked + 1) % num_to_ack;
-        if (chan->num_acked == num_to_ack - 1)
-                l2cap_send_ack(chan);
-        else
-                __set_ack_timer(chan);
+                        l2cap_pass_to_tx(chan, control);
+                        skb_queue_tail(&chan->srej_q, skb);
+                        skb_in_use = 1;
+                        BT_DBG("Queued %p (queue len %d)", skb,
+                               skb_queue_len(&chan->srej_q));
 
-        return 0;
+                        err = l2cap_rx_queued_iframes(chan);
+                        if (err)
+                                break;
 
-drop:
-        kfree_skb(skb);
-        return 0;
-}
+                        break;
+                case L2CAP_TXSEQ_UNEXPECTED:
+                        /* Got a frame that can't be reassembled yet.
+                         * Save it for later, and send SREJs to cover
+                         * the missing frames.
+                         */
+                        skb_queue_tail(&chan->srej_q, skb);
+                        skb_in_use = 1;
+                        BT_DBG("Queued %p (queue len %d)", skb,
+                               skb_queue_len(&chan->srej_q));
+
+                        l2cap_pass_to_tx(chan, control);
+                        l2cap_send_srej(chan, control->txseq);
+                        break;
+                case L2CAP_TXSEQ_UNEXPECTED_SREJ:
+                        /* This frame was requested with an SREJ, but
+                         * some expected retransmitted frames are
+                         * missing.  Request retransmission of missing
+                         * SREJ'd frames.
+                         */
+                        skb_queue_tail(&chan->srej_q, skb);
+                        skb_in_use = 1;
+                        BT_DBG("Queued %p (queue len %d)", skb,
+                               skb_queue_len(&chan->srej_q));
+
+                        l2cap_pass_to_tx(chan, control);
+                        l2cap_send_srej_list(chan, control->txseq);
+                        break;
+                case L2CAP_TXSEQ_DUPLICATE_SREJ:
+                        /* We've already queued this frame.  Drop this copy. */
+                        l2cap_pass_to_tx(chan, control);
+                        break;
+                case L2CAP_TXSEQ_DUPLICATE:
+                        /* Expecting a later sequence number, so this frame
+                         * was already received.  Ignore it completely.
+                         */
+                        break;
+                case L2CAP_TXSEQ_INVALID_IGNORE:
+                        break;
+                case L2CAP_TXSEQ_INVALID:
+                default:
+                        l2cap_send_disconn_req(chan->conn, chan,
+                                               ECONNRESET);
+                        break;
+                }
+                break;
+        case L2CAP_EV_RECV_RR:
+                l2cap_pass_to_tx(chan, control);
+                if (control->final) {
+                        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
-{
-        BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
-                                __get_reqseq(chan, rx_control), rx_control);
+                        if (!test_and_clear_bit(CONN_REJ_ACT,
+                                                &chan->conn_state)) {
+                                control->final = 0;
+                                l2cap_retransmit_all(chan, control);
+                        }
 
-        chan->expected_ack_seq = __get_reqseq(chan, rx_control);
-        l2cap_drop_acked_frames(chan);
+                        l2cap_ertm_send(chan);
+                } else if (control->poll) {
+                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
+                                               &chan->conn_state) &&
+                            chan->unacked_frames) {
+                                __set_retrans_timer(chan);
+                        }
 
-        if (__is_ctrl_poll(chan, rx_control)) {
-                set_bit(CONN_SEND_FBIT, &chan->conn_state);
-                if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
-                        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
-                                        (chan->unacked_frames > 0))
+                        set_bit(CONN_SEND_FBIT, &chan->conn_state);
+                        l2cap_send_srej_tail(chan);
+                } else {
+                        if (test_and_clear_bit(CONN_REMOTE_BUSY,
+                                               &chan->conn_state) &&
+                            chan->unacked_frames)
                                 __set_retrans_timer(chan);
 
-                        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
-                        l2cap_send_srejtail(chan);
+                        l2cap_send_ack(chan);
+                }
+                break;
+        case L2CAP_EV_RECV_RNR:
+                set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
+                l2cap_pass_to_tx(chan, control);
+                if (control->poll) {
+                        l2cap_send_srej_tail(chan);
                 } else {
-                        l2cap_send_i_or_rr_or_rnr(chan);
+                        struct l2cap_ctrl rr_control;
+                        memset(&rr_control, 0, sizeof(rr_control));
+                        rr_control.sframe = 1;
+                        rr_control.super = L2CAP_SUPER_RR;
+                        rr_control.reqseq = chan->buffer_seq;
+                        l2cap_send_sframe(chan, &rr_control);
                 }
 
-        } else if (__is_ctrl_final(chan, rx_control)) {
-                clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
-
-                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
-                        l2cap_retransmit_frames(chan);
-
-        } else {
-                if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
-                                (chan->unacked_frames > 0))
-                        __set_retrans_timer(chan);
+                break;
+        case L2CAP_EV_RECV_REJ:
+                l2cap_handle_rej(chan, control);
+                break;
+        case L2CAP_EV_RECV_SREJ:
+                l2cap_handle_srej(chan, control);
+                break;
+        }
 
-                clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
-                if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
-                        l2cap_send_ack(chan);
-                else
-                        l2cap_ertm_send(chan);
+        if (skb && !skb_in_use) {
+                BT_DBG("Freeing %p", skb);
+                kfree_skb(skb);
         }
+
+        return err;
 }
 
-static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
+static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq)
 {
-        u16 tx_seq = __get_reqseq(chan, rx_control);
-
-        BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
-
-        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
-
-        chan->expected_ack_seq = tx_seq;
-        l2cap_drop_acked_frames(chan);
-
-        if (__is_ctrl_final(chan, rx_control)) {
-                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
-                        l2cap_retransmit_frames(chan);
-        } else {
-                l2cap_retransmit_frames(chan);
+        /* Make sure reqseq is for a packet that has been sent but not acked */
+        u16 unacked;
 
-                if (test_bit(CONN_WAIT_F, &chan->conn_state))
-                        set_bit(CONN_REJ_ACT, &chan->conn_state);
-        }
+        unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq);
+        return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked;
 }
-static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
-{
-        u16 tx_seq = __get_reqseq(chan, rx_control);
-
-        BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
-
-        clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-        if (__is_ctrl_poll(chan, rx_control)) {
-                chan->expected_ack_seq = tx_seq;
-                l2cap_drop_acked_frames(chan);
-
-                set_bit(CONN_SEND_FBIT, &chan->conn_state);
-                l2cap_retransmit_one_frame(chan, tx_seq);
+static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
+                    struct sk_buff *skb, u8 event)
+{
+        int err = 0;
 
-                l2cap_ertm_send(chan);
+        BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan,
+               control, skb, event, chan->rx_state);
 
-                if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
-                        chan->srej_save_reqseq = tx_seq;
-                        set_bit(CONN_SREJ_ACT, &chan->conn_state);
+        if (__valid_reqseq(chan, control->reqseq)) {
+                switch (chan->rx_state) {
+                case L2CAP_RX_STATE_RECV:
+                        err = l2cap_rx_state_recv(chan, control, skb, event);
+                        break;
+                case L2CAP_RX_STATE_SREJ_SENT:
+                        err = l2cap_rx_state_srej_sent(chan, control, skb,
+                                                       event);
+                        break;
+                default:
+                        /* shut it down */
+                        break;
                 }
-        } else if (__is_ctrl_final(chan, rx_control)) {
-                if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
-                                chan->srej_save_reqseq == tx_seq)
-                        clear_bit(CONN_SREJ_ACT, &chan->conn_state);
-                else
-                        l2cap_retransmit_one_frame(chan, tx_seq);
         } else {
-                l2cap_retransmit_one_frame(chan, tx_seq);
-                if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
-                        chan->srej_save_reqseq = tx_seq;
-                        set_bit(CONN_SREJ_ACT, &chan->conn_state);
-                }
+                BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d",
+                       control->reqseq, chan->next_tx_seq,
+                       chan->expected_ack_seq);
+                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
         }
+
+        return err;
 }
 
-static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
+static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
+                           struct sk_buff *skb)
 {
-        u16 tx_seq = __get_reqseq(chan, rx_control);
+        int err = 0;
 
-        BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
+        BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb,
+               chan->rx_state);
 
-        set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
-        chan->expected_ack_seq = tx_seq;
-        l2cap_drop_acked_frames(chan);
+        if (l2cap_classify_txseq(chan, control->txseq) ==
+            L2CAP_TXSEQ_EXPECTED) {
+                l2cap_pass_to_tx(chan, control);
 
-        if (__is_ctrl_poll(chan, rx_control))
-                set_bit(CONN_SEND_FBIT, &chan->conn_state);
+                BT_DBG("buffer_seq %d->%d", chan->buffer_seq,
+                       __next_seq(chan, chan->buffer_seq));
 
-        if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
-                __clear_retrans_timer(chan);
-                if (__is_ctrl_poll(chan, rx_control))
-                        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
-                return;
-        }
+                chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
 
-        if (__is_ctrl_poll(chan, rx_control)) {
-                l2cap_send_srejtail(chan);
+                l2cap_reassemble_sdu(chan, skb, control);
         } else {
-                rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
-                l2cap_send_sframe(chan, rx_control);
-        }
-}
-
-static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
-{
-        BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
+                if (chan->sdu) {
+                        kfree_skb(chan->sdu);
+                        chan->sdu = NULL;
+                }
+                chan->sdu_last_frag = NULL;
+                chan->sdu_len = 0;
 
-        if (__is_ctrl_final(chan, rx_control) &&
-                        test_bit(CONN_WAIT_F, &chan->conn_state)) {
-                __clear_monitor_timer(chan);
-                if (chan->unacked_frames > 0)
-                        __set_retrans_timer(chan);
-                clear_bit(CONN_WAIT_F, &chan->conn_state);
+                if (skb) {
+                        BT_DBG("Freeing %p", skb);
+                        kfree_skb(skb);
+                }
         }
 
-        switch (__get_ctrl_super(chan, rx_control)) {
-        case L2CAP_SUPER_RR:
-                l2cap_data_channel_rrframe(chan, rx_control);
-                break;
+        chan->last_acked_seq = control->txseq;
+        chan->expected_tx_seq = __next_seq(chan, control->txseq);
 
-        case L2CAP_SUPER_REJ:
-                l2cap_data_channel_rejframe(chan, rx_control);
-                break;
-
-        case L2CAP_SUPER_SREJ:
-                l2cap_data_channel_srejframe(chan, rx_control);
-                break;
-
-        case L2CAP_SUPER_RNR:
-                l2cap_data_channel_rnrframe(chan, rx_control);
-                break;
-        }
-
-        kfree_skb(skb);
-        return 0;
+        return err;
 }
 
-static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
+static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
 {
-        u32 control;
-        u16 req_seq;
-        int len, next_tx_seq_offset, req_seq_offset;
+        struct l2cap_ctrl *control = &bt_cb(skb)->control;
+        u16 len;
+        u8 event;
 
         __unpack_control(chan, skb);
 
-        control = __get_control(chan, skb->data);
-        skb_pull(skb, __ctrl_size(chan));
         len = skb->len;
 
         /*
          * We can just drop the corrupted I-frame here.
          * Receiver will miss it and start proper recovery
-         * procedures and ask retransmission.
+         * procedures and ask for retransmission.
          */
         if (l2cap_check_fcs(chan, skb))
                 goto drop;
 
-        if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
+        if (!control->sframe && control->sar == L2CAP_SAR_START)
                 len -= L2CAP_SDULEN_SIZE;
 
         if (chan->fcs == L2CAP_FCS_CRC16)
@@ -4578,34 +5069,57 @@ static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb)
                 goto drop;
         }
 
-        req_seq = __get_reqseq(chan, control);
-
-        req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
-
-        next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
-                                                chan->expected_ack_seq);
+        if (!control->sframe) {
+                int err;
 
-        /* check for invalid req-seq */
-        if (req_seq_offset > next_tx_seq_offset) {
-                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
-                goto drop;
-        }
+                BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d",
+                       control->sar, control->reqseq, control->final,
+                       control->txseq);
 
-        if (!__is_sframe(chan, control)) {
-                if (len < 0) {
-                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+                /* Validate F-bit - F=0 always valid, F=1 only
+                 * valid in TX WAIT_F
+                 */
+                if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F)
                         goto drop;
+
+                if (chan->mode != L2CAP_MODE_STREAMING) {
+                        event = L2CAP_EV_RECV_IFRAME;
+                        err = l2cap_rx(chan, control, skb, event);
+                } else {
+                        err = l2cap_stream_rx(chan, control, skb);
                 }
 
-                l2cap_data_channel_iframe(chan, control, skb);
+                if (err)
+                        l2cap_send_disconn_req(chan->conn, chan,
+                                               ECONNRESET);
         } else {
+                const u8 rx_func_to_event[4] = {
+                        L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ,
+                        L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ
+                };
+
+                /* Only I-frames are expected in streaming mode */
+                if (chan->mode == L2CAP_MODE_STREAMING)
+                        goto drop;
+
+                BT_DBG("sframe reqseq %d, final %d, poll %d, super %d",
+                       control->reqseq, control->final, control->poll,
+                       control->super);
+
                 if (len != 0) {
                         BT_ERR("%d", len);
                         l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                         goto drop;
                 }
 
-                l2cap_data_channel_sframe(chan, control, skb);
+                /* Validate F and P bits */
+                if (control->final && (control->poll ||
+                                       chan->tx_state != L2CAP_TX_STATE_WAIT_F))
+                        goto drop;
+
+                event = rx_func_to_event[control->super];
+                if (l2cap_rx(chan, control, skb, event))
+                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
         }
 
         return 0;
@@ -4615,19 +5129,27 @@ drop:
         return 0;
 }
 
-static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
+static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid,
+                               struct sk_buff *skb)
 {
         struct l2cap_chan *chan;
-        u32 control;
-        u16 tx_seq;
-        int len;
 
         chan = l2cap_get_chan_by_scid(conn, cid);
         if (!chan) {
-                BT_DBG("unknown cid 0x%4.4x", cid);
-                /* Drop packet and return */
-                kfree_skb(skb);
-                return 0;
+                if (cid == L2CAP_CID_A2MP) {
+                        chan = a2mp_channel_create(conn, skb);
+                        if (!chan) {
+                                kfree_skb(skb);
+                                return;
+                        }
+
+                        l2cap_chan_lock(chan);
+                } else {
+                        BT_DBG("unknown cid 0x%4.4x", cid);
+                        /* Drop packet and return */
+                        kfree_skb(skb);
+                        return;
+                }
         }
 
         BT_DBG("chan %p, len %d", chan, skb->len);
@@ -4645,49 +5167,13 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 if (chan->imtu < skb->len)
                         goto drop;
 
-                if (!chan->ops->recv(chan->data, skb))
+                if (!chan->ops->recv(chan, skb))
                         goto done;
                 break;
 
         case L2CAP_MODE_ERTM:
-                l2cap_ertm_data_rcv(chan, skb);
-
-                goto done;
-
         case L2CAP_MODE_STREAMING:
-                control = __get_control(chan, skb->data);
-                skb_pull(skb, __ctrl_size(chan));
-                len = skb->len;
-
-                if (l2cap_check_fcs(chan, skb))
-                        goto drop;
-
-                if (__is_sar_start(chan, control))
-                        len -= L2CAP_SDULEN_SIZE;
-
-                if (chan->fcs == L2CAP_FCS_CRC16)
-                        len -= L2CAP_FCS_SIZE;
-
-                if (len > chan->mps || len < 0 || __is_sframe(chan, control))
-                        goto drop;
-
-                tx_seq = __get_txseq(chan, control);
-
-                if (chan->expected_tx_seq != tx_seq) {
-                        /* Frame(s) missing - must discard partial SDU */
-                        kfree_skb(chan->sdu);
-                        chan->sdu = NULL;
-                        chan->sdu_last_frag = NULL;
-                        chan->sdu_len = 0;
-
-                        /* TODO: Notify userland of missing data */
-                }
-
-                chan->expected_tx_seq = __next_seq(chan, tx_seq);
-
-                if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
-                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
-
+                l2cap_data_rcv(chan, skb);
                 goto done;
 
         default:
@@ -4700,11 +5186,10 @@ drop:
 
 done:
         l2cap_chan_unlock(chan);
-
-        return 0;
 }
 
-static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
+static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
+                                  struct sk_buff *skb)
 {
         struct l2cap_chan *chan;
 
@@ -4720,17 +5205,15 @@ static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, str
         if (chan->imtu < skb->len)
                 goto drop;
 
-        if (!chan->ops->recv(chan->data, skb))
-                return 0;
+        if (!chan->ops->recv(chan, skb))
+                return;
 
 drop:
         kfree_skb(skb);
-
-        return 0;
 }
 
-static inline int l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
-                                    struct sk_buff *skb)
+static void l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
+                              struct sk_buff *skb)
 {
         struct l2cap_chan *chan;
 
@@ -4746,13 +5229,11 @@ static inline int l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
         if (chan->imtu < skb->len)
                 goto drop;
 
-        if (!chan->ops->recv(chan->data, skb))
-                return 0;
+        if (!chan->ops->recv(chan, skb))
+                return;
 
 drop:
         kfree_skb(skb);
-
-        return 0;
 }
 
 static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
@@ -4780,7 +5261,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 
         case L2CAP_CID_CONN_LESS:
                 psm = get_unaligned((__le16 *) skb->data);
-                skb_pull(skb, 2);
+                skb_pull(skb, L2CAP_PSMLEN_SIZE);
                 l2cap_conless_channel(conn, psm, skb);
                 break;
 
@@ -4974,6 +5455,17 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         rsp.status = cpu_to_le16(stat);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
                                                         sizeof(rsp), &rsp);
+
+                        if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
+                            res == L2CAP_CR_SUCCESS) {
+                                char buf[128];
+                                set_bit(CONF_REQ_SENT, &chan->conf_state);
+                                l2cap_send_cmd(conn, l2cap_get_ident(conn),
+                                               L2CAP_CONF_REQ,
+                                               l2cap_build_conf_req(chan, buf),
+                                               buf);
+                                chan->num_conf_req++;
+                        }
                 }
 
                 l2cap_chan_unlock(chan);
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 3bb1611b9d4..a4bb27e8427 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -27,7 +27,6 @@
 
 /* Bluetooth L2CAP sockets. */
 
-#include <linux/security.h>
 #include <linux/export.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -89,8 +88,8 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         if (err < 0)
                 goto done;
 
-        if (__le16_to_cpu(la.l2_psm) == 0x0001 ||
-                                __le16_to_cpu(la.l2_psm) == 0x0003)
+        if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_SDP ||
+            __le16_to_cpu(la.l2_psm) == L2CAP_PSM_RFCOMM)
                 chan->sec_level = BT_SECURITY_SDP;
 
         bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
@@ -446,6 +445,22 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
         return err;
 }
 
+static bool l2cap_valid_mtu(struct l2cap_chan *chan, u16 mtu)
+{
+        switch (chan->scid) {
+        case L2CAP_CID_LE_DATA:
+                if (mtu < L2CAP_LE_MIN_MTU)
+                        return false;
+                break;
+
+        default:
+                if (mtu < L2CAP_DEFAULT_MIN_MTU)
+                        return false;
+        }
+
+        return true;
+}
+
 static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
 {
         struct sock *sk = sock->sk;
@@ -484,6 +499,11 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                         break;
                 }
 
+                if (!l2cap_valid_mtu(chan, opts.imtu)) {
+                        err = -EINVAL;
+                        break;
+                }
+
                 chan->mode = opts.mode;
                 switch (chan->mode) {
                 case L2CAP_MODE_BASIC:
@@ -873,9 +893,34 @@ static int l2cap_sock_release(struct socket *sock)
         return err;
 }
 
-static struct l2cap_chan *l2cap_sock_new_connection_cb(void *data)
+static void l2cap_sock_cleanup_listen(struct sock *parent)
 {
-        struct sock *sk, *parent = data;
+        struct sock *sk;
+
+        BT_DBG("parent %p", parent);
+
+        /* Close not yet accepted channels */
+        while ((sk = bt_accept_dequeue(parent, NULL))) {
+                struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+
+                l2cap_chan_lock(chan);
+                __clear_chan_timer(chan);
+                l2cap_chan_close(chan, ECONNRESET);
+                l2cap_chan_unlock(chan);
+
+                l2cap_sock_kill(sk);
+        }
+}
+
+static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
+{
+        struct sock *sk, *parent = chan->data;
+
+        /* Check for backlog size */
+        if (sk_acceptq_is_full(parent)) {
+                BT_DBG("backlog full %d", parent->sk_ack_backlog);
+                return NULL;
+        }
 
         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP,
                                                                 GFP_ATOMIC);
@@ -889,10 +934,10 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(void *data)
         return l2cap_pi(sk)->chan;
 }
 
-static int l2cap_sock_recv_cb(void *data, struct sk_buff *skb)
+static int l2cap_sock_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
         int err;
-        struct sock *sk = data;
+        struct sock *sk = chan->data;
         struct l2cap_pinfo *pi = l2cap_pi(sk);
 
         lock_sock(sk);
@@ -925,16 +970,57 @@ done:
         return err;
 }
 
-static void l2cap_sock_close_cb(void *data)
+static void l2cap_sock_close_cb(struct l2cap_chan *chan)
 {
-        struct sock *sk = data;
+        struct sock *sk = chan->data;
 
         l2cap_sock_kill(sk);
 }
 
-static void l2cap_sock_state_change_cb(void *data, int state)
+static void l2cap_sock_teardown_cb(struct l2cap_chan *chan, int err)
 {
-        struct sock *sk = data;
+        struct sock *sk = chan->data;
+        struct sock *parent;
+
+        lock_sock(sk);
+
+        parent = bt_sk(sk)->parent;
+
+        sock_set_flag(sk, SOCK_ZAPPED);
+
+        switch (chan->state) {
+        case BT_OPEN:
+        case BT_BOUND:
+        case BT_CLOSED:
+                break;
+        case BT_LISTEN:
+                l2cap_sock_cleanup_listen(sk);
+                sk->sk_state = BT_CLOSED;
+                chan->state = BT_CLOSED;
+
+                break;
+        default:
+                sk->sk_state = BT_CLOSED;
+                chan->state = BT_CLOSED;
+
+                sk->sk_err = err;
+
+                if (parent) {
+                        bt_accept_unlink(sk);
+                        parent->sk_data_ready(parent, 0);
+                } else {
+                        sk->sk_state_change(sk);
+                }
+
+                break;
+        }
+
+        release_sock(sk);
+}
+
+static void l2cap_sock_state_change_cb(struct l2cap_chan *chan, int state)
+{
+        struct sock *sk = chan->data;
 
         sk->sk_state = state;
 }
@@ -955,12 +1041,34 @@ static struct sk_buff *l2cap_sock_alloc_skb_cb(struct l2cap_chan *chan,
         return skb;
 }
 
+static void l2cap_sock_ready_cb(struct l2cap_chan *chan)
+{
+        struct sock *sk = chan->data;
+        struct sock *parent;
+
+        lock_sock(sk);
+
+        parent = bt_sk(sk)->parent;
+
+        BT_DBG("sk %p, parent %p", sk, parent);
+
+        sk->sk_state = BT_CONNECTED;
+        sk->sk_state_change(sk);
+
+        if (parent)
+                parent->sk_data_ready(parent, 0);
+
+        release_sock(sk);
+}
+
 static struct l2cap_ops l2cap_chan_ops = {
         .name           = "L2CAP Socket Interface",
         .new_connection = l2cap_sock_new_connection_cb,
         .recv           = l2cap_sock_recv_cb,
         .close          = l2cap_sock_close_cb,
+        .teardown       = l2cap_sock_teardown_cb,
         .state_change   = l2cap_sock_state_change_cb,
+        .ready          = l2cap_sock_ready_cb,
         .alloc_skb      = l2cap_sock_alloc_skb_cb,
 };
 
diff --git a/net/bluetooth/lib.c b/net/bluetooth/lib.c
index 506628876f3..e1c97527e16 100644
--- a/net/bluetooth/lib.c
+++ b/net/bluetooth/lib.c
@@ -26,12 +26,7 @@
 
 #define pr_fmt(fmt) "Bluetooth: " fmt
 
-#include <linux/module.h>
-
-#include <linux/kernel.h>
-#include <linux/stddef.h>
-#include <linux/string.h>
-#include <asm/errno.h>
+#include <linux/export.h>
 
 #include <net/bluetooth/bluetooth.h>
 
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 25d22077607..c72307cc25f 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -24,8 +24,6 @@
 
 /* Bluetooth HCI Management interface */
 
-#include <linux/kernel.h>
-#include <linux/uaccess.h>
 #include <linux/module.h>
 #include <asm/unaligned.h>
 
@@ -714,7 +712,8 @@ static struct pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
 }
 
 static void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
-                                 void (*cb)(struct pending_cmd *cmd, void *data),
+                                 void (*cb)(struct pending_cmd *cmd,
+                                            void *data),
                                  void *data)
 {
         struct list_head *p, *n;
@@ -871,7 +870,7 @@ static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data,
         }
 
         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) ||
-                        mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
+            mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
                 err = cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE,
                                  MGMT_STATUS_BUSY);
                 goto failed;
@@ -978,7 +977,7 @@ static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data,
         }
 
         if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) ||
-                        mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
+            mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) {
                 err = cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE,
                                  MGMT_STATUS_BUSY);
                 goto failed;
@@ -1001,7 +1000,7 @@ static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data,
                 scan = 0;
 
                 if (test_bit(HCI_ISCAN, &hdev->flags) &&
-                                                hdev->discov_timeout > 0)
+                    hdev->discov_timeout > 0)
                         cancel_delayed_work(&hdev->discov_off);
         }
 
@@ -1056,7 +1055,7 @@ static int set_link_security(struct sock *sk, struct hci_dev *hdev, void *data,
                 bool changed = false;
 
                 if (!!cp->val != test_bit(HCI_LINK_SECURITY,
-                                                        &hdev->dev_flags)) {
+                                          &hdev->dev_flags)) {
                         change_bit(HCI_LINK_SECURITY, &hdev->dev_flags);
                         changed = true;
                 }
@@ -1317,7 +1316,7 @@ static bool enable_service_cache(struct hci_dev *hdev)
 }
 
 static int remove_uuid(struct sock *sk, struct hci_dev *hdev, void *data,
-                                                                u16 len)
+                       u16 len)
 {
         struct mgmt_cp_remove_uuid *cp = data;
         struct pending_cmd *cmd;
@@ -1442,7 +1441,7 @@ unlock:
 }
 
 static int load_link_keys(struct sock *sk, struct hci_dev *hdev, void *data,
-                                                                u16 len)
+                          u16 len)
 {
         struct mgmt_cp_load_link_keys *cp = data;
         u16 key_count, expected_len;
@@ -1454,13 +1453,13 @@ static int load_link_keys(struct sock *sk, struct hci_dev *hdev, void *data,
                                         sizeof(struct mgmt_link_key_info);
         if (expected_len != len) {
                 BT_ERR("load_link_keys: expected %u bytes, got %u bytes",
-                                                        len, expected_len);
+                       len, expected_len);
                 return cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS,
                                   MGMT_STATUS_INVALID_PARAMS);
         }
 
         BT_DBG("%s debug_keys %u key_count %u", hdev->name, cp->debug_keys,
-                                                                key_count);
+               key_count);
 
         hci_dev_lock(hdev);
 
@@ -1535,10 +1534,10 @@ static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data,
         if (cp->disconnect) {
                 if (cp->addr.type == BDADDR_BREDR)
                         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
-                                                        &cp->addr.bdaddr);
+                                                       &cp->addr.bdaddr);
                 else
                         conn = hci_conn_hash_lookup_ba(hdev, LE_LINK,
-                                                        &cp->addr.bdaddr);
+                                                       &cp->addr.bdaddr);
         } else {
                 conn = NULL;
         }
@@ -1594,7 +1593,8 @@ static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data,
         }
 
         if (cp->addr.type == BDADDR_BREDR)
-                conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->addr.bdaddr);
+                conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
+                                               &cp->addr.bdaddr);
         else
                 conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->addr.bdaddr);
 
@@ -1813,7 +1813,7 @@ static int set_io_capability(struct sock *sk, struct hci_dev *hdev, void *data,
         hdev->io_capability = cp->io_capability;
 
         BT_DBG("%s IO capability set to 0x%02x", hdev->name,
-                                                        hdev->io_capability);
+               hdev->io_capability);
 
         hci_dev_unlock(hdev);
 
@@ -1821,7 +1821,7 @@ static int set_io_capability(struct sock *sk, struct hci_dev *hdev, void *data,
                             0);
 }
 
-static inline struct pending_cmd *find_pairing(struct hci_conn *conn)
+static struct pending_cmd *find_pairing(struct hci_conn *conn)
 {
         struct hci_dev *hdev = conn->hdev;
         struct pending_cmd *cmd;
@@ -1873,6 +1873,22 @@ static void pairing_complete_cb(struct hci_conn *conn, u8 status)
                 pairing_complete(cmd, mgmt_status(status));
 }
 
+static void le_connect_complete_cb(struct hci_conn *conn, u8 status)
+{
+        struct pending_cmd *cmd;
+
+        BT_DBG("status %u", status);
+
+        if (!status)
+                return;
+
+        cmd = find_pairing(conn);
+        if (!cmd)
+                BT_DBG("Unable to find a pending command");
+        else
+                pairing_complete(cmd, mgmt_status(status));
+}
+
 static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
                        u16 len)
 {
@@ -1911,8 +1927,15 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
         rp.addr.type = cp->addr.type;
 
         if (IS_ERR(conn)) {
+                int status;
+
+                if (PTR_ERR(conn) == -EBUSY)
+                        status = MGMT_STATUS_BUSY;
+                else
+                        status = MGMT_STATUS_CONNECT_FAILED;
+
                 err = cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE,
-                                   MGMT_STATUS_CONNECT_FAILED, &rp,
+                                   status, &rp,
                                    sizeof(rp));
                 goto unlock;
         }
@@ -1934,6 +1957,8 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
         /* For LE, just connecting isn't a proof that the pairing finished */
         if (cp->addr.type == BDADDR_BREDR)
                 conn->connect_cfm_cb = pairing_complete_cb;
+        else
+                conn->connect_cfm_cb = le_connect_complete_cb;
 
         conn->security_cfm_cb = pairing_complete_cb;
         conn->disconn_cfm_cb = pairing_complete_cb;
@@ -1941,7 +1966,7 @@ static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data,
         cmd->user_data = conn;
 
         if (conn->state == BT_CONNECTED &&
-                                hci_conn_security(conn, sec_level, auth_type))
+            hci_conn_security(conn, sec_level, auth_type))
                 pairing_complete(cmd, 0);
 
         err = 0;
@@ -2238,7 +2263,7 @@ unlock:
 }
 
 static int remove_remote_oob_data(struct sock *sk, struct hci_dev *hdev,
-                                                void *data, u16 len)
+                                  void *data, u16 len)
 {
         struct mgmt_cp_remove_remote_oob_data *cp = data;
         u8 status;
@@ -2407,7 +2432,7 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
 
         case DISCOVERY_RESOLVING:
                 e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY,
-                                                        NAME_PENDING);
+                                                     NAME_PENDING);
                 if (!e) {
                         mgmt_pending_remove(cmd);
                         err = cmd_complete(sk, hdev->id,
@@ -2629,7 +2654,7 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev,
                                         sizeof(struct mgmt_ltk_info);
         if (expected_len != len) {
                 BT_ERR("load_keys: expected %u bytes, got %u bytes",
-                                                        len, expected_len);
+                       len, expected_len);
                 return cmd_status(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS,
                                   EINVAL);
         }
@@ -2754,7 +2779,7 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
         }
 
         if (opcode >= ARRAY_SIZE(mgmt_handlers) ||
-                                        mgmt_handlers[opcode].func == NULL) {
+            mgmt_handlers[opcode].func == NULL) {
                 BT_DBG("Unknown op %u", opcode);
                 err = cmd_status(sk, index, opcode,
                                  MGMT_STATUS_UNKNOWN_COMMAND);
@@ -2762,7 +2787,7 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
         }
 
         if ((hdev && opcode < MGMT_OP_READ_INFO) ||
-                        (!hdev && opcode >= MGMT_OP_READ_INFO)) {
+            (!hdev && opcode >= MGMT_OP_READ_INFO)) {
                 err = cmd_status(sk, index, opcode,
                                  MGMT_STATUS_INVALID_INDEX);
                 goto done;
@@ -2771,7 +2796,7 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
         handler = &mgmt_handlers[opcode];
 
         if ((handler->var_len && len < handler->data_len) ||
-                        (!handler->var_len && len != handler->data_len)) {
+            (!handler->var_len && len != handler->data_len)) {
                 err = cmd_status(sk, index, opcode,
                                  MGMT_STATUS_INVALID_PARAMS);
                 goto done;
@@ -2955,7 +2980,7 @@ int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key,
         bacpy(&ev.key.addr.bdaddr, &key->bdaddr);
         ev.key.addr.type = BDADDR_BREDR;
         ev.key.type = key->type;
-        memcpy(ev.key.val, key->val, 16);
+        memcpy(ev.key.val, key->val, HCI_LINK_KEY_SIZE);
         ev.key.pin_len = key->pin_len;
 
         return mgmt_event(MGMT_EV_NEW_LINK_KEY, hdev, &ev, sizeof(ev), NULL);
@@ -3090,7 +3115,7 @@ int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
         mgmt_pending_remove(cmd);
 
         mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
-                                                                        hdev);
+                             hdev);
         return err;
 }
 
@@ -3180,7 +3205,7 @@ int mgmt_user_confirm_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
 }
 
 int mgmt_user_passkey_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                                                u8 link_type, u8 addr_type)
+                              u8 link_type, u8 addr_type)
 {
         struct mgmt_ev_user_passkey_request ev;
 
@@ -3194,8 +3219,8 @@ int mgmt_user_passkey_request(struct hci_dev *hdev, bdaddr_t *bdaddr,
 }
 
 static int user_pairing_resp_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                                        u8 link_type, u8 addr_type, u8 status,
-                                        u8 opcode)
+                                      u8 link_type, u8 addr_type, u8 status,
+                                      u8 opcode)
 {
         struct pending_cmd *cmd;
         struct mgmt_rp_user_confirm_reply rp;
@@ -3226,7 +3251,8 @@ int mgmt_user_confirm_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
                                          u8 link_type, u8 addr_type, u8 status)
 {
         return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type,
-                                          status, MGMT_OP_USER_CONFIRM_NEG_REPLY);
+                                          status,
+                                          MGMT_OP_USER_CONFIRM_NEG_REPLY);
 }
 
 int mgmt_user_passkey_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
@@ -3240,7 +3266,8 @@ int mgmt_user_passkey_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
                                          u8 link_type, u8 addr_type, u8 status)
 {
         return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type,
-                                          status, MGMT_OP_USER_PASSKEY_NEG_REPLY);
+                                          status,
+                                          MGMT_OP_USER_PASSKEY_NEG_REPLY);
 }
 
 int mgmt_auth_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
diff --git a/net/bluetooth/rfcomm/core.c b/net/bluetooth/rfcomm/core.c
index 8a602388f1e..c75107ef892 100644
--- a/net/bluetooth/rfcomm/core.c
+++ b/net/bluetooth/rfcomm/core.c
@@ -26,22 +26,8 @@
  */
 
 #include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/signal.h>
-#include <linux/init.h>
-#include <linux/wait.h>
-#include <linux/device.h>
 #include <linux/debugfs.h>
-#include <linux/seq_file.h>
-#include <linux/net.h>
-#include <linux/mutex.h>
 #include <linux/kthread.h>
-#include <linux/slab.h>
-
-#include <net/sock.h>
-#include <linux/uaccess.h>
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
@@ -115,14 +101,14 @@ static void rfcomm_session_del(struct rfcomm_session *s);
 #define __get_rpn_stop_bits(line) (((line) >> 2) & 0x1)
 #define __get_rpn_parity(line)    (((line) >> 3) & 0x7)
 
-static inline void rfcomm_schedule(void)
+static void rfcomm_schedule(void)
 {
         if (!rfcomm_thread)
                 return;
         wake_up_process(rfcomm_thread);
 }
 
-static inline void rfcomm_session_put(struct rfcomm_session *s)
+static void rfcomm_session_put(struct rfcomm_session *s)
 {
         if (atomic_dec_and_test(&s->refcnt))
                 rfcomm_session_del(s);
@@ -227,7 +213,7 @@ static int rfcomm_l2sock_create(struct socket **sock)
         return err;
 }
 
-static inline int rfcomm_check_security(struct rfcomm_dlc *d)
+static int rfcomm_check_security(struct rfcomm_dlc *d)
 {
         struct sock *sk = d->session->sock->sk;
         struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
@@ -1750,7 +1736,7 @@ static void rfcomm_process_connect(struct rfcomm_session *s)
 /* Send data queued for the DLC.
  * Return number of frames left in the queue.
  */
-static inline int rfcomm_process_tx(struct rfcomm_dlc *d)
+static int rfcomm_process_tx(struct rfcomm_dlc *d)
 {
         struct sk_buff *skb;
         int err;
@@ -1798,7 +1784,7 @@ static inline int rfcomm_process_tx(struct rfcomm_dlc *d)
         return skb_queue_len(&d->tx_queue);
 }
 
-static inline void rfcomm_process_dlcs(struct rfcomm_session *s)
+static void rfcomm_process_dlcs(struct rfcomm_session *s)
 {
         struct rfcomm_dlc *d;
         struct list_head *p, *n;
@@ -1858,7 +1844,7 @@ static inline void rfcomm_process_dlcs(struct rfcomm_session *s)
         }
 }
 
-static inline void rfcomm_process_rx(struct rfcomm_session *s)
+static void rfcomm_process_rx(struct rfcomm_session *s)
 {
         struct socket *sock = s->sock;
         struct sock *sk = sock->sk;
@@ -1883,7 +1869,7 @@ static inline void rfcomm_process_rx(struct rfcomm_session *s)
         }
 }
 
-static inline void rfcomm_accept_connection(struct rfcomm_session *s)
+static void rfcomm_accept_connection(struct rfcomm_session *s)
 {
         struct socket *sock = s->sock, *nsock;
         int err;
@@ -1917,7 +1903,7 @@ static inline void rfcomm_accept_connection(struct rfcomm_session *s)
                 sock_release(nsock);
 }
 
-static inline void rfcomm_check_connection(struct rfcomm_session *s)
+static void rfcomm_check_connection(struct rfcomm_session *s)
 {
         struct sock *sk = s->sock->sk;
 
@@ -1941,7 +1927,7 @@ static inline void rfcomm_check_connection(struct rfcomm_session *s)
         }
 }
 
-static inline void rfcomm_process_sessions(void)
+static void rfcomm_process_sessions(void)
 {
         struct list_head *p, *n;
 
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index e8707debb86..7e1e59645c0 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -25,27 +25,8 @@
  * RFCOMM sockets.
  */
 
-#include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/socket.h>
-#include <linux/skbuff.h>
-#include <linux/list.h>
-#include <linux/device.h>
+#include <linux/export.h>
 #include <linux/debugfs.h>
-#include <linux/seq_file.h>
-#include <linux/security.h>
-#include <net/sock.h>
-
-#include <linux/uaccess.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
diff --git a/net/bluetooth/rfcomm/tty.c b/net/bluetooth/rfcomm/tty.c
index d1820ff14ae..cb960773c00 100644
--- a/net/bluetooth/rfcomm/tty.c
+++ b/net/bluetooth/rfcomm/tty.c
@@ -31,11 +31,6 @@
 #include <linux/tty_driver.h>
 #include <linux/tty_flip.h>
 
-#include <linux/capability.h>
-#include <linux/slab.h>
-#include <linux/skbuff.h>
-#include <linux/workqueue.h>
-
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/rfcomm.h>
@@ -132,7 +127,7 @@ static struct rfcomm_dev *__rfcomm_dev_get(int id)
         return NULL;
 }
 
-static inline struct rfcomm_dev *rfcomm_dev_get(int id)
+static struct rfcomm_dev *rfcomm_dev_get(int id)
 {
         struct rfcomm_dev *dev;
 
@@ -345,7 +340,7 @@ static void rfcomm_wfree(struct sk_buff *skb)
         tty_port_put(&dev->port);
 }
 
-static inline void rfcomm_set_owner_w(struct sk_buff *skb, struct rfcomm_dev *dev)
+static void rfcomm_set_owner_w(struct sk_buff *skb, struct rfcomm_dev *dev)
 {
         tty_port_get(&dev->port);
         atomic_add(skb->truesize, &dev->wmem_alloc);
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index cbdd313659a..40bbe25dcff 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -25,26 +25,8 @@
 /* Bluetooth SCO sockets. */
 
 #include <linux/module.h>
-
-#include <linux/types.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/poll.h>
-#include <linux/fcntl.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/socket.h>
-#include <linux/skbuff.h>
-#include <linux/device.h>
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
-#include <linux/list.h>
-#include <linux/security.h>
-#include <net/sock.h>
-
-#include <linux/uaccess.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -123,7 +105,7 @@ static struct sco_conn *sco_conn_add(struct hci_conn *hcon)
         return conn;
 }
 
-static inline struct sock *sco_chan_get(struct sco_conn *conn)
+static struct sock *sco_chan_get(struct sco_conn *conn)
 {
         struct sock *sk = NULL;
         sco_conn_lock(conn);
@@ -157,7 +139,8 @@ static int sco_conn_del(struct hci_conn *hcon, int err)
         return 0;
 }
 
-static inline int sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent)
+static int sco_chan_add(struct sco_conn *conn, struct sock *sk,
+                        struct sock *parent)
 {
         int err = 0;
 
@@ -228,7 +211,7 @@ done:
         return err;
 }
 
-static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
+static int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
 {
         struct sco_conn *conn = sco_pi(sk)->conn;
         struct sk_buff *skb;
@@ -254,7 +237,7 @@ static inline int sco_send_frame(struct sock *sk, struct msghdr *msg, int len)
         return len;
 }
 
-static inline void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb)
+static void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb)
 {
         struct sock *sk = sco_chan_get(conn);
 
@@ -523,7 +506,7 @@ static int sco_sock_connect(struct socket *sock, struct sockaddr *addr, int alen
                 goto done;
 
         err = bt_sock_wait_state(sk, BT_CONNECTED,
-                        sock_sndtimeo(sk, flags & O_NONBLOCK));
+                                 sock_sndtimeo(sk, flags & O_NONBLOCK));
 
 done:
         release_sock(sk);
@@ -788,7 +771,7 @@ static int sco_sock_shutdown(struct socket *sock, int how)
 
                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
                         err = bt_sock_wait_state(sk, BT_CLOSED,
-                                                        sk->sk_lingertime);
+                                                 sk->sk_lingertime);
         }
         release_sock(sk);
         return err;
@@ -878,7 +861,7 @@ static void sco_conn_ready(struct sco_conn *conn)
                 bh_lock_sock(parent);
 
                 sk = sco_sock_alloc(sock_net(parent), NULL,
-                                BTPROTO_SCO, GFP_ATOMIC);
+                                    BTPROTO_SCO, GFP_ATOMIC);
                 if (!sk) {
                         bh_unlock_sock(parent);
                         goto done;
@@ -907,7 +890,7 @@ done:
 /* ----- SCO interface with lower layer (HCI) ----- */
 int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
 {
-        register struct sock *sk;
+        struct sock *sk;
         struct hlist_node *node;
         int lm = 0;
 
@@ -920,7 +903,7 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
                         continue;
 
                 if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr) ||
-                                !bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
+                    !bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
                         lm |= HCI_LM_ACCEPT;
                         break;
                 }
@@ -981,7 +964,7 @@ static int sco_debugfs_show(struct seq_file *f, void *p)
 
         sk_for_each(sk, node, &sco_sk_list.head) {
                 seq_printf(f, "%s %s %d\n", batostr(&bt_sk(sk)->src),
-                                batostr(&bt_sk(sk)->dst), sk->sk_state);
+                           batostr(&bt_sk(sk)->dst), sk->sk_state);
         }
 
         read_unlock(&sco_sk_list.lock);
@@ -1044,8 +1027,8 @@ int __init sco_init(void)
         }
 
         if (bt_debugfs) {
-                sco_debugfs = debugfs_create_file("sco", 0444,
-                                        bt_debugfs, NULL, &sco_debugfs_fops);
+                sco_debugfs = debugfs_create_file("sco", 0444, bt_debugfs,
+                                                  NULL, &sco_debugfs_fops);
                 if (!sco_debugfs)
                         BT_ERR("Failed to create SCO debug file");
         }
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index 6fc7c4708f3..16ef0dc85a0 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -20,14 +20,15 @@
    SOFTWARE IS DISCLAIMED.
 */
 
+#include <linux/crypto.h>
+#include <linux/scatterlist.h>
+#include <crypto/b128ops.h>
+
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/smp.h>
-#include <linux/crypto.h>
-#include <linux/scatterlist.h>
-#include <crypto/b128ops.h>
 
 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
 
@@ -648,7 +649,7 @@ static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
 
         auth |= (req->auth_req | rsp->auth_req) & SMP_AUTH_MITM;
 
-        ret = tk_request(conn, 0, auth, rsp->io_capability, req->io_capability);
+        ret = tk_request(conn, 0, auth, req->io_capability, rsp->io_capability);
         if (ret)
                 return SMP_UNSPECIFIED;
 
@@ -703,7 +704,7 @@ static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb)
         return 0;
 }
 
-static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
+static u8 smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level)
 {
         struct smp_ltk *key;
         struct hci_conn *hcon = conn->hcon;
@@ -712,6 +713,9 @@ static u8 smp_ltk_encrypt(struct l2cap_conn *conn)
         if (!key)
                 return 0;
 
+        if (sec_level > BT_SECURITY_MEDIUM && !key->authenticated)
+                return 0;
+
         if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags))
                 return 1;
 
@@ -732,7 +736,7 @@ static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 
         hcon->pending_sec_level = authreq_to_seclevel(rp->auth_req);
 
-        if (smp_ltk_encrypt(conn))
+        if (smp_ltk_encrypt(conn, hcon->pending_sec_level))
                 return 0;
 
         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
@@ -771,7 +775,7 @@ int smp_conn_security(struct l2cap_conn *conn, __u8 sec_level)
                 return 1;
 
         if (hcon->link_mode & HCI_LM_MASTER)
-                if (smp_ltk_encrypt(conn))
+                if (smp_ltk_encrypt(conn, sec_level))
                         goto done;
 
         if (test_and_set_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index ebc353ef690..0f02c8b77e1 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -2111,6 +2111,9 @@ static int ieee80211_set_bitrate_mask(struct wiphy *wiphy,
         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
         int i, ret;
 
+        if (!ieee80211_sdata_running(sdata))
+                return -ENETDOWN;
+
         if (local->hw.flags & IEEE80211_HW_HAS_RATE_CONTROL) {
                 ret = drv_set_bitrate_mask(local, sdata, mask);
                 if (ret)
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 398acc4f649..398ce8e9c4d 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -1331,6 +1331,8 @@ static void ieee80211_set_disassoc(struct ieee80211_sub_if_data *sdata,
         if (WARN_ON(!ifmgd->associated))
                 return;
 
+        ieee80211_stop_poll(sdata);
+
         memcpy(bssid, ifmgd->associated->bssid, ETH_ALEN);
 
         ifmgd->associated = NULL;
@@ -2589,8 +2591,6 @@ static void ieee80211_sta_connection_lost(struct ieee80211_sub_if_data *sdata,
         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
         u8 frame_buf[DEAUTH_DISASSOC_LEN];
 
-        ieee80211_stop_poll(sdata);
-
         ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH, reason,
                                false, frame_buf);
         mutex_unlock(&ifmgd->mtx);
@@ -3081,7 +3081,7 @@ static int ieee80211_prep_connection(struct ieee80211_sub_if_data *sdata,
         }
 
         local->oper_channel = cbss->channel;
-        ieee80211_hw_config(local, 0);
+        ieee80211_hw_config(local, IEEE80211_CONF_CHANGE_CHANNEL);
 
         if (sta) {
                 u32 rates = 0, basic_rates = 0;
diff --git a/net/mac80211/sta_info.h b/net/mac80211/sta_info.h
index 3bb24a121c9..a470e1123a5 100644
--- a/net/mac80211/sta_info.h
+++ b/net/mac80211/sta_info.h
@@ -271,6 +271,9 @@ struct sta_ampdu_mlme {
  * @plink_timer: peer link watch timer
  * @plink_timer_was_running: used by suspend/resume to restore timers
  * @t_offset: timing offset relative to this host
+ * @t_offset_setpoint: reference timing offset of this sta to be used when
+ *      calculating clockdrift
+ * @ch_type: peer's channel type
  * @debugfs: debug filesystem info
  * @dead: set to true when sta is unlinked
  * @uploaded: set to true when sta is uploaded to the driver
@@ -278,6 +281,8 @@ struct sta_ampdu_mlme {
  * @sta: station information we share with the driver
  * @sta_state: duplicates information about station state (for debug)
  * @beacon_loss_count: number of times beacon loss has triggered
+ * @supports_40mhz: tracks whether the station advertised 40 MHz support
+ *      as we overwrite its HT parameters with the currently used value
  */
 struct sta_info {
         /* General information, mostly static */
diff --git a/net/nfc/core.c b/net/nfc/core.c
index 9f6ce011d35..4177bb5104b 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -121,14 +121,14 @@ error:
  * The device remains polling for targets until a target is found or
  * the nfc_stop_poll function is called.
  */
-int nfc_start_poll(struct nfc_dev *dev, u32 protocols)
+int nfc_start_poll(struct nfc_dev *dev, u32 im_protocols, u32 tm_protocols)
 {
         int rc;
 
-        pr_debug("dev_name=%s protocols=0x%x\n",
-                 dev_name(&dev->dev), protocols);
+        pr_debug("dev_name %s initiator protocols 0x%x target protocols 0x%x\n",
+                 dev_name(&dev->dev), im_protocols, tm_protocols);
 
-        if (!protocols)
+        if (!im_protocols && !tm_protocols)
                 return -EINVAL;
 
         device_lock(&dev->dev);
@@ -143,9 +143,11 @@ int nfc_start_poll(struct nfc_dev *dev, u32 protocols)
                 goto error;
         }
 
-        rc = dev->ops->start_poll(dev, protocols);
-        if (!rc)
+        rc = dev->ops->start_poll(dev, im_protocols, tm_protocols);
+        if (!rc) {
                 dev->polling = true;
+                dev->rf_mode = NFC_RF_NONE;
+        }
 
 error:
         device_unlock(&dev->dev);
@@ -235,8 +237,10 @@ int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
         }
 
         rc = dev->ops->dep_link_up(dev, target, comm_mode, gb, gb_len);
-        if (!rc)
+        if (!rc) {
                 dev->active_target = target;
+                dev->rf_mode = NFC_RF_INITIATOR;
+        }
 
 error:
         device_unlock(&dev->dev);
@@ -264,11 +268,6 @@ int nfc_dep_link_down(struct nfc_dev *dev)
                 goto error;
         }
 
-        if (dev->dep_rf_mode == NFC_RF_TARGET) {
-                rc = -EOPNOTSUPP;
-                goto error;
-        }
-
         rc = dev->ops->dep_link_down(dev);
         if (!rc) {
                 dev->dep_link_up = false;
@@ -286,7 +285,6 @@ int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
                        u8 comm_mode, u8 rf_mode)
 {
         dev->dep_link_up = true;
-        dev->dep_rf_mode = rf_mode;
 
         nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode);
 
@@ -330,6 +328,7 @@ int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol)
         rc = dev->ops->activate_target(dev, target, protocol);
         if (!rc) {
                 dev->active_target = target;
+                dev->rf_mode = NFC_RF_INITIATOR;
 
                 if (dev->ops->check_presence)
                         mod_timer(&dev->check_pres_timer, jiffies +
@@ -409,27 +408,30 @@ int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
                 goto error;
         }
 
-        if (dev->active_target == NULL) {
-                rc = -ENOTCONN;
-                kfree_skb(skb);
-                goto error;
-        }
+        if (dev->rf_mode == NFC_RF_INITIATOR && dev->active_target != NULL) {
+                if (dev->active_target->idx != target_idx) {
+                        rc = -EADDRNOTAVAIL;
+                        kfree_skb(skb);
+                        goto error;
+                }
 
-        if (dev->active_target->idx != target_idx) {
-                rc = -EADDRNOTAVAIL;
+                if (dev->ops->check_presence)
+                        del_timer_sync(&dev->check_pres_timer);
+
+                rc = dev->ops->im_transceive(dev, dev->active_target, skb, cb,
+                                             cb_context);
+
+                if (!rc && dev->ops->check_presence)
+                        mod_timer(&dev->check_pres_timer, jiffies +
+                                  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+        } else if (dev->rf_mode == NFC_RF_TARGET && dev->ops->tm_send != NULL) {
+                rc = dev->ops->tm_send(dev, skb);
+        } else {
+                rc = -ENOTCONN;
                 kfree_skb(skb);
                 goto error;
         }
 
-        if (dev->ops->check_presence)
-                del_timer_sync(&dev->check_pres_timer);
-
-        rc = dev->ops->data_exchange(dev, dev->active_target, skb, cb,
-                                     cb_context);
-
-        if (!rc && dev->ops->check_presence)
-                mod_timer(&dev->check_pres_timer, jiffies +
-                          msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
 
 error:
         device_unlock(&dev->dev);
@@ -447,6 +449,63 @@ int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len)
 }
 EXPORT_SYMBOL(nfc_set_remote_general_bytes);
 
+u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, size_t *gb_len)
+{
+        pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+        return nfc_llcp_general_bytes(dev, gb_len);
+}
+EXPORT_SYMBOL(nfc_get_local_general_bytes);
+
+int nfc_tm_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+        /* Only LLCP target mode for now */
+        if (dev->dep_link_up == false) {
+                kfree_skb(skb);
+                return -ENOLINK;
+        }
+
+        return nfc_llcp_data_received(dev, skb);
+}
+EXPORT_SYMBOL(nfc_tm_data_received);
+
+int nfc_tm_activated(struct nfc_dev *dev, u32 protocol, u8 comm_mode,
+                     u8 *gb, size_t gb_len)
+{
+        int rc;
+
+        device_lock(&dev->dev);
+
+        dev->polling = false;
+
+        if (gb != NULL) {
+                rc = nfc_set_remote_general_bytes(dev, gb, gb_len);
+                if (rc < 0)
+                        goto out;
+        }
+
+        dev->rf_mode = NFC_RF_TARGET;
+
+        if (protocol == NFC_PROTO_NFC_DEP_MASK)
+                nfc_dep_link_is_up(dev, 0, comm_mode, NFC_RF_TARGET);
+
+        rc = nfc_genl_tm_activated(dev, protocol);
+
+out:
+        device_unlock(&dev->dev);
+
+        return rc;
+}
+EXPORT_SYMBOL(nfc_tm_activated);
+
+int nfc_tm_deactivated(struct nfc_dev *dev)
+{
+        dev->dep_link_up = false;
+
+        return nfc_genl_tm_deactivated(dev);
+}
+EXPORT_SYMBOL(nfc_tm_deactivated);
+
 /**
  * nfc_alloc_send_skb - allocate a skb for data exchange responses
  *
@@ -678,7 +737,7 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
         struct nfc_dev *dev;
 
         if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
-            !ops->deactivate_target || !ops->data_exchange)
+            !ops->deactivate_target || !ops->im_transceive)
                 return NULL;
 
         if (!supported_protocols)
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index e1a640d2b58..a8b0b71e8f8 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -481,12 +481,13 @@ static int hci_dev_down(struct nfc_dev *nfc_dev)
         return 0;
 }
 
-static int hci_start_poll(struct nfc_dev *nfc_dev, u32 protocols)
+static int hci_start_poll(struct nfc_dev *nfc_dev,
+                          u32 im_protocols, u32 tm_protocols)
 {
         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
 
         if (hdev->ops->start_poll)
-                return hdev->ops->start_poll(hdev, protocols);
+                return hdev->ops->start_poll(hdev, im_protocols, tm_protocols);
         else
                 return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
                                        NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
@@ -511,9 +512,9 @@ static void hci_deactivate_target(struct nfc_dev *nfc_dev,
 {
 }
 
-static int hci_data_exchange(struct nfc_dev *nfc_dev, struct nfc_target *target,
-                             struct sk_buff *skb, data_exchange_cb_t cb,
-                             void *cb_context)
+static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+                          struct sk_buff *skb, data_exchange_cb_t cb,
+                          void *cb_context)
 {
         struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
         int r;
@@ -579,7 +580,7 @@ static struct nfc_ops hci_nfc_ops = {
         .stop_poll = hci_stop_poll,
         .activate_target = hci_activate_target,
         .deactivate_target = hci_deactivate_target,
-        .data_exchange = hci_data_exchange,
+        .im_transceive = hci_transceive,
         .check_presence = hci_check_presence,
 };
 
diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c
index 5665dc6d893..6b836e6242b 100644
--- a/net/nfc/hci/shdlc.c
+++ b/net/nfc/hci/shdlc.c
@@ -765,14 +765,16 @@ static int nfc_shdlc_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb)
         return 0;
 }
 
-static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev, u32 protocols)
+static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev,
+                                u32 im_protocols, u32 tm_protocols)
 {
         struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev);
 
         pr_debug("\n");
 
         if (shdlc->ops->start_poll)
-                return shdlc->ops->start_poll(shdlc, protocols);
+                return shdlc->ops->start_poll(shdlc,
+                                              im_protocols, tm_protocols);
 
         return 0;
 }
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index bf8ae4f0b90..b982b5b890d 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -51,7 +51,7 @@ static u8 llcp_tlv8(u8 *tlv, u8 type)
         return tlv[2];
 }
 
-static u8 llcp_tlv16(u8 *tlv, u8 type)
+static u16 llcp_tlv16(u8 *tlv, u8 type)
 {
         if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
                 return 0;
@@ -67,7 +67,7 @@ static u8 llcp_tlv_version(u8 *tlv)
 
 static u16 llcp_tlv_miux(u8 *tlv)
 {
-        return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7f;
+        return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
 }
 
 static u16 llcp_tlv_wks(u8 *tlv)
@@ -117,8 +117,8 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
         return tlv;
 }
 
-int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
-                       u8 *tlv_array, u16 tlv_array_len)
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                          u8 *tlv_array, u16 tlv_array_len)
 {
         u8 *tlv = tlv_array, type, length, offset = 0;
 
@@ -149,8 +149,45 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
                 case LLCP_TLV_OPT:
                         local->remote_opt = llcp_tlv_opt(tlv);
                         break;
+                default:
+                        pr_err("Invalid gt tlv value 0x%x\n", type);
+                        break;
+                }
+
+                offset += length + 2;
+                tlv += length + 2;
+        }
+
+        pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
+                 local->remote_version, local->remote_miu,
+                 local->remote_lto, local->remote_opt,
+                 local->remote_wks);
+
+        return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                  u8 *tlv_array, u16 tlv_array_len)
+{
+        u8 *tlv = tlv_array, type, length, offset = 0;
+
+        pr_debug("TLV array length %d\n", tlv_array_len);
+
+        if (sock == NULL)
+                return -ENOTCONN;
+
+        while (offset < tlv_array_len) {
+                type = tlv[0];
+                length = tlv[1];
+
+                pr_debug("type 0x%x length %d\n", type, length);
+
+                switch (type) {
+                case LLCP_TLV_MIUX:
+                        sock->miu = llcp_tlv_miux(tlv) + 128;
+                        break;
                 case LLCP_TLV_RW:
-                        local->remote_rw = llcp_tlv_rw(tlv);
+                        sock->rw = llcp_tlv_rw(tlv);
                         break;
                 case LLCP_TLV_SN:
                         break;
@@ -163,10 +200,7 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
                 tlv += length + 2;
         }
 
-        pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n",
-                 local->remote_version, local->remote_miu,
-                 local->remote_lto, local->remote_opt,
-                 local->remote_wks, local->remote_rw);
+        pr_debug("sock %p rw %d miu %d\n", sock, sock->rw, sock->miu);
 
         return 0;
 }
@@ -474,7 +508,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
         while (remaining_len > 0) {
 
-                frag_len = min_t(size_t, local->remote_miu, remaining_len);
+                frag_len = min_t(size_t, sock->miu, remaining_len);
 
                 pr_debug("Fragment %zd bytes remaining %zd",
                          frag_len, remaining_len);
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 42994fac26d..5d503eeb15a 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -31,47 +31,41 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
 
 static struct list_head llcp_devices;
 
-static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
 {
-        struct nfc_llcp_sock *parent, *s, *n;
-        struct sock *sk, *parent_sk;
-        int i;
-
-        mutex_lock(&local->socket_lock);
-
-        for (i = 0; i < LLCP_MAX_SAP; i++) {
-                parent = local->sockets[i];
-                if (parent == NULL)
-                        continue;
-
-                /* Release all child sockets */
-                list_for_each_entry_safe(s, n, &parent->list, list) {
-                        list_del_init(&s->list);
-                        sk = &s->sk;
-
-                        lock_sock(sk);
-
-                        if (sk->sk_state == LLCP_CONNECTED)
-                                nfc_put_device(s->dev);
+        write_lock(&l->lock);
+        sk_add_node(sk, &l->head);
+        write_unlock(&l->lock);
+}
 
-                        sk->sk_state = LLCP_CLOSED;
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
+{
+        write_lock(&l->lock);
+        sk_del_node_init(sk);
+        write_unlock(&l->lock);
+}
 
-                        release_sock(sk);
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
+{
+        struct sock *sk;
+        struct hlist_node *node, *tmp;
+        struct nfc_llcp_sock *llcp_sock;
 
-                        sock_orphan(sk);
+        write_lock(&local->sockets.lock);
 
-                        s->local = NULL;
-                }
+        sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
+                llcp_sock = nfc_llcp_sock(sk);
 
-                parent_sk = &parent->sk;
+                lock_sock(sk);
 
-                lock_sock(parent_sk);
+                if (sk->sk_state == LLCP_CONNECTED)
+                        nfc_put_device(llcp_sock->dev);
 
-                if (parent_sk->sk_state == LLCP_LISTEN) {
+                if (sk->sk_state == LLCP_LISTEN) {
                         struct nfc_llcp_sock *lsk, *n;
                         struct sock *accept_sk;
 
-                        list_for_each_entry_safe(lsk, n, &parent->accept_queue,
+                        list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
                                                  accept_queue) {
                                 accept_sk = &lsk->sk;
                                 lock_sock(accept_sk);
@@ -83,24 +77,53 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
                                 release_sock(accept_sk);
 
                                 sock_orphan(accept_sk);
-
-                                lsk->local = NULL;
                         }
                 }
 
-                if (parent_sk->sk_state == LLCP_CONNECTED)
-                        nfc_put_device(parent->dev);
-
-                parent_sk->sk_state = LLCP_CLOSED;
+                sk->sk_state = LLCP_CLOSED;
 
-                release_sock(parent_sk);
+                release_sock(sk);
 
-                sock_orphan(parent_sk);
+                sock_orphan(sk);
 
-                parent->local = NULL;
+                sk_del_node_init(sk);
         }
 
-        mutex_unlock(&local->socket_lock);
+        write_unlock(&local->sockets.lock);
+}
+
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
+{
+        kref_get(&local->ref);
+
+        return local;
+}
+
+static void local_release(struct kref *ref)
+{
+        struct nfc_llcp_local *local;
+
+        local = container_of(ref, struct nfc_llcp_local, ref);
+
+        list_del(&local->list);
+        nfc_llcp_socket_release(local);
+        del_timer_sync(&local->link_timer);
+        skb_queue_purge(&local->tx_queue);
+        destroy_workqueue(local->tx_wq);
+        destroy_workqueue(local->rx_wq);
+        destroy_workqueue(local->timeout_wq);
+        kfree_skb(local->rx_pending);
+        kfree(local);
+}
+
+int nfc_llcp_local_put(struct nfc_llcp_local *local)
+{
+        WARN_ON(local == NULL);
+
+        if (local == NULL)
+                return 0;
+
+        return kref_put(&local->ref, local_release);
 }
 
 static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
@@ -384,31 +407,9 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
                 return -EINVAL;
         }
 
-        return nfc_llcp_parse_tlv(local,
-                                  &local->remote_gb[3],
-                                  local->remote_gb_len - 3);
-}
-
-static void nfc_llcp_tx_work(struct work_struct *work)
-{
-        struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                    tx_work);
-        struct sk_buff *skb;
-
-        skb = skb_dequeue(&local->tx_queue);
-        if (skb != NULL) {
-                pr_debug("Sending pending skb\n");
-                print_hex_dump(KERN_DEBUG, "LLCP Tx: ", DUMP_PREFIX_OFFSET,
-                               16, 1, skb->data, skb->len, true);
-
-                nfc_data_exchange(local->dev, local->target_idx,
-                                  skb, nfc_llcp_recv, local);
-        } else {
-                nfc_llcp_send_symm(local->dev);
-        }
-
-        mod_timer(&local->link_timer,
-                  jiffies + msecs_to_jiffies(local->remote_lto));
+        return nfc_llcp_parse_gb_tlv(local,
+                                     &local->remote_gb[3],
+                                     local->remote_gb_len - 3);
 }
 
 static u8 nfc_llcp_dsap(struct sk_buff *pdu)
@@ -443,46 +444,146 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
         sock->recv_ack_n = (sock->recv_n - 1) % 16;
 }
 
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+        struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                    tx_work);
+        struct sk_buff *skb;
+        struct sock *sk;
+        struct nfc_llcp_sock *llcp_sock;
+
+        skb = skb_dequeue(&local->tx_queue);
+        if (skb != NULL) {
+                sk = skb->sk;
+                llcp_sock = nfc_llcp_sock(sk);
+                if (llcp_sock != NULL) {
+                        int ret;
+
+                        pr_debug("Sending pending skb\n");
+                        print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+                                       DUMP_PREFIX_OFFSET, 16, 1,
+                                       skb->data, skb->len, true);
+
+                        ret = nfc_data_exchange(local->dev, local->target_idx,
+                                                skb, nfc_llcp_recv, local);
+
+                        if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+                                skb = skb_get(skb);
+                                skb_queue_tail(&llcp_sock->tx_pending_queue,
+                                               skb);
+                        }
+                } else {
+                        nfc_llcp_send_symm(local->dev);
+                }
+        } else {
+                nfc_llcp_send_symm(local->dev);
+        }
+
+        mod_timer(&local->link_timer,
+                  jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+                                                          u8 ssap)
+{
+        struct sock *sk;
+        struct nfc_llcp_sock *llcp_sock;
+        struct hlist_node *node;
+
+        read_lock(&local->connecting_sockets.lock);
+
+        sk_for_each(sk, node, &local->connecting_sockets.head) {
+                llcp_sock = nfc_llcp_sock(sk);
+
+                if (llcp_sock->ssap == ssap) {
+                        sock_hold(&llcp_sock->sk);
+                        goto out;
+                }
+        }
+
+        llcp_sock = NULL;
+
+out:
+        read_unlock(&local->connecting_sockets.lock);
+
+        return llcp_sock;
+}
+
 static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
                                                u8 ssap, u8 dsap)
 {
-        struct nfc_llcp_sock *sock, *llcp_sock, *n;
+        struct sock *sk;
+        struct hlist_node *node;
+        struct nfc_llcp_sock *llcp_sock;
 
         pr_debug("ssap dsap %d %d\n", ssap, dsap);
 
         if (ssap == 0 && dsap == 0)
                 return NULL;
 
-        mutex_lock(&local->socket_lock);
-        sock = local->sockets[ssap];
-        if (sock == NULL) {
-                mutex_unlock(&local->socket_lock);
-                return NULL;
-        }
+        read_lock(&local->sockets.lock);
 
-        pr_debug("root dsap %d (%d)\n", sock->dsap, dsap);
+        llcp_sock = NULL;
 
-        if (sock->dsap == dsap) {
-                sock_hold(&sock->sk);
-                mutex_unlock(&local->socket_lock);
-                return sock;
+        sk_for_each(sk, node, &local->sockets.head) {
+                llcp_sock = nfc_llcp_sock(sk);
+
+                if (llcp_sock->ssap == ssap &&
+                    llcp_sock->dsap == dsap)
+                        break;
         }
 
-        list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
-                pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
-                         &llcp_sock->sk, llcp_sock->dsap);
-                if (llcp_sock->dsap == dsap) {
-                        sock_hold(&llcp_sock->sk);
-                        mutex_unlock(&local->socket_lock);
-                        return llcp_sock;
-                }
+        read_unlock(&local->sockets.lock);
+
+        if (llcp_sock == NULL)
+                return NULL;
+
+        sock_hold(&llcp_sock->sk);
+
+        return llcp_sock;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+                                                  u8 *sn, size_t sn_len)
+{
+        struct sock *sk;
+        struct hlist_node *node;
+        struct nfc_llcp_sock *llcp_sock;
+
+        pr_debug("sn %zd\n", sn_len);
+
+        if (sn == NULL || sn_len == 0)
+                return NULL;
+
+        read_lock(&local->sockets.lock);
+
+        llcp_sock = NULL;
+
+        sk_for_each(sk, node, &local->sockets.head) {
+                llcp_sock = nfc_llcp_sock(sk);
+
+                if (llcp_sock->sk.sk_state != LLCP_LISTEN)
+                        continue;
+
+                if (llcp_sock->service_name == NULL ||
+                    llcp_sock->service_name_len == 0)
+                        continue;
+
+                if (llcp_sock->service_name_len != sn_len)
+                        continue;
+
+                if (memcmp(sn, llcp_sock->service_name, sn_len) == 0)
+                        break;
         }
 
-        pr_err("Could not find socket for %d %d\n", ssap, dsap);
+        read_unlock(&local->sockets.lock);
 
-        mutex_unlock(&local->socket_lock);
+        if (llcp_sock == NULL)
+                return NULL;
 
-        return NULL;
+        sock_hold(&llcp_sock->sk);
+
+        return llcp_sock;
 }
 
 static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
@@ -518,35 +619,19 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 {
         struct sock *new_sk, *parent;
         struct nfc_llcp_sock *sock, *new_sock;
-        u8 dsap, ssap, bound_sap, reason;
+        u8 dsap, ssap, reason;
 
         dsap = nfc_llcp_dsap(skb);
         ssap = nfc_llcp_ssap(skb);
 
         pr_debug("%d %d\n", dsap, ssap);
 
-        nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
-                           skb->len - LLCP_HEADER_SIZE);
-
         if (dsap != LLCP_SAP_SDP) {
-                bound_sap = dsap;
-
-                mutex_lock(&local->socket_lock);
-                sock = local->sockets[dsap];
-                if (sock == NULL) {
-                        mutex_unlock(&local->socket_lock);
+                sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+                if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
                         reason = LLCP_DM_NOBOUND;
                         goto fail;
                 }
-
-                sock_hold(&sock->sk);
-                mutex_unlock(&local->socket_lock);
-
-                lock_sock(&sock->sk);
-
-                if (sock->dsap == LLCP_SAP_SDP &&
-                    sock->sk.sk_state == LLCP_LISTEN)
-                        goto enqueue;
         } else {
                 u8 *sn;
                 size_t sn_len;
@@ -559,40 +644,15 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 
                 pr_debug("Service name length %zu\n", sn_len);
 
-                mutex_lock(&local->socket_lock);
-                for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
-                     bound_sap++) {
-                        sock = local->sockets[bound_sap];
-                        if (sock == NULL)
-                                continue;
-
-                        if (sock->service_name == NULL ||
-                            sock->service_name_len == 0)
-                                        continue;
-
-                        if (sock->service_name_len != sn_len)
-                                continue;
-
-                        if (sock->dsap == LLCP_SAP_SDP &&
-                            sock->sk.sk_state == LLCP_LISTEN &&
-                            !memcmp(sn, sock->service_name, sn_len)) {
-                                pr_debug("Found service name at SAP %d\n",
-                                         bound_sap);
-                                sock_hold(&sock->sk);
-                                mutex_unlock(&local->socket_lock);
-
-                                lock_sock(&sock->sk);
-
-                                goto enqueue;
-                        }
+                sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+                if (sock == NULL) {
+                        reason = LLCP_DM_NOBOUND;
+                        goto fail;
                 }
-                mutex_unlock(&local->socket_lock);
         }
 
-        reason = LLCP_DM_NOBOUND;
-        goto fail;
+        lock_sock(&sock->sk);
 
-enqueue:
         parent = &sock->sk;
 
         if (sk_acceptq_is_full(parent)) {
@@ -612,15 +672,19 @@ enqueue:
 
         new_sock = nfc_llcp_sock(new_sk);
         new_sock->dev = local->dev;
-        new_sock->local = local;
+        new_sock->local = nfc_llcp_local_get(local);
+        new_sock->miu = local->remote_miu;
         new_sock->nfc_protocol = sock->nfc_protocol;
-        new_sock->ssap = bound_sap;
+        new_sock->ssap = sock->ssap;
         new_sock->dsap = ssap;
         new_sock->parent = parent;
 
+        nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+                                      skb->len - LLCP_HEADER_SIZE);
+
         pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
 
-        list_add_tail(&new_sock->list, &sock->list);
+        nfc_llcp_sock_link(&local->sockets, new_sk);
 
         nfc_llcp_accept_enqueue(&sock->sk, new_sk);
 
@@ -654,12 +718,12 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
 
         pr_debug("Remote ready %d tx queue len %d remote rw %d",
                  sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
-                 local->remote_rw);
+                 sock->rw);
 
         /* Try to queue some I frames for transmission */
         while (sock->remote_ready &&
-               skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) {
-                struct sk_buff *pdu, *pending_pdu;
+               skb_queue_len(&sock->tx_pending_queue) < sock->rw) {
+                struct sk_buff *pdu;
 
                 pdu = skb_dequeue(&sock->tx_queue);
                 if (pdu == NULL)
@@ -668,10 +732,7 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
                 /* Update N(S)/N(R) */
                 nfc_llcp_set_nrns(sock, pdu);
 
-                pending_pdu = skb_clone(pdu, GFP_KERNEL);
-
                 skb_queue_tail(&local->tx_queue, pdu);
-                skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
                 nr_frames++;
         }
 
@@ -728,11 +789,21 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 
                 llcp_sock->send_ack_n = nr;
 
-                skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
-                        if (nfc_llcp_ns(s) <= nr) {
-                                skb_unlink(s, &llcp_sock->tx_pending_queue);
-                                kfree_skb(s);
-                        }
+                /* Remove and free all skbs until ns == nr */
+                skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+                        skb_unlink(s, &llcp_sock->tx_pending_queue);
+                        kfree_skb(s);
+
+                        if (nfc_llcp_ns(s) == nr)
+                                break;
+                }
+
+                /* Re-queue the remaining skbs for transmission */
+                skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+                                            s, tmp) {
+                        skb_unlink(s, &llcp_sock->tx_pending_queue);
+                        skb_queue_head(&local->tx_queue, s);
+                }
         }
 
         if (ptype == LLCP_PDU_RR)
@@ -740,7 +811,7 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
         else if (ptype == LLCP_PDU_RNR)
                 llcp_sock->remote_ready = false;
 
-        if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
+        if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
                 nfc_llcp_send_rr(llcp_sock);
 
         release_sock(sk);
@@ -791,11 +862,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
         dsap = nfc_llcp_dsap(skb);
         ssap = nfc_llcp_ssap(skb);
 
-        llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
-
-        if (llcp_sock == NULL)
-                llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
-
+        llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
         if (llcp_sock == NULL) {
                 pr_err("Invalid CC\n");
                 nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
@@ -803,11 +870,15 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
                 return;
         }
 
-        llcp_sock->dsap = ssap;
         sk = &llcp_sock->sk;
 
-        nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
-                           skb->len - LLCP_HEADER_SIZE);
+        /* Unlink from connecting and link to the client array */
+        nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+        nfc_llcp_sock_link(&local->sockets, sk);
+        llcp_sock->dsap = ssap;
+
+        nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+                                      skb->len - LLCP_HEADER_SIZE);
 
         sk->sk_state = LLCP_CONNECTED;
         sk->sk_state_change(sk);
@@ -891,6 +962,21 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
         return;
 }
 
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+        struct nfc_llcp_local *local;
+
+        local = nfc_llcp_find_local(dev);
+        if (local == NULL)
+                return -ENODEV;
+
+        local->rx_pending = skb_get(skb);
+        del_timer(&local->link_timer);
+        queue_work(local->rx_wq, &local->rx_work);
+
+        return 0;
+}
+
 void nfc_llcp_mac_is_down(struct nfc_dev *dev)
 {
         struct nfc_llcp_local *local;
@@ -943,8 +1029,8 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 
         local->dev = ndev;
         INIT_LIST_HEAD(&local->list);
+        kref_init(&local->ref);
         mutex_init(&local->sdp_lock);
-        mutex_init(&local->socket_lock);
         init_timer(&local->link_timer);
         local->link_timer.data = (unsigned long) local;
         local->link_timer.function = nfc_llcp_symm_timer;
@@ -984,11 +1070,13 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
                 goto err_rx_wq;
         }
 
+        local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
+        local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
+
         nfc_llcp_build_gb(local);
 
         local->remote_miu = LLCP_DEFAULT_MIU;
         local->remote_lto = LLCP_DEFAULT_LTO;
-        local->remote_rw = LLCP_DEFAULT_RW;
 
         list_add(&llcp_devices, &local->list);
 
@@ -1015,14 +1103,7 @@ void nfc_llcp_unregister_device(struct nfc_dev *dev)
                 return;
         }
 
-        list_del(&local->list);
-        nfc_llcp_socket_release(local);
-        del_timer_sync(&local->link_timer);
-        skb_queue_purge(&local->tx_queue);
-        destroy_workqueue(local->tx_wq);
-        destroy_workqueue(local->rx_wq);
-        kfree_skb(local->rx_pending);
-        kfree(local);
+        nfc_llcp_local_put(local);
 }
 
 int __init nfc_llcp_init(void)
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 50680ce5ae4..7286c86982f 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -40,12 +40,18 @@ enum llcp_state {
 
 struct nfc_llcp_sock;
 
+struct llcp_sock_list {
+        struct hlist_head head;
+        rwlock_t          lock;
+};
+
 struct nfc_llcp_local {
         struct list_head list;
         struct nfc_dev *dev;
 
+        struct kref ref;
+
         struct mutex sdp_lock;
-        struct mutex socket_lock;
 
         struct timer_list link_timer;
         struct sk_buff_head tx_queue;
@@ -77,24 +83,26 @@ struct nfc_llcp_local {
         u16 remote_lto;
         u8  remote_opt;
         u16 remote_wks;
-        u8  remote_rw;
 
         /* sockets array */
-        struct nfc_llcp_sock *sockets[LLCP_MAX_SAP];
+        struct llcp_sock_list sockets;
+        struct llcp_sock_list connecting_sockets;
 };
 
 struct nfc_llcp_sock {
         struct sock sk;
-        struct list_head list;
         struct nfc_dev *dev;
         struct nfc_llcp_local *local;
         u32 target_idx;
         u32 nfc_protocol;
 
+        /* Link parameters */
         u8 ssap;
         u8 dsap;
         char *service_name;
         size_t service_name_len;
+        u8 rw;
+        u16 miu;
 
         /* Link variables */
         u8 send_n;
@@ -164,7 +172,11 @@ struct nfc_llcp_sock {
 #define LLCP_DM_REJ     0x03
 
 
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s);
 struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local);
+int nfc_llcp_local_put(struct nfc_llcp_local *local);
 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
                          struct nfc_llcp_sock *sock);
 u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
@@ -179,8 +191,10 @@ void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
 struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
 
 /* TLV API */
-int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
-                       u8 *tlv_array, u16 tlv_array_len);
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                          u8 *tlv_array, u16 tlv_array_len);
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                  u8 *tlv_array, u16 tlv_array_len);
 
 /* Commands API */
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 3f339b19d14..2c0b317344b 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -111,7 +111,7 @@ static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         }
 
         llcp_sock->dev = dev;
-        llcp_sock->local = local;
+        llcp_sock->local = nfc_llcp_local_get(local);
         llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
         llcp_sock->service_name_len = min_t(unsigned int,
                                             llcp_addr.service_name_len,
@@ -124,7 +124,7 @@ static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         if (llcp_sock->ssap == LLCP_MAX_SAP)
                 goto put_dev;
 
-        local->sockets[llcp_sock->ssap] = llcp_sock;
+        nfc_llcp_sock_link(&local->sockets, sk);
 
         pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
 
@@ -292,6 +292,9 @@ static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr,
 
         pr_debug("%p\n", sk);
 
+        if (llcp_sock == NULL)
+                return -EBADFD;
+
         addr->sa_family = AF_NFC;
         *len = sizeof(struct sockaddr_nfc_llcp);
 
@@ -379,15 +382,6 @@ static int llcp_sock_release(struct socket *sock)
                 goto out;
         }
 
-        mutex_lock(&local->socket_lock);
-
-        if (llcp_sock == local->sockets[llcp_sock->ssap])
-                local->sockets[llcp_sock->ssap] = NULL;
-        else
-                list_del_init(&llcp_sock->list);
-
-        mutex_unlock(&local->socket_lock);
-
         lock_sock(sk);
 
         /* Send a DISC */
@@ -412,14 +406,12 @@ static int llcp_sock_release(struct socket *sock)
                 }
         }
 
-        /* Freeing the SAP */
-        if ((sk->sk_state == LLCP_CONNECTED
-             && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) ||
-            sk->sk_state == LLCP_BOUND || sk->sk_state == LLCP_LISTEN)
-                nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
+        nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
 
         release_sock(sk);
 
+        nfc_llcp_sock_unlink(&local->sockets, sk);
+
 out:
         sock_orphan(sk);
         sock_put(sk);
@@ -487,7 +479,8 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
         }
 
         llcp_sock->dev = dev;
-        llcp_sock->local = local;
+        llcp_sock->local = nfc_llcp_local_get(local);
+        llcp_sock->miu = llcp_sock->local->remote_miu;
         llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
         if (llcp_sock->ssap == LLCP_SAP_MAX) {
                 ret = -ENOMEM;
@@ -505,21 +498,26 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
                                           llcp_sock->service_name_len,
                                           GFP_KERNEL);
 
-        local->sockets[llcp_sock->ssap] = llcp_sock;
+        nfc_llcp_sock_link(&local->connecting_sockets, sk);
 
         ret = nfc_llcp_send_connect(llcp_sock);
         if (ret)
-                goto put_dev;
+                goto sock_unlink;
 
         ret = sock_wait_state(sk, LLCP_CONNECTED,
                               sock_sndtimeo(sk, flags & O_NONBLOCK));
         if (ret)
-                goto put_dev;
+                goto sock_unlink;
 
         release_sock(sk);
 
         return 0;
 
+sock_unlink:
+        nfc_llcp_put_ssap(local, llcp_sock->ssap);
+
+        nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+
 put_dev:
         nfc_put_device(dev);
 
@@ -684,13 +682,14 @@ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
 
         llcp_sock->ssap = 0;
         llcp_sock->dsap = LLCP_SAP_SDP;
+        llcp_sock->rw = LLCP_DEFAULT_RW;
+        llcp_sock->miu = LLCP_DEFAULT_MIU;
         llcp_sock->send_n = llcp_sock->send_ack_n = 0;
         llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
         llcp_sock->remote_ready = 1;
         skb_queue_head_init(&llcp_sock->tx_queue);
         skb_queue_head_init(&llcp_sock->tx_pending_queue);
         skb_queue_head_init(&llcp_sock->tx_backlog_queue);
-        INIT_LIST_HEAD(&llcp_sock->list);
         INIT_LIST_HEAD(&llcp_sock->accept_queue);
 
         if (sock != NULL)
@@ -701,8 +700,6 @@ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
 
 void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
 {
-        struct nfc_llcp_local *local = sock->local;
-
         kfree(sock->service_name);
 
         skb_queue_purge(&sock->tx_queue);
@@ -711,12 +708,9 @@ void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
 
         list_del_init(&sock->accept_queue);
 
-        if (local != NULL && sock == local->sockets[sock->ssap])
-                local->sockets[sock->ssap] = NULL;
-        else
-                list_del_init(&sock->list);
-
         sock->parent = NULL;
+
+        nfc_llcp_local_put(sock->local);
 }
 
 static int llcp_sock_create(struct net *net, struct socket *sock,
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index d560e6f1307..766a02b1dfa 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -387,7 +387,8 @@ static int nci_dev_down(struct nfc_dev *nfc_dev)
         return nci_close_device(ndev);
 }
 
-static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 protocols)
+static int nci_start_poll(struct nfc_dev *nfc_dev,
+                          __u32 im_protocols, __u32 tm_protocols)
 {
         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
         int rc;
@@ -413,11 +414,11 @@ static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 protocols)
                         return -EBUSY;
         }
 
-        rc = nci_request(ndev, nci_rf_discover_req, protocols,
+        rc = nci_request(ndev, nci_rf_discover_req, im_protocols,
                          msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
 
         if (!rc)
-                ndev->poll_prots = protocols;
+                ndev->poll_prots = im_protocols;
 
         return rc;
 }
@@ -521,9 +522,9 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev,
         }
 }
 
-static int nci_data_exchange(struct nfc_dev *nfc_dev, struct nfc_target *target,
-                             struct sk_buff *skb,
-                             data_exchange_cb_t cb, void *cb_context)
+static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+                          struct sk_buff *skb,
+                          data_exchange_cb_t cb, void *cb_context)
 {
         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
         int rc;
@@ -556,7 +557,7 @@ static struct nfc_ops nci_nfc_ops = {
         .stop_poll = nci_stop_poll,
         .activate_target = nci_activate_target,
         .deactivate_target = nci_deactivate_target,
-        .data_exchange = nci_data_exchange,
+        .im_transceive = nci_transceive,
 };
 
 /* ---- Interface to NCI drivers ---- */
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 581d419083a..03c31db38f1 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -49,6 +49,8 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
         [NFC_ATTR_COMM_MODE] = { .type = NLA_U8 },
         [NFC_ATTR_RF_MODE] = { .type = NLA_U8 },
         [NFC_ATTR_DEVICE_POWERED] = { .type = NLA_U8 },
+        [NFC_ATTR_IM_PROTOCOLS] = { .type = NLA_U32 },
+        [NFC_ATTR_TM_PROTOCOLS] = { .type = NLA_U32 },
 };
 
 static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target,
@@ -219,6 +221,68 @@ free_msg:
         return -EMSGSIZE;
 }
 
+int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol)
+{
+        struct sk_buff *msg;
+        void *hdr;
+
+        msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+        if (!msg)
+                return -ENOMEM;
+
+        hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+                          NFC_EVENT_TM_ACTIVATED);
+        if (!hdr)
+                goto free_msg;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+                goto nla_put_failure;
+        if (nla_put_u32(msg, NFC_ATTR_TM_PROTOCOLS, protocol))
+                goto nla_put_failure;
+
+        genlmsg_end(msg, hdr);
+
+        genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL);
+
+        return 0;
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+free_msg:
+        nlmsg_free(msg);
+        return -EMSGSIZE;
+}
+
+int nfc_genl_tm_deactivated(struct nfc_dev *dev)
+{
+        struct sk_buff *msg;
+        void *hdr;
+
+        msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+        if (!msg)
+                return -ENOMEM;
+
+        hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+                          NFC_EVENT_TM_DEACTIVATED);
+        if (!hdr)
+                goto free_msg;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+                goto nla_put_failure;
+
+        genlmsg_end(msg, hdr);
+
+        genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL);
+
+        return 0;
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+free_msg:
+        nlmsg_free(msg);
+        return -EMSGSIZE;
+}
+
 int nfc_genl_device_added(struct nfc_dev *dev)
 {
         struct sk_buff *msg;
@@ -519,16 +583,25 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
         struct nfc_dev *dev;
         int rc;
         u32 idx;
-        u32 protocols;
+        u32 im_protocols = 0, tm_protocols = 0;
 
         pr_debug("Poll start\n");
 
         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
-            !info->attrs[NFC_ATTR_PROTOCOLS])
+            ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
+              !info->attrs[NFC_ATTR_PROTOCOLS]) &&
+             !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
                 return -EINVAL;
 
         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
-        protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
+
+        if (info->attrs[NFC_ATTR_TM_PROTOCOLS])
+                tm_protocols = nla_get_u32(info->attrs[NFC_ATTR_TM_PROTOCOLS]);
+
+        if (info->attrs[NFC_ATTR_IM_PROTOCOLS])
+                im_protocols = nla_get_u32(info->attrs[NFC_ATTR_IM_PROTOCOLS]);
+        else if (info->attrs[NFC_ATTR_PROTOCOLS])
+                im_protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
 
         dev = nfc_get_device(idx);
         if (!dev)
@@ -536,7 +609,7 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
 
         mutex_lock(&dev->genl_data.genl_data_mutex);
 
-        rc = nfc_start_poll(dev, protocols);
+        rc = nfc_start_poll(dev, im_protocols, tm_protocols);
         if (!rc)
                 dev->genl_data.poll_req_pid = info->snd_pid;
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 3dd4232ae66..c5e42b79a41 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -55,6 +55,7 @@ int nfc_llcp_register_device(struct nfc_dev *dev);
 void nfc_llcp_unregister_device(struct nfc_dev *dev);
 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len);
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
 int __init nfc_llcp_init(void);
 void nfc_llcp_exit(void);
 
@@ -90,6 +91,12 @@ static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *gb_len)
         return NULL;
 }
 
+static inline int nfc_llcp_data_received(struct nfc_dev *dev,
+                                         struct sk_buff *skb)
+{
+        return 0;
+}
+
 static inline int nfc_llcp_init(void)
 {
         return 0;
@@ -128,6 +135,9 @@ int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
                                u8 comm_mode, u8 rf_mode);
 int nfc_genl_dep_link_down_event(struct nfc_dev *dev);
 
+int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol);
+int nfc_genl_tm_deactivated(struct nfc_dev *dev);
+
 struct nfc_dev *nfc_get_device(unsigned int idx);
 
 static inline void nfc_put_device(struct nfc_dev *dev)
@@ -158,7 +168,7 @@ int nfc_dev_up(struct nfc_dev *dev);
 
 int nfc_dev_down(struct nfc_dev *dev);
 
-int nfc_start_poll(struct nfc_dev *dev, u32 protocols);
+int nfc_start_poll(struct nfc_dev *dev, u32 im_protocols, u32 tm_protocols);
 
 int nfc_stop_poll(struct nfc_dev *dev);
 
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index 15f347477a9..baf5704740e 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -1389,7 +1389,7 @@ static void reg_set_request_processed(void)
         spin_unlock(&reg_requests_lock);
 
         if (last_request->initiator == NL80211_REGDOM_SET_BY_USER)
-                cancel_delayed_work_sync(&reg_timeout);
+                cancel_delayed_work(&reg_timeout);
 
         if (need_more_processing)
                 schedule_work(&reg_work);
diff --git a/net/wireless/util.c b/net/wireless/util.c
index 8f2d68fc3a4..316cfd00914 100644
--- a/net/wireless/util.c
+++ b/net/wireless/util.c
@@ -804,7 +804,7 @@ int cfg80211_change_iface(struct cfg80211_registered_device *rdev,
              ntype == NL80211_IFTYPE_P2P_CLIENT))
                 return -EBUSY;
 
-        if (ntype != otype) {
+        if (ntype != otype && netif_running(dev)) {
                 err = cfg80211_can_change_interface(rdev, dev->ieee80211_ptr,
                                                     ntype);
                 if (err)