Merge remote-tracking branch 'wireless-next/master' into mac80211-next

175 files changed, 8252 insertions, 2951 deletions

diff --git a/MAINTAINERS b/MAINTAINERS
index e73060fe0788..568ea9373091 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -7477,6 +7477,12 @@ S:	Maintained
 F:      Documentation/usb/acm.txt
 F:      drivers/usb/class/cdc-acm.*
 
+USB AR5523 WIRELESS DRIVER
+M:      Pontus Fuchs <pontus.fuchs@gmail.com>
+L:      linux-wireless@vger.kernel.org
+S:      Maintained
+F:      drivers/net/wireless/ath/ar5523/
+
 USB ATTACHED SCSI
 M:      Matthew Wilcox <willy@linux.intel.com>
 M:      Sarah Sharp <sarah.a.sharp@linux.intel.com>
diff --git a/arch/mips/bcm47xx/nvram.c b/arch/mips/bcm47xx/nvram.c
index d43ceff5be47..48a4c70b3842 100644
--- a/arch/mips/bcm47xx/nvram.c
+++ b/arch/mips/bcm47xx/nvram.c
@@ -43,8 +43,8 @@ static void early_nvram_init(void)
 #ifdef CONFIG_BCM47XX_SSB
         case BCM47XX_BUS_TYPE_SSB:
                 mcore_ssb = &bcm47xx_bus.ssb.mipscore;
-                base = mcore_ssb->flash_window;
-                lim = mcore_ssb->flash_window_size;
+                base = mcore_ssb->pflash.window;
+                lim = mcore_ssb->pflash.window_size;
                 break;
 #endif
 #ifdef CONFIG_BCM47XX_BCMA
diff --git a/arch/mips/bcm47xx/wgt634u.c b/arch/mips/bcm47xx/wgt634u.c
index e9f9ec8d443b..e80d585731aa 100644
--- a/arch/mips/bcm47xx/wgt634u.c
+++ b/arch/mips/bcm47xx/wgt634u.c
@@ -156,10 +156,10 @@ static int __init wgt634u_init(void)
                                             SSB_CHIPCO_IRQ_GPIO);
                 }
 
-                wgt634u_flash_data.width = mcore->flash_buswidth;
-                wgt634u_flash_resource.start = mcore->flash_window;
-                wgt634u_flash_resource.end = mcore->flash_window
-                                           + mcore->flash_window_size
+                wgt634u_flash_data.width = mcore->pflash.buswidth;
+                wgt634u_flash_resource.start = mcore->pflash.window;
+                wgt634u_flash_resource.end = mcore->pflash.window
+                                           + mcore->pflash.window_size
                                            - 1;
                 return platform_add_devices(wgt634u_devices,
                                             ARRAY_SIZE(wgt634u_devices));
diff --git a/drivers/bcma/driver_chipcommon.c b/drivers/bcma/driver_chipcommon.c
index a4c3ebcc4c86..ffd74e51f02d 100644
--- a/drivers/bcma/driver_chipcommon.c
+++ b/drivers/bcma/driver_chipcommon.c
@@ -22,12 +22,9 @@ static inline u32 bcma_cc_write32_masked(struct bcma_drv_cc *cc, u16 offset,
         return value;
 }
 
-void bcma_core_chipcommon_init(struct bcma_drv_cc *cc)
+void bcma_core_chipcommon_early_init(struct bcma_drv_cc *cc)
 {
-        u32 leddc_on = 10;
-        u32 leddc_off = 90;
-
-        if (cc->setup_done)
+        if (cc->early_setup_done)
                 return;
 
         if (cc->core->id.rev >= 11)
@@ -36,6 +33,22 @@ void bcma_core_chipcommon_init(struct bcma_drv_cc *cc)
         if (cc->core->id.rev >= 35)
                 cc->capabilities_ext = bcma_cc_read32(cc, BCMA_CC_CAP_EXT);
 
+        if (cc->capabilities & BCMA_CC_CAP_PMU)
+                bcma_pmu_early_init(cc);
+
+        cc->early_setup_done = true;
+}
+
+void bcma_core_chipcommon_init(struct bcma_drv_cc *cc)
+{
+        u32 leddc_on = 10;
+        u32 leddc_off = 90;
+
+        if (cc->setup_done)
+                return;
+
+        bcma_core_chipcommon_early_init(cc);
+
         if (cc->core->id.rev >= 20) {
                 bcma_cc_write32(cc, BCMA_CC_GPIOPULLUP, 0);
                 bcma_cc_write32(cc, BCMA_CC_GPIOPULLDOWN, 0);
diff --git a/drivers/bcma/driver_chipcommon_nflash.c b/drivers/bcma/driver_chipcommon_nflash.c
index 9042781edec3..dbda91e4dff5 100644
--- a/drivers/bcma/driver_chipcommon_nflash.c
+++ b/drivers/bcma/driver_chipcommon_nflash.c
@@ -32,6 +32,9 @@ int bcma_nflash_init(struct bcma_drv_cc *cc)
         }
 
         cc->nflash.present = true;
+        if (cc->core->id.rev == 38 &&
+            (cc->status & BCMA_CC_CHIPST_5357_NAND_BOOT))
+                cc->nflash.boot = true;
 
         /* Prepare platform device, but don't register it yet. It's too early,
          * malloc (required by device_private_init) is not available yet. */
diff --git a/drivers/bcma/driver_chipcommon_pmu.c b/drivers/bcma/driver_chipcommon_pmu.c
index 201faf106b3f..a63ddd9c70eb 100644
--- a/drivers/bcma/driver_chipcommon_pmu.c
+++ b/drivers/bcma/driver_chipcommon_pmu.c
@@ -144,7 +144,7 @@ static void bcma_pmu_workarounds(struct bcma_drv_cc *cc)
         }
 }
 
-void bcma_pmu_init(struct bcma_drv_cc *cc)
+void bcma_pmu_early_init(struct bcma_drv_cc *cc)
 {
         u32 pmucap;
 
@@ -153,7 +153,10 @@ void bcma_pmu_init(struct bcma_drv_cc *cc)
 
         bcma_debug(cc->core->bus, "Found rev %u PMU (capabilities 0x%08X)\n",
                    cc->pmu.rev, pmucap);
+}
 
+void bcma_pmu_init(struct bcma_drv_cc *cc)
+{
         if (cc->pmu.rev == 1)
                 bcma_cc_mask32(cc, BCMA_CC_PMU_CTL,
                               ~BCMA_CC_PMU_CTL_NOILPONW);
diff --git a/drivers/bcma/driver_chipcommon_sflash.c b/drivers/bcma/driver_chipcommon_sflash.c
index 2c4eec2ca5a0..63e688393825 100644
--- a/drivers/bcma/driver_chipcommon_sflash.c
+++ b/drivers/bcma/driver_chipcommon_sflash.c
@@ -12,7 +12,7 @@
 
 static struct resource bcma_sflash_resource = {
         .name   = "bcma_sflash",
-        .start  = BCMA_SFLASH,
+        .start  = BCMA_SOC_FLASH2,
         .end    = 0,
         .flags  = IORESOURCE_MEM | IORESOURCE_READONLY,
 };
@@ -31,15 +31,42 @@ struct bcma_sflash_tbl_e {
 };
 
 static struct bcma_sflash_tbl_e bcma_sflash_st_tbl[] = {
-        { "", 0x14, 0x10000, 32, },
+        { "M25P20", 0x11, 0x10000, 4, },
+        { "M25P40", 0x12, 0x10000, 8, },
+
+        { "M25P16", 0x14, 0x10000, 32, },
+        { "M25P32", 0x14, 0x10000, 64, },
+        { "M25P64", 0x16, 0x10000, 128, },
+        { "M25FL128", 0x17, 0x10000, 256, },
         { 0 },
 };
 
 static struct bcma_sflash_tbl_e bcma_sflash_sst_tbl[] = {
+        { "SST25WF512", 1, 0x1000, 16, },
+        { "SST25VF512", 0x48, 0x1000, 16, },
+        { "SST25WF010", 2, 0x1000, 32, },
+        { "SST25VF010", 0x49, 0x1000, 32, },
+        { "SST25WF020", 3, 0x1000, 64, },
+        { "SST25VF020", 0x43, 0x1000, 64, },
+        { "SST25WF040", 4, 0x1000, 128, },
+        { "SST25VF040", 0x44, 0x1000, 128, },
+        { "SST25VF040B", 0x8d, 0x1000, 128, },
+        { "SST25WF080", 5, 0x1000, 256, },
+        { "SST25VF080B", 0x8e, 0x1000, 256, },
+        { "SST25VF016", 0x41, 0x1000, 512, },
+        { "SST25VF032", 0x4a, 0x1000, 1024, },
+        { "SST25VF064", 0x4b, 0x1000, 2048, },
         { 0 },
 };
 
 static struct bcma_sflash_tbl_e bcma_sflash_at_tbl[] = {
+        { "AT45DB011", 0xc, 256, 512, },
+        { "AT45DB021", 0x14, 256, 1024, },
+        { "AT45DB041", 0x1c, 256, 2048, },
+        { "AT45DB081", 0x24, 256, 4096, },
+        { "AT45DB161", 0x2c, 512, 4096, },
+        { "AT45DB321", 0x34, 512, 8192, },
+        { "AT45DB642", 0x3c, 1024, 8192, },
         { 0 },
 };
 
@@ -84,6 +111,8 @@ int bcma_sflash_init(struct bcma_drv_cc *cc)
                                         break;
                         }
                         break;
+                case 0x13:
+                        return -ENOTSUPP;
                 default:
                         for (e = bcma_sflash_st_tbl; e->name; e++) {
                                 if (e->id == id)
@@ -116,7 +145,7 @@ int bcma_sflash_init(struct bcma_drv_cc *cc)
                 return -ENOTSUPP;
         }
 
-        sflash->window = BCMA_SFLASH;
+        sflash->window = BCMA_SOC_FLASH2;
         sflash->blocksize = e->blocksize;
         sflash->numblocks = e->numblocks;
         sflash->size = sflash->blocksize * sflash->numblocks;
diff --git a/drivers/bcma/driver_mips.c b/drivers/bcma/driver_mips.c
index cc65b45b4368..170822ea51c7 100644
--- a/drivers/bcma/driver_mips.c
+++ b/drivers/bcma/driver_mips.c
@@ -181,47 +181,66 @@ EXPORT_SYMBOL(bcma_cpu_clock);
 static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 {
         struct bcma_bus *bus = mcore->core->bus;
+        struct bcma_drv_cc *cc = &bus->drv_cc;
 
-        switch (bus->drv_cc.capabilities & BCMA_CC_CAP_FLASHT) {
+        switch (cc->capabilities & BCMA_CC_CAP_FLASHT) {
         case BCMA_CC_FLASHT_STSER:
         case BCMA_CC_FLASHT_ATSER:
                 bcma_debug(bus, "Found serial flash\n");
-                bcma_sflash_init(&bus->drv_cc);
+                bcma_sflash_init(cc);
                 break;
         case BCMA_CC_FLASHT_PARA:
                 bcma_debug(bus, "Found parallel flash\n");
-                bus->drv_cc.pflash.window = 0x1c000000;
-                bus->drv_cc.pflash.window_size = 0x02000000;
+                cc->pflash.present = true;
+                cc->pflash.window = BCMA_SOC_FLASH2;
+                cc->pflash.window_size = BCMA_SOC_FLASH2_SZ;
 
-                if ((bcma_read32(bus->drv_cc.core, BCMA_CC_FLASH_CFG) &
+                if ((bcma_read32(cc->core, BCMA_CC_FLASH_CFG) &
                      BCMA_CC_FLASH_CFG_DS) == 0)
-                        bus->drv_cc.pflash.buswidth = 1;
+                        cc->pflash.buswidth = 1;
                 else
-                        bus->drv_cc.pflash.buswidth = 2;
+                        cc->pflash.buswidth = 2;
                 break;
         default:
                 bcma_err(bus, "Flash type not supported\n");
         }
 
-        if (bus->drv_cc.core->id.rev == 38 ||
+        if (cc->core->id.rev == 38 ||
             bus->chipinfo.id == BCMA_CHIP_ID_BCM4706) {
-                if (bus->drv_cc.capabilities & BCMA_CC_CAP_NFLASH) {
+                if (cc->capabilities & BCMA_CC_CAP_NFLASH) {
                         bcma_debug(bus, "Found NAND flash\n");
-                        bcma_nflash_init(&bus->drv_cc);
+                        bcma_nflash_init(cc);
                 }
         }
 }
 
+void bcma_core_mips_early_init(struct bcma_drv_mips *mcore)
+{
+        struct bcma_bus *bus = mcore->core->bus;
+
+        if (mcore->early_setup_done)
+                return;
+
+        bcma_chipco_serial_init(&bus->drv_cc);
+        bcma_core_mips_flash_detect(mcore);
+
+        mcore->early_setup_done = true;
+}
+
 void bcma_core_mips_init(struct bcma_drv_mips *mcore)
 {
         struct bcma_bus *bus;
         struct bcma_device *core;
         bus = mcore->core->bus;
 
+        if (mcore->setup_done)
+                return;
+
         bcma_info(bus, "Initializing MIPS core...\n");
 
-        if (!mcore->setup_done)
-                mcore->assigned_irqs = 1;
+        bcma_core_mips_early_init(mcore);
+
+        mcore->assigned_irqs = 1;
 
         /* Assign IRQs to all cores on the bus */
         list_for_each_entry(core, &bus->cores, list) {
@@ -256,10 +275,5 @@ void bcma_core_mips_init(struct bcma_drv_mips *mcore)
         bcma_info(bus, "IRQ reconfiguration done\n");
         bcma_core_mips_dump_irq(bus);
 
-        if (mcore->setup_done)
-                return;
-
-        bcma_chipco_serial_init(&bus->drv_cc);
-        bcma_core_mips_flash_detect(mcore);
         mcore->setup_done = true;
 }
diff --git a/drivers/bcma/driver_pci_host.c b/drivers/bcma/driver_pci_host.c
index 9baf886e82df..e56449506695 100644
--- a/drivers/bcma/driver_pci_host.c
+++ b/drivers/bcma/driver_pci_host.c
@@ -35,11 +35,6 @@ bool __devinit bcma_core_pci_is_in_hostmode(struct bcma_drv_pci *pc)
             chipid_top != 0x5300)
                 return false;
 
-        if (bus->sprom.boardflags_lo & BCMA_CORE_PCI_BFL_NOPCI) {
-                bcma_info(bus, "This PCI core is disabled and not working\n");
-                return false;
-        }
-
         bcma_core_enable(pc->core, 0);
 
         return !mips_busprobe32(tmp, pc->core->io_addr);
@@ -396,6 +391,11 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
 
         bcma_info(bus, "PCIEcore in host mode found\n");
 
+        if (bus->sprom.boardflags_lo & BCMA_CORE_PCI_BFL_NOPCI) {
+                bcma_info(bus, "This PCIE core is disabled and not working\n");
+                return;
+        }
+
         pc_host = kzalloc(sizeof(*pc_host), GFP_KERNEL);
         if (!pc_host)  {
                 bcma_err(bus, "can not allocate memory");
@@ -452,6 +452,8 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
                         pc_host->mem_resource.start = BCMA_SOC_PCI_MEM;
                         pc_host->mem_resource.end = BCMA_SOC_PCI_MEM +
                                                     BCMA_SOC_PCI_MEM_SZ - 1;
+                        pc_host->io_resource.start = 0x100;
+                        pc_host->io_resource.end = 0x47F;
                         pci_membase_1G = BCMA_SOC_PCIE_DMA_H32;
                         pcicore_write32(pc, BCMA_CORE_PCI_SBTOPCI0,
                                         tmp | BCMA_SOC_PCI_MEM);
@@ -459,6 +461,8 @@ void __devinit bcma_core_pci_hostmode_init(struct bcma_drv_pci *pc)
                         pc_host->mem_resource.start = BCMA_SOC_PCI1_MEM;
                         pc_host->mem_resource.end = BCMA_SOC_PCI1_MEM +
                                                     BCMA_SOC_PCI_MEM_SZ - 1;
+                        pc_host->io_resource.start = 0x480;
+                        pc_host->io_resource.end = 0x7FF;
                         pci_membase_1G = BCMA_SOC_PCIE1_DMA_H32;
                         pc_host->host_cfg_addr = BCMA_SOC_PCI1_CFG;
                         pcicore_write32(pc, BCMA_CORE_PCI_SBTOPCI0,
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index b6b4b5ebd4c2..98fdc3e014e7 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -238,7 +238,7 @@ static void __devexit bcma_host_pci_remove(struct pci_dev *dev)
         pci_set_drvdata(dev, NULL);
 }
 
-#ifdef CONFIG_PM
+#ifdef CONFIG_PM_SLEEP
 static int bcma_host_pci_suspend(struct device *dev)
 {
         struct pci_dev *pdev = to_pci_dev(dev);
@@ -261,11 +261,11 @@ static SIMPLE_DEV_PM_OPS(bcma_pm_ops, bcma_host_pci_suspend,
                          bcma_host_pci_resume);
 #define BCMA_PM_OPS     (&bcma_pm_ops)
 
-#else /* CONFIG_PM */
+#else /* CONFIG_PM_SLEEP */
 
 #define BCMA_PM_OPS     NULL
 
-#endif /* CONFIG_PM */
+#endif /* CONFIG_PM_SLEEP */
 
 static DEFINE_PCI_DEVICE_TABLE(bcma_pci_bridge_tbl) = {
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x0576) },
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 432aeeedfd5e..a9718893000b 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -81,6 +81,18 @@ struct bcma_device *bcma_find_core(struct bcma_bus *bus, u16 coreid)
 }
 EXPORT_SYMBOL_GPL(bcma_find_core);
 
+static struct bcma_device *bcma_find_core_unit(struct bcma_bus *bus, u16 coreid,
+                                               u8 unit)
+{
+        struct bcma_device *core;
+
+        list_for_each_entry(core, &bus->cores, list) {
+                if (core->id.id == coreid && core->core_unit == unit)
+                        return core;
+        }
+        return NULL;
+}
+
 static void bcma_release_core_dev(struct device *dev)
 {
         struct bcma_device *core = container_of(dev, struct bcma_device, dev);
@@ -158,9 +170,10 @@ static int bcma_register_cores(struct bcma_bus *bus)
 
 static void bcma_unregister_cores(struct bcma_bus *bus)
 {
-        struct bcma_device *core;
+        struct bcma_device *core, *tmp;
 
-        list_for_each_entry(core, &bus->cores, list) {
+        list_for_each_entry_safe(core, tmp, &bus->cores, list) {
+                list_del(&core->list);
                 if (core->dev_registered)
                         device_unregister(&core->dev);
         }
@@ -182,6 +195,20 @@ int __devinit bcma_bus_register(struct bcma_bus *bus)
                 return -1;
         }
 
+        /* Early init CC core */
+        core = bcma_find_core(bus, bcma_cc_core_id(bus));
+        if (core) {
+                bus->drv_cc.core = core;
+                bcma_core_chipcommon_early_init(&bus->drv_cc);
+        }
+
+        /* Try to get SPROM */
+        err = bcma_sprom_get(bus);
+        if (err == -ENOENT) {
+                bcma_err(bus, "No SPROM available\n");
+        } else if (err)
+                bcma_err(bus, "Failed to get SPROM: %d\n", err);
+
         /* Init CC core */
         core = bcma_find_core(bus, bcma_cc_core_id(bus));
         if (core) {
@@ -197,10 +224,17 @@ int __devinit bcma_bus_register(struct bcma_bus *bus)
         }
 
         /* Init PCIE core */
-        core = bcma_find_core(bus, BCMA_CORE_PCIE);
+        core = bcma_find_core_unit(bus, BCMA_CORE_PCIE, 0);
+        if (core) {
+                bus->drv_pci[0].core = core;
+                bcma_core_pci_init(&bus->drv_pci[0]);
+        }
+
+        /* Init PCIE core */
+        core = bcma_find_core_unit(bus, BCMA_CORE_PCIE, 1);
         if (core) {
-                bus->drv_pci.core = core;
-                bcma_core_pci_init(&bus->drv_pci);
+                bus->drv_pci[1].core = core;
+                bcma_core_pci_init(&bus->drv_pci[1]);
         }
 
         /* Init GBIT MAC COMMON core */
@@ -210,13 +244,6 @@ int __devinit bcma_bus_register(struct bcma_bus *bus)
                 bcma_core_gmac_cmn_init(&bus->drv_gmac_cmn);
         }
 
-        /* Try to get SPROM */
-        err = bcma_sprom_get(bus);
-        if (err == -ENOENT) {
-                bcma_err(bus, "No SPROM available\n");
-        } else if (err)
-                bcma_err(bus, "Failed to get SPROM: %d\n", err);
-
         /* Register found cores */
         bcma_register_cores(bus);
 
@@ -274,18 +301,18 @@ int __init bcma_bus_early_register(struct bcma_bus *bus,
                 return -1;
         }
 
-        /* Init CC core */
+        /* Early init CC core */
         core = bcma_find_core(bus, bcma_cc_core_id(bus));
         if (core) {
                 bus->drv_cc.core = core;
-                bcma_core_chipcommon_init(&bus->drv_cc);
+                bcma_core_chipcommon_early_init(&bus->drv_cc);
         }
 
-        /* Init MIPS core */
+        /* Early init MIPS core */
         core = bcma_find_core(bus, BCMA_CORE_MIPS_74K);
         if (core) {
                 bus->drv_mips.core = core;
-                bcma_core_mips_init(&bus->drv_mips);
+                bcma_core_mips_early_init(&bus->drv_mips);
         }
 
         bcma_info(bus, "Early bus registered\n");
diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c
index 0d546b64be34..4adf9ef9a113 100644
--- a/drivers/bcma/sprom.c
+++ b/drivers/bcma/sprom.c
@@ -595,8 +595,11 @@ int bcma_sprom_get(struct bcma_bus *bus)
                 bcma_chipco_bcm4331_ext_pa_lines_ctl(&bus->drv_cc, true);
 
         err = bcma_sprom_valid(sprom);
-        if (err)
+        if (err) {
+                bcma_warn(bus, "invalid sprom read from the PCIe card, try to use fallback sprom\n");
+                err = bcma_fill_sprom_with_fallback(bus, &bus->sprom);
                 goto out;
+        }
 
         bcma_sprom_extract_r8(bus, sprom);
 
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 3f4bfc814dc7..9959d4cb23dc 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -492,7 +492,7 @@ done:
 static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 {
         u16 buf_len = 0;
-        int ret, buf_block_len, blksz;
+        int ret, num_blocks, blksz;
         struct sk_buff *skb = NULL;
         u32 type;
         u8 *payload = NULL;
@@ -514,18 +514,17 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
         }
 
         blksz = SDIO_BLOCK_SIZE;
-        buf_block_len = (buf_len + blksz - 1) / blksz;
+        num_blocks = DIV_ROUND_UP(buf_len, blksz);
 
         if (buf_len <= SDIO_HEADER_LEN
-                        || (buf_block_len * blksz) > ALLOC_BUF_SIZE) {
+            || (num_blocks * blksz) > ALLOC_BUF_SIZE) {
                 BT_ERR("invalid packet length: %d", buf_len);
                 ret = -EINVAL;
                 goto exit;
         }
 
         /* Allocate buffer */
-        skb = bt_skb_alloc(buf_block_len * blksz + BTSDIO_DMA_ALIGN,
-                                                                GFP_ATOMIC);
+        skb = bt_skb_alloc(num_blocks * blksz + BTSDIO_DMA_ALIGN, GFP_ATOMIC);
         if (skb == NULL) {
                 BT_ERR("No free skb");
                 goto exit;
@@ -541,7 +540,7 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
         payload = skb->data;
 
         ret = sdio_readsb(card->func, payload, card->ioport,
-                          buf_block_len * blksz);
+                          num_blocks * blksz);
         if (ret < 0) {
                 BT_ERR("readsb failed: %d", ret);
                 ret = -EIO;
@@ -553,7 +552,16 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
          */
 
         buf_len = payload[0];
-        buf_len |= (u16) payload[1] << 8;
+        buf_len |= payload[1] << 8;
+        buf_len |= payload[2] << 16;
+
+        if (buf_len > blksz * num_blocks) {
+                BT_ERR("Skip incorrect packet: hdrlen %d buffer %d",
+                       buf_len, blksz * num_blocks);
+                ret = -EIO;
+                goto exit;
+        }
+
         type = payload[3];
 
         switch (type) {
@@ -589,8 +597,7 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 
         default:
                 BT_ERR("Unknown packet type:%d", type);
-                print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, payload,
-                                                blksz * buf_block_len);
+                BT_ERR("hex: %*ph", blksz * num_blocks, payload);
 
                 kfree_skb(skb);
                 skb = NULL;
@@ -849,8 +856,7 @@ static int btmrvl_sdio_host_to_card(struct btmrvl_private *priv,
                 if (ret < 0) {
                         i++;
                         BT_ERR("i=%d writesb failed: %d", i, ret);
-                        print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
-                                                payload, nb);
+                        BT_ERR("hex: %*ph", nb, payload);
                         ret = -EIO;
                         if (i > MAX_WRITE_IOMEM_RETRY)
                                 goto exit;
diff --git a/drivers/net/wireless/ath/Kconfig b/drivers/net/wireless/ath/Kconfig
index 09602241901b..c25dcf192fec 100644
--- a/drivers/net/wireless/ath/Kconfig
+++ b/drivers/net/wireless/ath/Kconfig
@@ -26,5 +26,6 @@ source "drivers/net/wireless/ath/ath5k/Kconfig"
 source "drivers/net/wireless/ath/ath9k/Kconfig"
 source "drivers/net/wireless/ath/carl9170/Kconfig"
 source "drivers/net/wireless/ath/ath6kl/Kconfig"
+source "drivers/net/wireless/ath/ar5523/Kconfig"
 
 endif
diff --git a/drivers/net/wireless/ath/Makefile b/drivers/net/wireless/ath/Makefile
index d716b748e574..1e18621326dc 100644
--- a/drivers/net/wireless/ath/Makefile
+++ b/drivers/net/wireless/ath/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_ATH5K)		+= ath5k/
 obj-$(CONFIG_ATH9K_HW)          += ath9k/
 obj-$(CONFIG_CARL9170)          += carl9170/
 obj-$(CONFIG_ATH6KL)            += ath6kl/
+obj-$(CONFIG_AR5523)            += ar5523/
 
 obj-$(CONFIG_ATH_COMMON)        += ath.o
 
diff --git a/drivers/net/wireless/ath/ar5523/Kconfig b/drivers/net/wireless/ath/ar5523/Kconfig
new file mode 100644
index 000000000000..11d99ee8de51
--- /dev/null
+++ b/drivers/net/wireless/ath/ar5523/Kconfig
@@ -0,0 +1,7 @@
+config AR5523
+       tristate "Atheros AR5523 wireless driver support"
+       depends on MAC80211 && USB
+       select FW_LOADER
+       ---help---
+         This module add support for AR5523 based USB dongles such as D-Link
+         DWL-G132, Netgear WPN111 and many more.
diff --git a/drivers/net/wireless/ath/ar5523/Makefile b/drivers/net/wireless/ath/ar5523/Makefile
new file mode 100644
index 000000000000..ebf7f3bf0a33
--- /dev/null
+++ b/drivers/net/wireless/ath/ar5523/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_AR5523)   := ar5523.o
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.c b/drivers/net/wireless/ath/ar5523/ar5523.c
new file mode 100644
index 000000000000..f782b6e502bf
--- /dev/null
+++ b/drivers/net/wireless/ath/ar5523/ar5523.c
@@ -0,0 +1,1806 @@
+/*
+ * Copyright (c) 2006 Damien Bergamini <damien.bergamini@free.fr>
+ * Copyright (c) 2006 Sam Leffler, Errno Consulting
+ * Copyright (c) 2007 Christoph Hellwig <hch@lst.de>
+ * Copyright (c) 2008-2009 Weongyo Jeong <weongyo@freebsd.org>
+ * Copyright (c) 2012 Pontus Fuchs <pontus.fuchs@gmail.com>
+ *
+ * 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.
+ */
+
+/*
+ * This driver is based on the uath driver written by Damien Bergamini for
+ * OpenBSD, who did black-box analysis of the Windows binary driver to find
+ * out how the hardware works.  It contains a lot magic numbers because of
+ * that and only has minimal functionality.
+ */
+#include <linux/compiler.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/completion.h>
+#include <linux/firmware.h>
+#include <linux/skbuff.h>
+#include <linux/usb.h>
+#include <net/mac80211.h>
+
+#include "ar5523.h"
+#include "ar5523_hw.h"
+
+/*
+ * Various supported device vendors/products.
+ * UB51: AR5005UG 802.11b/g, UB52: AR5005UX 802.11a/b/g
+ */
+
+static int ar5523_submit_rx_cmd(struct ar5523 *ar);
+static void ar5523_data_tx_pkt_put(struct ar5523 *ar);
+
+static void ar5523_read_reply(struct ar5523 *ar, struct ar5523_cmd_hdr *hdr,
+                              struct ar5523_tx_cmd *cmd)
+{
+        int dlen, olen;
+        u32 *rp;
+
+        dlen = hdr->len - sizeof(*hdr);
+
+        if (dlen < 0) {
+                WARN_ON(1);
+                goto out;
+        }
+
+        ar5523_dbg(ar, "Code = %d len = %d\n", hdr->code & 0xff, dlen);
+
+        rp = (u32 *)(hdr + 1);
+        if (dlen >= sizeof(u32)) {
+                olen = be32_to_cpu(rp[0]);
+                dlen -= sizeof(u32);
+                if (olen == 0) {
+                        /* convention is 0 =>'s one word */
+                        olen = sizeof(u32);
+                }
+        } else
+                olen = 0;
+
+        if (cmd->odata) {
+                if (cmd->olen < olen) {
+                        ar5523_err(ar, "olen to small %d < %d\n",
+                                   cmd->olen, olen);
+                        cmd->olen = 0;
+                        cmd->res = -EOVERFLOW;
+                } else {
+                        cmd->olen = olen;
+                        memcpy(cmd->odata, &rp[1], olen);
+                        cmd->res = 0;
+                }
+        }
+
+out:
+        complete(&cmd->done);
+}
+
+static void ar5523_cmd_rx_cb(struct urb *urb)
+{
+        struct ar5523 *ar = urb->context;
+        struct ar5523_tx_cmd *cmd = &ar->tx_cmd;
+        struct ar5523_cmd_hdr *hdr = ar->rx_cmd_buf;
+        int dlen;
+
+        if (urb->status) {
+                if (urb->status != -ESHUTDOWN)
+                        ar5523_err(ar, "RX USB error %d.\n", urb->status);
+                goto skip;
+        }
+
+        if (urb->actual_length < sizeof(struct ar5523_cmd_hdr)) {
+                ar5523_err(ar, "RX USB to short.\n");
+                goto skip;
+        }
+
+        ar5523_dbg(ar, "%s code %02x priv %d\n", __func__,
+                   be32_to_cpu(hdr->code) & 0xff, hdr->priv);
+
+        hdr->code = be32_to_cpu(hdr->code);
+        hdr->len = be32_to_cpu(hdr->len);
+
+        switch (hdr->code & 0xff) {
+        default:
+                /* reply to a read command */
+                if (hdr->priv != AR5523_CMD_ID) {
+                        ar5523_err(ar, "Unexpected command id: %02x\n",
+                                   hdr->code & 0xff);
+                        goto skip;
+                }
+                ar5523_read_reply(ar, hdr, cmd);
+                break;
+
+        case WDCMSG_DEVICE_AVAIL:
+                ar5523_dbg(ar, "WDCMSG_DEVICE_AVAIL\n");
+                cmd->res = 0;
+                cmd->olen = 0;
+                complete(&cmd->done);
+                break;
+
+        case WDCMSG_SEND_COMPLETE:
+                ar5523_dbg(ar, "WDCMSG_SEND_COMPLETE: %d pending\n",
+                        atomic_read(&ar->tx_nr_pending));
+                if (!test_bit(AR5523_HW_UP, &ar->flags))
+                        ar5523_dbg(ar, "Unexpected WDCMSG_SEND_COMPLETE\n");
+                else {
+                        mod_timer(&ar->tx_wd_timer,
+                                  jiffies + AR5523_TX_WD_TIMEOUT);
+                        ar5523_data_tx_pkt_put(ar);
+
+                }
+                break;
+
+        case WDCMSG_TARGET_START:
+                /* This command returns a bogus id so it needs special
+                   handling */
+                dlen = hdr->len - sizeof(*hdr);
+                if (dlen != (int)sizeof(u32)) {
+                        ar5523_err(ar, "Invalid reply to WDCMSG_TARGET_START");
+                        return;
+                }
+                memcpy(cmd->odata, hdr + 1, sizeof(u32));
+                cmd->olen = sizeof(u32);
+                cmd->res = 0;
+                complete(&cmd->done);
+                break;
+
+        case WDCMSG_STATS_UPDATE:
+                ar5523_dbg(ar, "WDCMSG_STATS_UPDATE\n");
+                break;
+        }
+
+skip:
+        ar5523_submit_rx_cmd(ar);
+}
+
+static int ar5523_alloc_rx_cmd(struct ar5523 *ar)
+{
+        ar->rx_cmd_urb = usb_alloc_urb(0, GFP_KERNEL);
+        if (!ar->rx_cmd_urb)
+                return -ENOMEM;
+
+        ar->rx_cmd_buf = usb_alloc_coherent(ar->dev, AR5523_MAX_RXCMDSZ,
+                                            GFP_KERNEL,
+                                            &ar->rx_cmd_urb->transfer_dma);
+        if (!ar->rx_cmd_buf) {
+                usb_free_urb(ar->rx_cmd_urb);
+                return -ENOMEM;
+        }
+        return 0;
+}
+
+static void ar5523_cancel_rx_cmd(struct ar5523 *ar)
+{
+        usb_kill_urb(ar->rx_cmd_urb);
+}
+
+static void ar5523_free_rx_cmd(struct ar5523 *ar)
+{
+        usb_free_coherent(ar->dev, AR5523_MAX_RXCMDSZ,
+                          ar->rx_cmd_buf, ar->rx_cmd_urb->transfer_dma);
+        usb_free_urb(ar->rx_cmd_urb);
+}
+
+static int ar5523_submit_rx_cmd(struct ar5523 *ar)
+{
+        int error;
+
+        usb_fill_bulk_urb(ar->rx_cmd_urb, ar->dev,
+                          ar5523_cmd_rx_pipe(ar->dev), ar->rx_cmd_buf,
+                          AR5523_MAX_RXCMDSZ, ar5523_cmd_rx_cb, ar);
+        ar->rx_cmd_urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+        error = usb_submit_urb(ar->rx_cmd_urb, GFP_ATOMIC);
+        if (error) {
+                if (error != -ENODEV)
+                        ar5523_err(ar, "error %d when submitting rx urb\n",
+                                   error);
+                return error;
+        }
+        return 0;
+}
+
+/*
+ * Command submitted cb
+ */
+static void ar5523_cmd_tx_cb(struct urb *urb)
+{
+        struct ar5523_tx_cmd *cmd = urb->context;
+        struct ar5523 *ar = cmd->ar;
+
+        if (urb->status) {
+                ar5523_err(ar, "Failed to TX command. Status = %d\n",
+                           urb->status);
+                cmd->res = urb->status;
+                complete(&cmd->done);
+                return;
+        }
+
+        if (!(cmd->flags & AR5523_CMD_FLAG_READ)) {
+                cmd->res = 0;
+                complete(&cmd->done);
+        }
+}
+
+static int ar5523_cmd(struct ar5523 *ar, u32 code, const void *idata,
+                      int ilen, void *odata, int olen, int flags)
+{
+        struct ar5523_cmd_hdr *hdr;
+        struct ar5523_tx_cmd *cmd = &ar->tx_cmd;
+        int xferlen, error;
+
+        /* always bulk-out a multiple of 4 bytes */
+        xferlen = (sizeof(struct ar5523_cmd_hdr) + ilen + 3) & ~3;
+
+        hdr = (struct ar5523_cmd_hdr *)cmd->buf_tx;
+        memset(hdr, 0, sizeof(struct ar5523_cmd_hdr));
+        hdr->len  = cpu_to_be32(xferlen);
+        hdr->code = cpu_to_be32(code);
+        hdr->priv = AR5523_CMD_ID;
+
+        if (flags & AR5523_CMD_FLAG_MAGIC)
+                hdr->magic = cpu_to_be32(1 << 24);
+        memcpy(hdr + 1, idata, ilen);
+
+        cmd->odata = odata;
+        cmd->olen = olen;
+        cmd->flags = flags;
+
+        ar5523_dbg(ar, "do cmd %02x\n", code);
+
+        usb_fill_bulk_urb(cmd->urb_tx, ar->dev, ar5523_cmd_tx_pipe(ar->dev),
+                          cmd->buf_tx, xferlen, ar5523_cmd_tx_cb, cmd);
+        cmd->urb_tx->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+        error = usb_submit_urb(cmd->urb_tx, GFP_KERNEL);
+        if (error) {
+                ar5523_err(ar, "could not send command 0x%x, error=%d\n",
+                           code, error);
+                return error;
+        }
+
+        if (!wait_for_completion_timeout(&cmd->done, 2 * HZ)) {
+                cmd->odata = NULL;
+                ar5523_err(ar, "timeout waiting for command %02x reply\n",
+                           code);
+                cmd->res = -ETIMEDOUT;
+        }
+        return cmd->res;
+}
+
+static int ar5523_cmd_write(struct ar5523 *ar, u32 code, const void *data,
+                            int len, int flags)
+{
+        flags &= ~AR5523_CMD_FLAG_READ;
+        return ar5523_cmd(ar, code, data, len, NULL, 0, flags);
+}
+
+static int ar5523_cmd_read(struct ar5523 *ar, u32 code, const void *idata,
+                           int ilen, void *odata, int olen, int flags)
+{
+        flags |= AR5523_CMD_FLAG_READ;
+        return ar5523_cmd(ar, code, idata, ilen, odata, olen, flags);
+}
+
+static int ar5523_config(struct ar5523 *ar, u32 reg, u32 val)
+{
+        struct ar5523_write_mac write;
+        int error;
+
+        write.reg = cpu_to_be32(reg);
+        write.len = cpu_to_be32(0);     /* 0 = single write */
+        *(u32 *)write.data = cpu_to_be32(val);
+
+        error = ar5523_cmd_write(ar, WDCMSG_TARGET_SET_CONFIG, &write,
+                                 3 * sizeof(u32), 0);
+        if (error != 0)
+                ar5523_err(ar, "could not write register 0x%02x\n", reg);
+        return error;
+}
+
+static int ar5523_config_multi(struct ar5523 *ar, u32 reg, const void *data,
+                               int len)
+{
+        struct ar5523_write_mac write;
+        int error;
+
+        write.reg = cpu_to_be32(reg);
+        write.len = cpu_to_be32(len);
+        memcpy(write.data, data, len);
+
+        /* properly handle the case where len is zero (reset) */
+        error = ar5523_cmd_write(ar, WDCMSG_TARGET_SET_CONFIG, &write,
+            (len == 0) ? sizeof(u32) : 2 * sizeof(u32) + len, 0);
+        if (error != 0)
+                ar5523_err(ar, "could not write %d bytes to register 0x%02x\n",
+                           len, reg);
+        return error;
+}
+
+static int ar5523_get_status(struct ar5523 *ar, u32 which, void *odata,
+                             int olen)
+{
+        int error;
+
+        which = cpu_to_be32(which);
+        error = ar5523_cmd_read(ar, WDCMSG_TARGET_GET_STATUS,
+            &which, sizeof(which), odata, olen, AR5523_CMD_FLAG_MAGIC);
+        if (error != 0)
+                ar5523_err(ar, "could not read EEPROM offset 0x%02x\n",
+                           be32_to_cpu(which));
+        return error;
+}
+
+static int ar5523_get_capability(struct ar5523 *ar, u32 cap, u32 *val)
+{
+        int error;
+
+        cap = cpu_to_be32(cap);
+        error = ar5523_cmd_read(ar, WDCMSG_TARGET_GET_CAPABILITY,
+            &cap, sizeof(cap), val, sizeof(u32), AR5523_CMD_FLAG_MAGIC);
+        if (error != 0) {
+                ar5523_err(ar, "could not read capability %u\n",
+                           be32_to_cpu(cap));
+                return error;
+        }
+        *val = be32_to_cpu(*val);
+        return error;
+}
+
+static int ar5523_get_devcap(struct ar5523 *ar)
+{
+#define GETCAP(x) do {                          \
+        error = ar5523_get_capability(ar, x, &cap);             \
+        if (error != 0)                                 \
+                return error;                           \
+        ar5523_info(ar, "Cap: "                 \
+            "%s=0x%08x\n", #x, cap);    \
+} while (0)
+        int error;
+        u32 cap;
+
+        /* collect device capabilities */
+        GETCAP(CAP_TARGET_VERSION);
+        GETCAP(CAP_TARGET_REVISION);
+        GETCAP(CAP_MAC_VERSION);
+        GETCAP(CAP_MAC_REVISION);
+        GETCAP(CAP_PHY_REVISION);
+        GETCAP(CAP_ANALOG_5GHz_REVISION);
+        GETCAP(CAP_ANALOG_2GHz_REVISION);
+
+        GETCAP(CAP_REG_DOMAIN);
+        GETCAP(CAP_REG_CAP_BITS);
+        GETCAP(CAP_WIRELESS_MODES);
+        GETCAP(CAP_CHAN_SPREAD_SUPPORT);
+        GETCAP(CAP_COMPRESS_SUPPORT);
+        GETCAP(CAP_BURST_SUPPORT);
+        GETCAP(CAP_FAST_FRAMES_SUPPORT);
+        GETCAP(CAP_CHAP_TUNING_SUPPORT);
+        GETCAP(CAP_TURBOG_SUPPORT);
+        GETCAP(CAP_TURBO_PRIME_SUPPORT);
+        GETCAP(CAP_DEVICE_TYPE);
+        GETCAP(CAP_WME_SUPPORT);
+        GETCAP(CAP_TOTAL_QUEUES);
+        GETCAP(CAP_CONNECTION_ID_MAX);
+
+        GETCAP(CAP_LOW_5GHZ_CHAN);
+        GETCAP(CAP_HIGH_5GHZ_CHAN);
+        GETCAP(CAP_LOW_2GHZ_CHAN);
+        GETCAP(CAP_HIGH_2GHZ_CHAN);
+        GETCAP(CAP_TWICE_ANTENNAGAIN_5G);
+        GETCAP(CAP_TWICE_ANTENNAGAIN_2G);
+
+        GETCAP(CAP_CIPHER_AES_CCM);
+        GETCAP(CAP_CIPHER_TKIP);
+        GETCAP(CAP_MIC_TKIP);
+        return 0;
+}
+
+static int ar5523_set_ledsteady(struct ar5523 *ar, int lednum, int ledmode)
+{
+        struct ar5523_cmd_ledsteady led;
+
+        led.lednum = cpu_to_be32(lednum);
+        led.ledmode = cpu_to_be32(ledmode);
+
+        ar5523_dbg(ar, "set %s led %s (steady)\n",
+                   (lednum == UATH_LED_LINK) ? "link" : "activity",
+                   ledmode ? "on" : "off");
+        return ar5523_cmd_write(ar, WDCMSG_SET_LED_STEADY, &led, sizeof(led),
+                                 0);
+}
+
+static int ar5523_set_rxfilter(struct ar5523 *ar, u32 bits, u32 op)
+{
+        struct ar5523_cmd_rx_filter rxfilter;
+
+        rxfilter.bits = cpu_to_be32(bits);
+        rxfilter.op = cpu_to_be32(op);
+
+        ar5523_dbg(ar, "setting Rx filter=0x%x flags=0x%x\n", bits, op);
+        return ar5523_cmd_write(ar, WDCMSG_RX_FILTER, &rxfilter,
+                                 sizeof(rxfilter), 0);
+}
+
+static int ar5523_reset_tx_queues(struct ar5523 *ar)
+{
+        __be32 qid = cpu_to_be32(0);
+
+        ar5523_dbg(ar, "resetting Tx queue\n");
+        return ar5523_cmd_write(ar, WDCMSG_RELEASE_TX_QUEUE,
+                                 &qid, sizeof(qid), 0);
+}
+
+static int ar5523_set_chan(struct ar5523 *ar)
+{
+        struct ieee80211_conf *conf = &ar->hw->conf;
+
+        struct ar5523_cmd_reset reset;
+
+        memset(&reset, 0, sizeof(reset));
+        reset.flags |= cpu_to_be32(UATH_CHAN_2GHZ);
+        reset.flags |= cpu_to_be32(UATH_CHAN_OFDM);
+        reset.freq = cpu_to_be32(conf->channel->center_freq);
+        reset.maxrdpower = cpu_to_be32(50);     /* XXX */
+        reset.channelchange = cpu_to_be32(1);
+        reset.keeprccontent = cpu_to_be32(0);
+
+        ar5523_dbg(ar, "set chan flags 0x%x freq %d\n",
+                   be32_to_cpu(reset.flags),
+                   conf->channel->center_freq);
+        return ar5523_cmd_write(ar, WDCMSG_RESET, &reset, sizeof(reset), 0);
+}
+
+static int ar5523_queue_init(struct ar5523 *ar)
+{
+        struct ar5523_cmd_txq_setup qinfo;
+
+        ar5523_dbg(ar, "setting up Tx queue\n");
+        qinfo.qid            = cpu_to_be32(0);
+        qinfo.len            = cpu_to_be32(sizeof(qinfo.attr));
+        qinfo.attr.priority  = cpu_to_be32(0);  /* XXX */
+        qinfo.attr.aifs      = cpu_to_be32(3);
+        qinfo.attr.logcwmin  = cpu_to_be32(4);
+        qinfo.attr.logcwmax  = cpu_to_be32(10);
+        qinfo.attr.bursttime = cpu_to_be32(0);
+        qinfo.attr.mode      = cpu_to_be32(0);
+        qinfo.attr.qflags    = cpu_to_be32(1);  /* XXX? */
+        return ar5523_cmd_write(ar, WDCMSG_SETUP_TX_QUEUE, &qinfo,
+                                 sizeof(qinfo), 0);
+}
+
+static int ar5523_switch_chan(struct ar5523 *ar)
+{
+        int error;
+
+        error = ar5523_set_chan(ar);
+        if (error) {
+                ar5523_err(ar, "could not set chan, error %d\n", error);
+                goto out_err;
+        }
+
+        /* reset Tx rings */
+        error = ar5523_reset_tx_queues(ar);
+        if (error) {
+                ar5523_err(ar, "could not reset Tx queues, error %d\n",
+                           error);
+                goto out_err;
+        }
+        /* set Tx rings WME properties */
+        error = ar5523_queue_init(ar);
+        if (error)
+                ar5523_err(ar, "could not init wme, error %d\n", error);
+
+out_err:
+        return error;
+}
+
+static void ar5523_rx_data_put(struct ar5523 *ar,
+                                struct ar5523_rx_data *data)
+{
+        unsigned long flags;
+        spin_lock_irqsave(&ar->rx_data_list_lock, flags);
+        list_move(&data->list, &ar->rx_data_free);
+        spin_unlock_irqrestore(&ar->rx_data_list_lock, flags);
+}
+
+static void ar5523_data_rx_cb(struct urb *urb)
+{
+        struct ar5523_rx_data *data = urb->context;
+        struct ar5523 *ar = data->ar;
+        struct ar5523_rx_desc *desc;
+        struct ar5523_chunk *chunk;
+        struct ieee80211_hw *hw = ar->hw;
+        struct ieee80211_rx_status *rx_status;
+        u32 rxlen;
+        int usblen = urb->actual_length;
+        int hdrlen, pad;
+
+        ar5523_dbg(ar, "%s\n", __func__);
+        /* sync/async unlink faults aren't errors */
+        if (urb->status) {
+                if (urb->status != -ESHUTDOWN)
+                        ar5523_err(ar, "%s: USB err: %d\n", __func__,
+                                   urb->status);
+                goto skip;
+        }
+
+        if (usblen < AR5523_MIN_RXBUFSZ) {
+                ar5523_err(ar, "RX: wrong xfer size (usblen=%d)\n", usblen);
+                goto skip;
+        }
+
+        chunk = (struct ar5523_chunk *) data->skb->data;
+
+        if (((chunk->flags & UATH_CFLAGS_FINAL) == 0) ||
+                chunk->seqnum != 0) {
+                ar5523_dbg(ar, "RX: No final flag. s: %d f: %02x l: %d\n",
+                           chunk->seqnum, chunk->flags,
+                           be16_to_cpu(chunk->length));
+                goto skip;
+        }
+
+        /* Rx descriptor is located at the end, 32-bit aligned */
+        desc = (struct ar5523_rx_desc *)
+                (data->skb->data + usblen - sizeof(struct ar5523_rx_desc));
+
+        rxlen = be32_to_cpu(desc->len);
+        if (rxlen > ar->rxbufsz) {
+                ar5523_dbg(ar, "RX: Bad descriptor (len=%d)\n",
+                           be32_to_cpu(desc->len));
+                goto skip;
+        }
+
+        if (!rxlen) {
+                ar5523_dbg(ar, "RX: rxlen is 0\n");
+                goto skip;
+        }
+
+        if (be32_to_cpu(desc->status) != 0) {
+                ar5523_dbg(ar, "Bad RX status (0x%x len = %d). Skip\n",
+                           be32_to_cpu(desc->status), be32_to_cpu(desc->len));
+                goto skip;
+        }
+
+        skb_reserve(data->skb, sizeof(*chunk));
+        skb_put(data->skb, rxlen - sizeof(struct ar5523_rx_desc));
+
+        hdrlen = ieee80211_get_hdrlen_from_skb(data->skb);
+        if (!IS_ALIGNED(hdrlen, 4)) {
+                ar5523_dbg(ar, "eek, alignment workaround activated\n");
+                pad = ALIGN(hdrlen, 4) - hdrlen;
+                memmove(data->skb->data + pad, data->skb->data, hdrlen);
+                skb_pull(data->skb, pad);
+                skb_put(data->skb, pad);
+        }
+
+        rx_status = IEEE80211_SKB_RXCB(data->skb);
+        memset(rx_status, 0, sizeof(*rx_status));
+        rx_status->freq = be32_to_cpu(desc->channel);
+        rx_status->band = hw->conf.channel->band;
+        rx_status->signal = -95 + be32_to_cpu(desc->rssi);
+
+        ieee80211_rx_irqsafe(hw, data->skb);
+        data->skb = NULL;
+
+skip:
+        if (data->skb) {
+                dev_kfree_skb_irq(data->skb);
+                data->skb = NULL;
+        }
+
+        ar5523_rx_data_put(ar, data);
+        if (atomic_inc_return(&ar->rx_data_free_cnt) >=
+            AR5523_RX_DATA_REFILL_COUNT &&
+            test_bit(AR5523_HW_UP, &ar->flags))
+                queue_work(ar->wq, &ar->rx_refill_work);
+}
+
+static void ar5523_rx_refill_work(struct work_struct *work)
+{
+        struct ar5523 *ar = container_of(work, struct ar5523, rx_refill_work);
+        struct ar5523_rx_data *data;
+        unsigned long flags;
+        int error;
+
+        ar5523_dbg(ar, "%s\n", __func__);
+        do {
+                spin_lock_irqsave(&ar->rx_data_list_lock, flags);
+
+                if (!list_empty(&ar->rx_data_free))
+                        data = (struct ar5523_rx_data *) ar->rx_data_free.next;
+                else
+                        data = NULL;
+                spin_unlock_irqrestore(&ar->rx_data_list_lock, flags);
+
+                if (!data)
+                        goto done;
+
+                data->skb = alloc_skb(ar->rxbufsz, GFP_KERNEL);
+                if (!data->skb) {
+                        ar5523_err(ar, "could not allocate rx skbuff\n");
+                        return;
+                }
+
+                usb_fill_bulk_urb(data->urb, ar->dev,
+                                  ar5523_data_rx_pipe(ar->dev), data->skb->data,
+                                  ar->rxbufsz, ar5523_data_rx_cb, data);
+
+                spin_lock_irqsave(&ar->rx_data_list_lock, flags);
+                list_move(&data->list, &ar->rx_data_used);
+                spin_unlock_irqrestore(&ar->rx_data_list_lock, flags);
+                atomic_dec(&ar->rx_data_free_cnt);
+
+                error = usb_submit_urb(data->urb, GFP_KERNEL);
+                if (error) {
+                        kfree_skb(data->skb);
+                        if (error != -ENODEV)
+                                ar5523_err(ar, "Err sending rx data urb %d\n",
+                                           error);
+                        ar5523_rx_data_put(ar, data);
+                        atomic_inc(&ar->rx_data_free_cnt);
+                        return;
+                }
+
+        } while (true);
+done:
+        return;
+}
+
+static void ar5523_cancel_rx_bufs(struct ar5523 *ar)
+{
+        struct ar5523_rx_data *data;
+        unsigned long flags;
+
+        do {
+                spin_lock_irqsave(&ar->rx_data_list_lock, flags);
+                if (!list_empty(&ar->rx_data_used))
+                        data = (struct ar5523_rx_data *) ar->rx_data_used.next;
+                else
+                        data = NULL;
+                spin_unlock_irqrestore(&ar->rx_data_list_lock, flags);
+
+                if (!data)
+                        break;
+
+                usb_kill_urb(data->urb);
+                list_move(&data->list, &ar->rx_data_free);
+                atomic_inc(&ar->rx_data_free_cnt);
+        } while (data);
+}
+
+static void ar5523_free_rx_bufs(struct ar5523 *ar)
+{
+        struct ar5523_rx_data *data;
+
+        ar5523_cancel_rx_bufs(ar);
+        while (!list_empty(&ar->rx_data_free)) {
+                data = (struct ar5523_rx_data *) ar->rx_data_free.next;
+                list_del(&data->list);
+                usb_free_urb(data->urb);
+        }
+}
+
+static int ar5523_alloc_rx_bufs(struct ar5523 *ar)
+{
+        int i;
+
+        for (i = 0; i < AR5523_RX_DATA_COUNT; i++) {
+                struct ar5523_rx_data *data = &ar->rx_data[i];
+
+                data->ar = ar;
+                data->urb = usb_alloc_urb(0, GFP_KERNEL);
+                if (!data->urb) {
+                        ar5523_err(ar, "could not allocate rx data urb\n");
+                        goto err;
+                }
+                list_add_tail(&data->list, &ar->rx_data_free);
+                atomic_inc(&ar->rx_data_free_cnt);
+        }
+        return 0;
+
+err:
+        ar5523_free_rx_bufs(ar);
+        return -ENOMEM;
+}
+
+static void ar5523_data_tx_pkt_put(struct ar5523 *ar)
+{
+        atomic_dec(&ar->tx_nr_total);
+        if (!atomic_dec_return(&ar->tx_nr_pending)) {
+                del_timer(&ar->tx_wd_timer);
+                wake_up(&ar->tx_flush_waitq);
+        }
+
+        if (atomic_read(&ar->tx_nr_total) < AR5523_TX_DATA_RESTART_COUNT) {
+                ar5523_dbg(ar, "restart tx queue\n");
+                ieee80211_wake_queues(ar->hw);
+        }
+}
+
+static void ar5523_data_tx_cb(struct urb *urb)
+{
+        struct sk_buff *skb = urb->context;
+        struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+        struct ar5523_tx_data *data = (struct ar5523_tx_data *)
+                                       txi->driver_data;
+        struct ar5523 *ar = data->ar;
+        unsigned long flags;
+
+        ar5523_dbg(ar, "data tx urb completed: %d\n", urb->status);
+
+        spin_lock_irqsave(&ar->tx_data_list_lock, flags);
+        list_del(&data->list);
+        spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
+
+        if (urb->status) {
+                ar5523_dbg(ar, "%s: urb status: %d\n", __func__, urb->status);
+                ar5523_data_tx_pkt_put(ar);
+                ieee80211_free_txskb(ar->hw, skb);
+        } else {
+                skb_pull(skb, sizeof(struct ar5523_tx_desc) + sizeof(__be32));
+                ieee80211_tx_status_irqsafe(ar->hw, skb);
+        }
+        usb_free_urb(urb);
+}
+
+static void ar5523_tx(struct ieee80211_hw *hw,
+                       struct ieee80211_tx_control *control,
+                       struct sk_buff *skb)
+{
+        struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+        struct ar5523_tx_data *data = (struct ar5523_tx_data *)
+                                        txi->driver_data;
+        struct ar5523 *ar = hw->priv;
+        unsigned long flags;
+
+        ar5523_dbg(ar, "tx called\n");
+        if (atomic_inc_return(&ar->tx_nr_total) >= AR5523_TX_DATA_COUNT) {
+                ar5523_dbg(ar, "tx queue full\n");
+                ar5523_dbg(ar, "stop queues (tot %d pend %d)\n",
+                           atomic_read(&ar->tx_nr_total),
+                           atomic_read(&ar->tx_nr_pending));
+                ieee80211_stop_queues(hw);
+        }
+
+        data->skb = skb;
+
+        spin_lock_irqsave(&ar->tx_data_list_lock, flags);
+        list_add_tail(&data->list, &ar->tx_queue_pending);
+        spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
+
+        ieee80211_queue_work(ar->hw, &ar->tx_work);
+}
+
+static void ar5523_tx_work_locked(struct ar5523 *ar)
+{
+        struct ar5523_tx_data *data;
+        struct ar5523_tx_desc *desc;
+        struct ar5523_chunk *chunk;
+        struct ieee80211_tx_info *txi;
+        struct urb *urb;
+        struct sk_buff *skb;
+        int error = 0, paylen;
+        u32 txqid;
+        unsigned long flags;
+
+        BUILD_BUG_ON(sizeof(struct ar5523_tx_data) >
+                     IEEE80211_TX_INFO_DRIVER_DATA_SIZE);
+
+        ar5523_dbg(ar, "%s\n", __func__);
+        do {
+                spin_lock_irqsave(&ar->tx_data_list_lock, flags);
+                if (!list_empty(&ar->tx_queue_pending)) {
+                        data = (struct ar5523_tx_data *)
+                                ar->tx_queue_pending.next;
+                        list_del(&data->list);
+                } else
+                        data = NULL;
+                spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
+
+                if (!data)
+                        break;
+
+                skb = data->skb;
+                txqid = 0;
+                txi = IEEE80211_SKB_CB(skb);
+                paylen = skb->len;
+                urb = usb_alloc_urb(0, GFP_KERNEL);
+                if (!urb) {
+                        ar5523_err(ar, "Failed to allocate TX urb\n");
+                        ieee80211_free_txskb(ar->hw, skb);
+                        continue;
+                }
+
+                data->ar = ar;
+                data->urb = urb;
+
+                desc = (struct ar5523_tx_desc *)skb_push(skb, sizeof(*desc));
+                chunk = (struct ar5523_chunk *)skb_push(skb, sizeof(*chunk));
+
+                chunk->seqnum = 0;
+                chunk->flags = UATH_CFLAGS_FINAL;
+                chunk->length = cpu_to_be16(skb->len);
+
+                desc->msglen = cpu_to_be32(skb->len);
+                desc->msgid  = AR5523_DATA_ID;
+                desc->buflen = cpu_to_be32(paylen);
+                desc->type   = cpu_to_be32(WDCMSG_SEND);
+                desc->flags  = cpu_to_be32(UATH_TX_NOTIFY);
+
+                if (test_bit(AR5523_CONNECTED, &ar->flags))
+                        desc->connid = cpu_to_be32(AR5523_ID_BSS);
+                else
+                        desc->connid = cpu_to_be32(AR5523_ID_BROADCAST);
+
+                if (txi->flags & IEEE80211_TX_CTL_USE_MINRATE)
+                        txqid |= UATH_TXQID_MINRATE;
+
+                desc->txqid = cpu_to_be32(txqid);
+
+                urb->transfer_flags = URB_ZERO_PACKET;
+                usb_fill_bulk_urb(urb, ar->dev, ar5523_data_tx_pipe(ar->dev),
+                                  skb->data, skb->len, ar5523_data_tx_cb, skb);
+
+                spin_lock_irqsave(&ar->tx_data_list_lock, flags);
+                list_add_tail(&data->list, &ar->tx_queue_submitted);
+                spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
+                mod_timer(&ar->tx_wd_timer, jiffies + AR5523_TX_WD_TIMEOUT);
+                atomic_inc(&ar->tx_nr_pending);
+
+                ar5523_dbg(ar, "TX Frame (%d pending)\n",
+                           atomic_read(&ar->tx_nr_pending));
+                error = usb_submit_urb(urb, GFP_KERNEL);
+                if (error) {
+                        ar5523_err(ar, "error %d when submitting tx urb\n",
+                                   error);
+                        spin_lock_irqsave(&ar->tx_data_list_lock, flags);
+                        list_del(&data->list);
+                        spin_unlock_irqrestore(&ar->tx_data_list_lock, flags);
+                        atomic_dec(&ar->tx_nr_pending);
+                        ar5523_data_tx_pkt_put(ar);
+                        usb_free_urb(urb);
+                        ieee80211_free_txskb(ar->hw, skb);
+                }
+        } while (true);
+}
+
+static void ar5523_tx_work(struct work_struct *work)
+{
+        struct ar5523 *ar = container_of(work, struct ar5523, tx_work);
+
+        ar5523_dbg(ar, "%s\n", __func__);
+        mutex_lock(&ar->mutex);
+        ar5523_tx_work_locked(ar);
+        mutex_unlock(&ar->mutex);
+}
+
+static void ar5523_tx_wd_timer(unsigned long arg)
+{
+        struct ar5523 *ar = (struct ar5523 *) arg;
+
+        ar5523_dbg(ar, "TX watchdog timer triggered\n");
+        ieee80211_queue_work(ar->hw, &ar->tx_wd_work);
+}
+
+static void ar5523_tx_wd_work(struct work_struct *work)
+{
+        struct ar5523 *ar = container_of(work, struct ar5523, tx_wd_work);
+
+        /* Occasionally the TX queues stop responding. The only way to
+         * recover seems to be to reset the dongle.
+         */
+
+        mutex_lock(&ar->mutex);
+        ar5523_err(ar, "TX queue stuck (tot %d pend %d)\n",
+                   atomic_read(&ar->tx_nr_total),
+                   atomic_read(&ar->tx_nr_pending));
+
+        ar5523_err(ar, "Will restart dongle.\n");
+        ar5523_cmd_write(ar, WDCMSG_TARGET_RESET, NULL, 0, 0);
+        mutex_unlock(&ar->mutex);
+}
+
+static void ar5523_flush_tx(struct ar5523 *ar)
+{
+        ar5523_tx_work_locked(ar);
+
+        /* Don't waste time trying to flush if USB is disconnected */
+        if (test_bit(AR5523_USB_DISCONNECTED, &ar->flags))
+                return;
+        if (!wait_event_timeout(ar->tx_flush_waitq,
+            !atomic_read(&ar->tx_nr_pending), AR5523_FLUSH_TIMEOUT))
+                ar5523_err(ar, "flush timeout (tot %d pend %d)\n",
+                           atomic_read(&ar->tx_nr_total),
+                           atomic_read(&ar->tx_nr_pending));
+}
+
+static void ar5523_free_tx_cmd(struct ar5523 *ar)
+{
+        struct ar5523_tx_cmd *cmd = &ar->tx_cmd;
+
+        usb_free_coherent(ar->dev, AR5523_MAX_RXCMDSZ, cmd->buf_tx,
+                          cmd->urb_tx->transfer_dma);
+        usb_free_urb(cmd->urb_tx);
+}
+
+static int ar5523_alloc_tx_cmd(struct ar5523 *ar)
+{
+        struct ar5523_tx_cmd *cmd = &ar->tx_cmd;
+
+        cmd->ar = ar;
+        init_completion(&cmd->done);
+
+        cmd->urb_tx = usb_alloc_urb(0, GFP_KERNEL);
+        if (!cmd->urb_tx) {
+                ar5523_err(ar, "could not allocate urb\n");
+                return -ENOMEM;
+        }
+        cmd->buf_tx = usb_alloc_coherent(ar->dev, AR5523_MAX_TXCMDSZ,
+                                         GFP_KERNEL,
+                                         &cmd->urb_tx->transfer_dma);
+        if (!cmd->buf_tx) {
+                usb_free_urb(cmd->urb_tx);
+                return -ENOMEM;
+        }
+        return 0;
+}
+
+/*
+ * This function is called periodically (every second) when associated to
+ * query device statistics.
+ */
+static void ar5523_stat_work(struct work_struct *work)
+{
+        struct ar5523 *ar = container_of(work, struct ar5523, stat_work.work);
+        int error;
+
+        ar5523_dbg(ar, "%s\n", __func__);
+        mutex_lock(&ar->mutex);
+
+        /*
+         * Send request for statistics asynchronously once a second. This
+         * seems to be important. Throughput is a lot better if this is done.
+         */
+        error = ar5523_cmd_write(ar, WDCMSG_TARGET_GET_STATS, NULL, 0, 0);
+        if (error)
+                ar5523_err(ar, "could not query stats, error %d\n", error);
+        mutex_unlock(&ar->mutex);
+        ieee80211_queue_delayed_work(ar->hw, &ar->stat_work, HZ);
+}
+
+/*
+ * Interface routines to the mac80211 stack.
+ */
+static int ar5523_start(struct ieee80211_hw *hw)
+{
+        struct ar5523 *ar = hw->priv;
+        int error;
+        __be32 val;
+
+        ar5523_dbg(ar, "start called\n");
+
+        mutex_lock(&ar->mutex);
+        val = cpu_to_be32(0);
+        ar5523_cmd_write(ar, WDCMSG_BIND, &val, sizeof(val), 0);
+
+        /* set MAC address */
+        ar5523_config_multi(ar, CFG_MAC_ADDR, &ar->hw->wiphy->perm_addr,
+                            ETH_ALEN);
+
+        /* XXX honor net80211 state */
+        ar5523_config(ar, CFG_RATE_CONTROL_ENABLE, 0x00000001);
+        ar5523_config(ar, CFG_DIVERSITY_CTL, 0x00000001);
+        ar5523_config(ar, CFG_ABOLT, 0x0000003f);
+        ar5523_config(ar, CFG_WME_ENABLED, 0x00000000);
+
+        ar5523_config(ar, CFG_SERVICE_TYPE, 1);
+        ar5523_config(ar, CFG_TP_SCALE, 0x00000000);
+        ar5523_config(ar, CFG_TPC_HALF_DBM5, 0x0000003c);
+        ar5523_config(ar, CFG_TPC_HALF_DBM2, 0x0000003c);
+        ar5523_config(ar, CFG_OVERRD_TX_POWER, 0x00000000);
+        ar5523_config(ar, CFG_GMODE_PROTECTION, 0x00000000);
+        ar5523_config(ar, CFG_GMODE_PROTECT_RATE_INDEX, 0x00000003);
+        ar5523_config(ar, CFG_PROTECTION_TYPE, 0x00000000);
+        ar5523_config(ar, CFG_MODE_CTS, 0x00000002);
+
+        error = ar5523_cmd_read(ar, WDCMSG_TARGET_START, NULL, 0,
+            &val, sizeof(val), AR5523_CMD_FLAG_MAGIC);
+        if (error) {
+                ar5523_dbg(ar, "could not start target, error %d\n", error);
+                goto err;
+        }
+        ar5523_dbg(ar, "WDCMSG_TARGET_START returns handle: 0x%x\n",
+                   be32_to_cpu(val));
+
+        ar5523_switch_chan(ar);
+
+        val = cpu_to_be32(TARGET_DEVICE_AWAKE);
+        ar5523_cmd_write(ar, WDCMSG_SET_PWR_MODE, &val, sizeof(val), 0);
+        /* XXX? check */
+        ar5523_cmd_write(ar, WDCMSG_RESET_KEY_CACHE, NULL, 0, 0);
+
+        set_bit(AR5523_HW_UP, &ar->flags);
+        queue_work(ar->wq, &ar->rx_refill_work);
+
+        /* enable Rx */
+        ar5523_set_rxfilter(ar, 0, UATH_FILTER_OP_INIT);
+        ar5523_set_rxfilter(ar,
+                            UATH_FILTER_RX_UCAST | UATH_FILTER_RX_MCAST |
+                            UATH_FILTER_RX_BCAST | UATH_FILTER_RX_BEACON,
+                            UATH_FILTER_OP_SET);
+
+        ar5523_set_ledsteady(ar, UATH_LED_ACTIVITY, UATH_LED_ON);
+        ar5523_dbg(ar, "start OK\n");
+
+err:
+        mutex_unlock(&ar->mutex);
+        return error;
+}
+
+static void ar5523_stop(struct ieee80211_hw *hw)
+{
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "stop called\n");
+
+        cancel_delayed_work_sync(&ar->stat_work);
+        mutex_lock(&ar->mutex);
+        clear_bit(AR5523_HW_UP, &ar->flags);
+
+        ar5523_set_ledsteady(ar, UATH_LED_LINK, UATH_LED_OFF);
+        ar5523_set_ledsteady(ar, UATH_LED_ACTIVITY, UATH_LED_OFF);
+
+        ar5523_cmd_write(ar, WDCMSG_TARGET_STOP, NULL, 0, 0);
+
+        del_timer_sync(&ar->tx_wd_timer);
+        cancel_work_sync(&ar->tx_wd_work);
+        cancel_work_sync(&ar->rx_refill_work);
+        ar5523_cancel_rx_bufs(ar);
+        mutex_unlock(&ar->mutex);
+}
+
+static int ar5523_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
+{
+        struct ar5523 *ar = hw->priv;
+        int ret;
+
+        ar5523_dbg(ar, "set_rts_threshold called\n");
+        mutex_lock(&ar->mutex);
+
+        ret = ar5523_config(ar, CFG_USER_RTS_THRESHOLD, value);
+
+        mutex_unlock(&ar->mutex);
+        return ret;
+}
+
+static void ar5523_flush(struct ieee80211_hw *hw, bool drop)
+{
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "flush called\n");
+        ar5523_flush_tx(ar);
+}
+
+static int ar5523_add_interface(struct ieee80211_hw *hw,
+                                struct ieee80211_vif *vif)
+{
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "add interface called\n");
+
+        if (ar->vif) {
+                ar5523_dbg(ar, "invalid add_interface\n");
+                return -EOPNOTSUPP;
+        }
+
+        switch (vif->type) {
+        case NL80211_IFTYPE_STATION:
+                ar->vif = vif;
+                break;
+        default:
+                return -EOPNOTSUPP;
+        }
+        return 0;
+}
+
+static void ar5523_remove_interface(struct ieee80211_hw *hw,
+                                    struct ieee80211_vif *vif)
+{
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "remove interface called\n");
+        ar->vif = NULL;
+}
+
+static int ar5523_hwconfig(struct ieee80211_hw *hw, u32 changed)
+{
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "config called\n");
+        mutex_lock(&ar->mutex);
+        if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+                ar5523_dbg(ar, "Do channel switch\n");
+                ar5523_flush_tx(ar);
+                ar5523_switch_chan(ar);
+        }
+        mutex_unlock(&ar->mutex);
+        return 0;
+}
+
+static int ar5523_get_wlan_mode(struct ar5523 *ar,
+                                struct ieee80211_bss_conf *bss_conf)
+{
+        struct ieee80211_supported_band *band;
+        int bit;
+        struct ieee80211_sta *sta;
+        u32 sta_rate_set;
+
+        band = ar->hw->wiphy->bands[ar->hw->conf.channel->band];
+        sta = ieee80211_find_sta(ar->vif, bss_conf->bssid);
+        if (!sta) {
+                ar5523_info(ar, "STA not found!\n");
+                return WLAN_MODE_11b;
+        }
+        sta_rate_set = sta->supp_rates[ar->hw->conf.channel->band];
+
+        for (bit = 0; bit < band->n_bitrates; bit++) {
+                if (sta_rate_set & 1) {
+                        int rate = band->bitrates[bit].bitrate;
+                        switch (rate) {
+                        case 60:
+                        case 90:
+                        case 120:
+                        case 180:
+                        case 240:
+                        case 360:
+                        case 480:
+                        case 540:
+                                return WLAN_MODE_11g;
+                        }
+                }
+                sta_rate_set >>= 1;
+        }
+        return WLAN_MODE_11b;
+}
+
+static void ar5523_create_rateset(struct ar5523 *ar,
+                                  struct ieee80211_bss_conf *bss_conf,
+                                  struct ar5523_cmd_rateset *rs,
+                                  bool basic)
+{
+        struct ieee80211_supported_band *band;
+        struct ieee80211_sta *sta;
+        int bit, i = 0;
+        u32 sta_rate_set, basic_rate_set;
+
+        sta = ieee80211_find_sta(ar->vif, bss_conf->bssid);
+        basic_rate_set = bss_conf->basic_rates;
+        if (!sta) {
+                ar5523_info(ar, "STA not found. Cannot set rates\n");
+                sta_rate_set = bss_conf->basic_rates;
+        }
+        sta_rate_set = sta->supp_rates[ar->hw->conf.channel->band];
+
+        ar5523_dbg(ar, "sta rate_set = %08x\n", sta_rate_set);
+
+        band = ar->hw->wiphy->bands[ar->hw->conf.channel->band];
+        for (bit = 0; bit < band->n_bitrates; bit++) {
+                BUG_ON(i >= AR5523_MAX_NRATES);
+                ar5523_dbg(ar, "Considering rate %d : %d\n",
+                           band->bitrates[bit].hw_value, sta_rate_set & 1);
+                if (sta_rate_set & 1) {
+                        rs->set[i] = band->bitrates[bit].hw_value;
+                        if (basic_rate_set & 1 && basic)
+                                rs->set[i] |= 0x80;
+                        i++;
+                }
+                sta_rate_set >>= 1;
+                basic_rate_set >>= 1;
+        }
+
+        rs->length = i;
+}
+
+static int ar5523_set_basic_rates(struct ar5523 *ar,
+                                  struct ieee80211_bss_conf *bss)
+{
+        struct ar5523_cmd_rates rates;
+
+        memset(&rates, 0, sizeof(rates));
+        rates.connid = cpu_to_be32(2);          /* XXX */
+        rates.size   = cpu_to_be32(sizeof(struct ar5523_cmd_rateset));
+        ar5523_create_rateset(ar, bss, &rates.rateset, true);
+
+        return ar5523_cmd_write(ar, WDCMSG_SET_BASIC_RATE, &rates,
+                                sizeof(rates), 0);
+}
+
+static int ar5523_create_connection(struct ar5523 *ar,
+                                    struct ieee80211_vif *vif,
+                                    struct ieee80211_bss_conf *bss)
+{
+        struct ar5523_cmd_create_connection create;
+        int wlan_mode;
+
+        memset(&create, 0, sizeof(create));
+        create.connid = cpu_to_be32(2);
+        create.bssid = cpu_to_be32(0);
+        /* XXX packed or not?  */
+        create.size = cpu_to_be32(sizeof(struct ar5523_cmd_rateset));
+
+        ar5523_create_rateset(ar, bss, &create.connattr.rateset, false);
+
+        wlan_mode = ar5523_get_wlan_mode(ar, bss);
+        create.connattr.wlanmode = cpu_to_be32(wlan_mode);
+
+        return ar5523_cmd_write(ar, WDCMSG_CREATE_CONNECTION, &create,
+                                sizeof(create), 0);
+}
+
+static int ar5523_write_associd(struct ar5523 *ar,
+                                struct ieee80211_bss_conf *bss)
+{
+        struct ar5523_cmd_set_associd associd;
+
+        memset(&associd, 0, sizeof(associd));
+        associd.defaultrateix = cpu_to_be32(0); /* XXX */
+        associd.associd = cpu_to_be32(bss->aid);
+        associd.timoffset = cpu_to_be32(0x3b);  /* XXX */
+        memcpy(associd.bssid, bss->bssid, ETH_ALEN);
+        return ar5523_cmd_write(ar, WDCMSG_WRITE_ASSOCID, &associd,
+                                sizeof(associd), 0);
+}
+
+static void ar5523_bss_info_changed(struct ieee80211_hw *hw,
+                                    struct ieee80211_vif *vif,
+                                    struct ieee80211_bss_conf *bss,
+                                    u32 changed)
+{
+        struct ar5523 *ar = hw->priv;
+        int error;
+
+        ar5523_dbg(ar, "bss_info_changed called\n");
+        mutex_lock(&ar->mutex);
+
+        if (!(changed & BSS_CHANGED_ASSOC))
+                goto out_unlock;
+
+        if (bss->assoc) {
+                error = ar5523_create_connection(ar, vif, bss);
+                if (error) {
+                        ar5523_err(ar, "could not create connection\n");
+                        goto out_unlock;
+                }
+
+                error = ar5523_set_basic_rates(ar, bss);
+                if (error) {
+                        ar5523_err(ar, "could not set negotiated rate set\n");
+                        goto out_unlock;
+                }
+
+                error = ar5523_write_associd(ar, bss);
+                if (error) {
+                        ar5523_err(ar, "could not set association\n");
+                        goto out_unlock;
+                }
+
+                /* turn link LED on */
+                ar5523_set_ledsteady(ar, UATH_LED_LINK, UATH_LED_ON);
+                set_bit(AR5523_CONNECTED, &ar->flags);
+                ieee80211_queue_delayed_work(hw, &ar->stat_work, HZ);
+
+        } else {
+                cancel_delayed_work(&ar->stat_work);
+                clear_bit(AR5523_CONNECTED, &ar->flags);
+                ar5523_set_ledsteady(ar, UATH_LED_LINK, UATH_LED_OFF);
+        }
+
+out_unlock:
+        mutex_unlock(&ar->mutex);
+
+}
+
+#define AR5523_SUPPORTED_FILTERS (FIF_PROMISC_IN_BSS | \
+                                  FIF_ALLMULTI | \
+                                  FIF_FCSFAIL | \
+                                  FIF_OTHER_BSS)
+
+static void ar5523_configure_filter(struct ieee80211_hw *hw,
+                                    unsigned int changed_flags,
+                                    unsigned int *total_flags,
+                                    u64 multicast)
+{
+        struct ar5523 *ar = hw->priv;
+        u32 filter = 0;
+
+        ar5523_dbg(ar, "configure_filter called\n");
+        mutex_lock(&ar->mutex);
+        ar5523_flush_tx(ar);
+
+        *total_flags &= AR5523_SUPPORTED_FILTERS;
+
+        /* The filters seems strange. UATH_FILTER_RX_BCAST and
+         * UATH_FILTER_RX_MCAST does not result in those frames being RXed.
+         * The only way I have found to get [mb]cast frames seems to be
+         * to set UATH_FILTER_RX_PROM. */
+        filter |= UATH_FILTER_RX_UCAST | UATH_FILTER_RX_MCAST |
+                  UATH_FILTER_RX_BCAST | UATH_FILTER_RX_BEACON |
+                  UATH_FILTER_RX_PROM;
+
+        ar5523_set_rxfilter(ar, 0, UATH_FILTER_OP_INIT);
+        ar5523_set_rxfilter(ar, filter, UATH_FILTER_OP_SET);
+
+        mutex_unlock(&ar->mutex);
+}
+
+static const struct ieee80211_ops ar5523_ops = {
+        .start                  = ar5523_start,
+        .stop                   = ar5523_stop,
+        .tx                     = ar5523_tx,
+        .set_rts_threshold      = ar5523_set_rts_threshold,
+        .add_interface          = ar5523_add_interface,
+        .remove_interface       = ar5523_remove_interface,
+        .config                 = ar5523_hwconfig,
+        .bss_info_changed       = ar5523_bss_info_changed,
+        .configure_filter       = ar5523_configure_filter,
+        .flush                  = ar5523_flush,
+};
+
+static int ar5523_host_available(struct ar5523 *ar)
+{
+        struct ar5523_cmd_host_available setup;
+
+        /* inform target the host is available */
+        setup.sw_ver_major = cpu_to_be32(ATH_SW_VER_MAJOR);
+        setup.sw_ver_minor = cpu_to_be32(ATH_SW_VER_MINOR);
+        setup.sw_ver_patch = cpu_to_be32(ATH_SW_VER_PATCH);
+        setup.sw_ver_build = cpu_to_be32(ATH_SW_VER_BUILD);
+        return ar5523_cmd_read(ar, WDCMSG_HOST_AVAILABLE,
+                               &setup, sizeof(setup), NULL, 0, 0);
+}
+
+static int ar5523_get_devstatus(struct ar5523 *ar)
+{
+        u8 macaddr[ETH_ALEN];
+        int error;
+
+        /* retrieve MAC address */
+        error = ar5523_get_status(ar, ST_MAC_ADDR, macaddr, ETH_ALEN);
+        if (error) {
+                ar5523_err(ar, "could not read MAC address\n");
+                return error;
+        }
+
+        SET_IEEE80211_PERM_ADDR(ar->hw, macaddr);
+
+        error = ar5523_get_status(ar, ST_SERIAL_NUMBER,
+            &ar->serial[0], sizeof(ar->serial));
+        if (error) {
+                ar5523_err(ar, "could not read device serial number\n");
+                return error;
+        }
+        return 0;
+}
+
+#define AR5523_SANE_RXBUFSZ 2000
+
+static int ar5523_get_max_rxsz(struct ar5523 *ar)
+{
+        int error;
+        __be32 rxsize;
+
+        /* Get max rx size */
+        error = ar5523_get_status(ar, ST_WDC_TRANSPORT_CHUNK_SIZE, &rxsize,
+                                  sizeof(rxsize));
+        if (error != 0) {
+                ar5523_err(ar, "could not read max RX size\n");
+                return error;
+        }
+
+        ar->rxbufsz = be32_to_cpu(rxsize);
+
+        if (!ar->rxbufsz || ar->rxbufsz > AR5523_SANE_RXBUFSZ) {
+                ar5523_err(ar, "Bad rxbufsz from device. Using %d instead\n",
+                           AR5523_SANE_RXBUFSZ);
+                ar->rxbufsz = AR5523_SANE_RXBUFSZ;
+        }
+
+        ar5523_dbg(ar, "Max RX buf size: %d\n", ar->rxbufsz);
+        return 0;
+}
+
+/*
+ * This is copied from rtl818x, but we should probably move this
+ * to common code as in OpenBSD.
+ */
+static const struct ieee80211_rate ar5523_rates[] = {
+        { .bitrate = 10, .hw_value = 2, },
+        { .bitrate = 20, .hw_value = 4 },
+        { .bitrate = 55, .hw_value = 11, },
+        { .bitrate = 110, .hw_value = 22, },
+        { .bitrate = 60, .hw_value = 12, },
+        { .bitrate = 90, .hw_value = 18, },
+        { .bitrate = 120, .hw_value = 24, },
+        { .bitrate = 180, .hw_value = 36, },
+        { .bitrate = 240, .hw_value = 48, },
+        { .bitrate = 360, .hw_value = 72, },
+        { .bitrate = 480, .hw_value = 96, },
+        { .bitrate = 540, .hw_value = 108, },
+};
+
+static const struct ieee80211_channel ar5523_channels[] = {
+        { .center_freq = 2412 },
+        { .center_freq = 2417 },
+        { .center_freq = 2422 },
+        { .center_freq = 2427 },
+        { .center_freq = 2432 },
+        { .center_freq = 2437 },
+        { .center_freq = 2442 },
+        { .center_freq = 2447 },
+        { .center_freq = 2452 },
+        { .center_freq = 2457 },
+        { .center_freq = 2462 },
+        { .center_freq = 2467 },
+        { .center_freq = 2472 },
+        { .center_freq = 2484 },
+};
+
+static int ar5523_init_modes(struct ar5523 *ar)
+{
+        BUILD_BUG_ON(sizeof(ar->channels) != sizeof(ar5523_channels));
+        BUILD_BUG_ON(sizeof(ar->rates) != sizeof(ar5523_rates));
+
+        memcpy(ar->channels, ar5523_channels, sizeof(ar5523_channels));
+        memcpy(ar->rates, ar5523_rates, sizeof(ar5523_rates));
+
+        ar->band.band = IEEE80211_BAND_2GHZ;
+        ar->band.channels = ar->channels;
+        ar->band.n_channels = ARRAY_SIZE(ar5523_channels);
+        ar->band.bitrates = ar->rates;
+        ar->band.n_bitrates = ARRAY_SIZE(ar5523_rates);
+        ar->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &ar->band;
+        return 0;
+}
+
+/*
+ * Load the MIPS R4000 microcode into the device.  Once the image is loaded,
+ * the device will detach itself from the bus and reattach later with a new
+ * product Id (a la ezusb).
+ */
+static int ar5523_load_firmware(struct usb_device *dev)
+{
+        struct ar5523_fwblock *txblock, *rxblock;
+        const struct firmware *fw;
+        void *fwbuf;
+        int len, offset;
+        int foolen; /* XXX(hch): handle short transfers */
+        int error = -ENXIO;
+
+        if (request_firmware(&fw, AR5523_FIRMWARE_FILE, &dev->dev)) {
+                dev_err(&dev->dev, "no firmware found: %s\n",
+                        AR5523_FIRMWARE_FILE);
+                return -ENOENT;
+        }
+
+        txblock = kmalloc(sizeof(*txblock), GFP_KERNEL);
+        if (!txblock)
+                goto out;
+
+        rxblock = kmalloc(sizeof(*rxblock), GFP_KERNEL);
+        if (!rxblock)
+                goto out_free_txblock;
+
+        fwbuf = kmalloc(AR5523_MAX_FWBLOCK_SIZE, GFP_KERNEL);
+        if (!fwbuf)
+                goto out_free_rxblock;
+
+        memset(txblock, 0, sizeof(struct ar5523_fwblock));
+        txblock->flags = cpu_to_be32(AR5523_WRITE_BLOCK);
+        txblock->total = cpu_to_be32(fw->size);
+
+        offset = 0;
+        len = fw->size;
+        while (len > 0) {
+                int mlen = min(len, AR5523_MAX_FWBLOCK_SIZE);
+
+                txblock->remain = cpu_to_be32(len - mlen);
+                txblock->len = cpu_to_be32(mlen);
+
+                /* send firmware block meta-data */
+                error = usb_bulk_msg(dev, ar5523_cmd_tx_pipe(dev),
+                                     txblock, sizeof(*txblock), &foolen,
+                                     AR5523_CMD_TIMEOUT);
+                if (error) {
+                        dev_err(&dev->dev,
+                                "could not send firmware block info\n");
+                        goto out_free_fwbuf;
+                }
+
+                /* send firmware block data */
+                memcpy(fwbuf, fw->data + offset, mlen);
+                error = usb_bulk_msg(dev, ar5523_data_tx_pipe(dev),
+                                     fwbuf, mlen, &foolen,
+                                     AR5523_DATA_TIMEOUT);
+                if (error) {
+                        dev_err(&dev->dev,
+                                "could not send firmware block data\n");
+                        goto out_free_fwbuf;
+                }
+
+                /* wait for ack from firmware */
+                error = usb_bulk_msg(dev, ar5523_cmd_rx_pipe(dev),
+                                     rxblock, sizeof(*rxblock), &foolen,
+                                     AR5523_CMD_TIMEOUT);
+                if (error) {
+                        dev_err(&dev->dev,
+                                "could not read firmware answer\n");
+                        goto out_free_fwbuf;
+                }
+
+                len -= mlen;
+                offset += mlen;
+        }
+
+        /*
+         * Set the error to -ENXIO to make sure we continue probing for
+         * a driver.
+         */
+        error = -ENXIO;
+
+ out_free_fwbuf:
+        kfree(fwbuf);
+ out_free_rxblock:
+        kfree(rxblock);
+ out_free_txblock:
+        kfree(txblock);
+ out:
+        release_firmware(fw);
+        return error;
+}
+
+static int ar5523_probe(struct usb_interface *intf,
+                        const struct usb_device_id *id)
+{
+        struct usb_device *dev = interface_to_usbdev(intf);
+        struct ieee80211_hw *hw;
+        struct ar5523 *ar;
+        int error = -ENOMEM;
+
+        /*
+         * Load firmware if the device requires it.  This will return
+         * -ENXIO on success and we'll get called back afer the usb
+         * id changes to indicate that the firmware is present.
+         */
+        if (id->driver_info & AR5523_FLAG_PRE_FIRMWARE)
+                return ar5523_load_firmware(dev);
+
+
+        hw = ieee80211_alloc_hw(sizeof(*ar), &ar5523_ops);
+        if (!hw)
+                goto out;
+        SET_IEEE80211_DEV(hw, &intf->dev);
+
+        ar = hw->priv;
+        ar->hw = hw;
+        ar->dev = dev;
+        mutex_init(&ar->mutex);
+
+        INIT_DELAYED_WORK(&ar->stat_work, ar5523_stat_work);
+        init_timer(&ar->tx_wd_timer);
+        setup_timer(&ar->tx_wd_timer, ar5523_tx_wd_timer, (unsigned long) ar);
+        INIT_WORK(&ar->tx_wd_work, ar5523_tx_wd_work);
+        INIT_WORK(&ar->tx_work, ar5523_tx_work);
+        INIT_LIST_HEAD(&ar->tx_queue_pending);
+        INIT_LIST_HEAD(&ar->tx_queue_submitted);
+        spin_lock_init(&ar->tx_data_list_lock);
+        atomic_set(&ar->tx_nr_total, 0);
+        atomic_set(&ar->tx_nr_pending, 0);
+        init_waitqueue_head(&ar->tx_flush_waitq);
+
+        atomic_set(&ar->rx_data_free_cnt, 0);
+        INIT_WORK(&ar->rx_refill_work, ar5523_rx_refill_work);
+        INIT_LIST_HEAD(&ar->rx_data_free);
+        INIT_LIST_HEAD(&ar->rx_data_used);
+        spin_lock_init(&ar->rx_data_list_lock);
+
+        ar->wq = create_singlethread_workqueue("ar5523");
+        if (!ar->wq) {
+                ar5523_err(ar, "Could not create wq\n");
+                goto out_free_ar;
+        }
+
+        error = ar5523_alloc_rx_bufs(ar);
+        if (error) {
+                ar5523_err(ar, "Could not allocate rx buffers\n");
+                goto out_free_wq;
+        }
+
+        error = ar5523_alloc_rx_cmd(ar);
+        if (error) {
+                ar5523_err(ar, "Could not allocate rx command buffers\n");
+                goto out_free_rx_bufs;
+        }
+
+        error = ar5523_alloc_tx_cmd(ar);
+        if (error) {
+                ar5523_err(ar, "Could not allocate tx command buffers\n");
+                goto out_free_rx_cmd;
+        }
+
+        error = ar5523_submit_rx_cmd(ar);
+        if (error) {
+                ar5523_err(ar, "Failed to submit rx cmd\n");
+                goto out_free_tx_cmd;
+        }
+
+        /*
+         * We're now ready to send/receive firmware commands.
+         */
+        error = ar5523_host_available(ar);
+        if (error) {
+                ar5523_err(ar, "could not initialize adapter\n");
+                goto out_cancel_rx_cmd;
+        }
+
+        error = ar5523_get_max_rxsz(ar);
+        if (error) {
+                ar5523_err(ar, "could not get caps from adapter\n");
+                goto out_cancel_rx_cmd;
+        }
+
+        error = ar5523_get_devcap(ar);
+        if (error) {
+                ar5523_err(ar, "could not get caps from adapter\n");
+                goto out_cancel_rx_cmd;
+        }
+
+        error = ar5523_get_devstatus(ar);
+        if (error != 0) {
+                ar5523_err(ar, "could not get device status\n");
+                goto out_cancel_rx_cmd;
+        }
+
+        ar5523_info(ar, "MAC/BBP AR5523, RF AR%c112\n",
+                        (id->driver_info & AR5523_FLAG_ABG) ? '5' : '2');
+
+        ar->vif = NULL;
+        hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+                    IEEE80211_HW_SIGNAL_DBM |
+                    IEEE80211_HW_HAS_RATE_CONTROL;
+        hw->extra_tx_headroom = sizeof(struct ar5523_tx_desc) +
+                                sizeof(struct ar5523_chunk);
+        hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+        hw->queues = 1;
+
+        error = ar5523_init_modes(ar);
+        if (error)
+                goto out_cancel_rx_cmd;
+
+        usb_set_intfdata(intf, hw);
+
+        error = ieee80211_register_hw(hw);
+        if (error) {
+                ar5523_err(ar, "could not register device\n");
+                goto out_cancel_rx_cmd;
+        }
+
+        ar5523_info(ar, "Found and initialized AR5523 device\n");
+        return 0;
+
+out_cancel_rx_cmd:
+        ar5523_cancel_rx_cmd(ar);
+out_free_tx_cmd:
+        ar5523_free_tx_cmd(ar);
+out_free_rx_cmd:
+        ar5523_free_rx_cmd(ar);
+out_free_rx_bufs:
+        ar5523_free_rx_bufs(ar);
+out_free_wq:
+        destroy_workqueue(ar->wq);
+out_free_ar:
+        ieee80211_free_hw(hw);
+out:
+        return error;
+}
+
+static void ar5523_disconnect(struct usb_interface *intf)
+{
+        struct ieee80211_hw *hw = usb_get_intfdata(intf);
+        struct ar5523 *ar = hw->priv;
+
+        ar5523_dbg(ar, "detaching\n");
+        set_bit(AR5523_USB_DISCONNECTED, &ar->flags);
+
+        ieee80211_unregister_hw(hw);
+
+        ar5523_cancel_rx_cmd(ar);
+        ar5523_free_tx_cmd(ar);
+        ar5523_free_rx_cmd(ar);
+        ar5523_free_rx_bufs(ar);
+
+        destroy_workqueue(ar->wq);
+
+        ieee80211_free_hw(hw);
+        usb_set_intfdata(intf, NULL);
+}
+
+#define AR5523_DEVICE_UG(vendor, device) \
+        { USB_DEVICE((vendor), (device)) }, \
+        { USB_DEVICE((vendor), (device) + 1), \
+                .driver_info = AR5523_FLAG_PRE_FIRMWARE }
+#define AR5523_DEVICE_UX(vendor, device) \
+        { USB_DEVICE((vendor), (device)), \
+                .driver_info = AR5523_FLAG_ABG }, \
+        { USB_DEVICE((vendor), (device) + 1), \
+                .driver_info = AR5523_FLAG_ABG|AR5523_FLAG_PRE_FIRMWARE }
+
+static struct usb_device_id ar5523_id_table[] = {
+        AR5523_DEVICE_UG(0x168c, 0x0001),       /* Atheros / AR5523 */
+        AR5523_DEVICE_UG(0x0cf3, 0x0001),       /* Atheros2 / AR5523_1 */
+        AR5523_DEVICE_UG(0x0cf3, 0x0003),       /* Atheros2 / AR5523_2 */
+        AR5523_DEVICE_UX(0x0cf3, 0x0005),       /* Atheros2 / AR5523_3 */
+        AR5523_DEVICE_UG(0x0d8e, 0x7801),       /* Conceptronic / AR5523_1 */
+        AR5523_DEVICE_UX(0x0d8e, 0x7811),       /* Conceptronic / AR5523_2 */
+        AR5523_DEVICE_UX(0x2001, 0x3a00),       /* Dlink / DWLAG132 */
+        AR5523_DEVICE_UG(0x2001, 0x3a02),       /* Dlink / DWLG132 */
+        AR5523_DEVICE_UX(0x2001, 0x3a04),       /* Dlink / DWLAG122 */
+        AR5523_DEVICE_UG(0x1690, 0x0712),       /* Gigaset / AR5523 */
+        AR5523_DEVICE_UG(0x1690, 0x0710),       /* Gigaset / SMCWUSBTG */
+        AR5523_DEVICE_UG(0x129b, 0x160c),       /* Gigaset / USB stick 108
+                                                   (CyberTAN Technology) */
+        AR5523_DEVICE_UG(0x16ab, 0x7801),       /* Globalsun / AR5523_1 */
+        AR5523_DEVICE_UX(0x16ab, 0x7811),       /* Globalsun / AR5523_2 */
+        AR5523_DEVICE_UG(0x0d8e, 0x7802),       /* Globalsun / AR5523_3 */
+        AR5523_DEVICE_UX(0x0846, 0x4300),       /* Netgear / WG111U */
+        AR5523_DEVICE_UG(0x0846, 0x4250),       /* Netgear / WG111T */
+        AR5523_DEVICE_UG(0x0846, 0x5f00),       /* Netgear / WPN111 */
+        AR5523_DEVICE_UG(0x157e, 0x3006),       /* Umedia / AR5523_1 */
+        AR5523_DEVICE_UX(0x157e, 0x3205),       /* Umedia / AR5523_2 */
+        AR5523_DEVICE_UG(0x157e, 0x3006),       /* Umedia / TEW444UBEU */
+        AR5523_DEVICE_UG(0x1435, 0x0826),       /* Wistronneweb / AR5523_1 */
+        AR5523_DEVICE_UX(0x1435, 0x0828),       /* Wistronneweb / AR5523_2 */
+        AR5523_DEVICE_UG(0x0cde, 0x0012),       /* Zcom / AR5523 */
+        AR5523_DEVICE_UG(0x1385, 0x4250),       /* Netgear3 / WG111T (2) */
+        AR5523_DEVICE_UG(0x1385, 0x5f00),       /* Netgear / WPN111 */
+        AR5523_DEVICE_UG(0x1385, 0x5f02),       /* Netgear / WPN111 */
+        { }
+};
+MODULE_DEVICE_TABLE(usb, ar5523_id_table);
+
+static struct usb_driver ar5523_driver = {
+        .name           = "ar5523",
+        .id_table       = ar5523_id_table,
+        .probe          = ar5523_probe,
+        .disconnect     = ar5523_disconnect,
+};
+
+static int __init ar5523_init(void)
+{
+        return usb_register(&ar5523_driver);
+}
+
+static void __exit ar5523_exit(void)
+{
+        usb_deregister(&ar5523_driver);
+}
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_FIRMWARE(AR5523_FIRMWARE_FILE);
+
+module_init(ar5523_init);
+module_exit(ar5523_exit);
diff --git a/drivers/net/wireless/ath/ar5523/ar5523.h b/drivers/net/wireless/ath/ar5523/ar5523.h
new file mode 100644
index 000000000000..6086ba3fb543
--- /dev/null
+++ b/drivers/net/wireless/ath/ar5523/ar5523.h
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2006 Damien Bergamini <damien.bergamini@free.fr>
+ * Copyright (c) 2006 Sam Leffler, Errno Consulting
+ * Copyright (c) 2007 Christoph Hellwig <hch@lst.de>
+ * Copyright (c) 2008-2009 Weongyo Jeong <weongyo@freebsd.org>
+ * Copyright (c) 2012 Pontus Fuchs <pontus.fuchs@gmail.com>
+ *
+ * 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.
+ */
+
+#define AR5523_FLAG_PRE_FIRMWARE        (1 << 0)
+#define AR5523_FLAG_ABG                 (1 << 1)
+
+#define AR5523_FIRMWARE_FILE    "ar5523.bin"
+
+#define AR5523_CMD_TX_PIPE      0x01
+#define AR5523_DATA_TX_PIPE     0x02
+#define AR5523_CMD_RX_PIPE      0x81
+#define AR5523_DATA_RX_PIPE     0x82
+
+#define ar5523_cmd_tx_pipe(dev) \
+        usb_sndbulkpipe((dev), AR5523_CMD_TX_PIPE)
+#define ar5523_data_tx_pipe(dev) \
+        usb_sndbulkpipe((dev), AR5523_DATA_TX_PIPE)
+#define ar5523_cmd_rx_pipe(dev) \
+        usb_rcvbulkpipe((dev), AR5523_CMD_RX_PIPE)
+#define ar5523_data_rx_pipe(dev) \
+        usb_rcvbulkpipe((dev), AR5523_DATA_RX_PIPE)
+
+#define AR5523_DATA_TIMEOUT     10000
+#define AR5523_CMD_TIMEOUT      1000
+
+#define AR5523_TX_DATA_COUNT            8
+#define AR5523_TX_DATA_RESTART_COUNT    2
+#define AR5523_RX_DATA_COUNT            16
+#define AR5523_RX_DATA_REFILL_COUNT     8
+
+#define AR5523_CMD_ID   1
+#define AR5523_DATA_ID  2
+
+#define AR5523_TX_WD_TIMEOUT    (HZ * 2)
+#define AR5523_FLUSH_TIMEOUT    (HZ * 3)
+
+enum AR5523_flags {
+        AR5523_HW_UP,
+        AR5523_USB_DISCONNECTED,
+        AR5523_CONNECTED
+};
+
+struct ar5523_tx_cmd {
+        struct ar5523           *ar;
+        struct urb              *urb_tx;
+        void                    *buf_tx;
+        void                    *odata;
+        int                     olen;
+        int                     flags;
+        int                     res;
+        struct completion       done;
+};
+
+/* This struct is placed in tx_info->driver_data. It must not be larger
+ *  than IEEE80211_TX_INFO_DRIVER_DATA_SIZE.
+ */
+struct ar5523_tx_data {
+        struct list_head        list;
+        struct ar5523           *ar;
+        struct sk_buff          *skb;
+        struct urb              *urb;
+};
+
+struct ar5523_rx_data {
+        struct  list_head       list;
+        struct ar5523           *ar;
+        struct urb              *urb;
+        struct sk_buff          *skb;
+};
+
+struct ar5523 {
+        struct usb_device       *dev;
+        struct ieee80211_hw     *hw;
+
+        unsigned long           flags;
+        struct mutex            mutex;
+        struct workqueue_struct *wq;
+
+        struct ar5523_tx_cmd    tx_cmd;
+
+        struct delayed_work     stat_work;
+
+        struct timer_list       tx_wd_timer;
+        struct work_struct      tx_wd_work;
+        struct work_struct      tx_work;
+        struct list_head        tx_queue_pending;
+        struct list_head        tx_queue_submitted;
+        spinlock_t              tx_data_list_lock;
+        wait_queue_head_t       tx_flush_waitq;
+
+        /* Queued + Submitted TX frames */
+        atomic_t                tx_nr_total;
+
+        /* Submitted TX frames */
+        atomic_t                tx_nr_pending;
+
+        void                    *rx_cmd_buf;
+        struct urb              *rx_cmd_urb;
+
+        struct ar5523_rx_data   rx_data[AR5523_RX_DATA_COUNT];
+        spinlock_t              rx_data_list_lock;
+        struct list_head        rx_data_free;
+        struct list_head        rx_data_used;
+        atomic_t                rx_data_free_cnt;
+
+        struct work_struct      rx_refill_work;
+
+        int                     rxbufsz;
+        u8                      serial[16];
+
+        struct ieee80211_channel channels[14];
+        struct ieee80211_rate   rates[12];
+        struct ieee80211_supported_band band;
+        struct ieee80211_vif    *vif;
+};
+
+/* flags for sending firmware commands */
+#define AR5523_CMD_FLAG_READ    (1 << 1)
+#define AR5523_CMD_FLAG_MAGIC   (1 << 2)
+
+#define ar5523_dbg(ar, format, arg...) \
+        dev_dbg(&(ar)->dev->dev, format, ## arg)
+
+/* On USB hot-unplug there can be a lot of URBs in flight and they'll all
+ * fail. Instead of dealing with them in every possible place just surpress
+ * any messages on USB disconnect.
+ */
+#define ar5523_err(ar, format, arg...) \
+do { \
+        if (!test_bit(AR5523_USB_DISCONNECTED, &ar->flags)) { \
+                dev_err(&(ar)->dev->dev, format, ## arg); \
+        } \
+} while (0)
+#define ar5523_info(ar, format, arg...) \
+        dev_info(&(ar)->dev->dev, format, ## arg)
diff --git a/drivers/net/wireless/ath/ar5523/ar5523_hw.h b/drivers/net/wireless/ath/ar5523/ar5523_hw.h
new file mode 100644
index 000000000000..a0e8bf460316
--- /dev/null
+++ b/drivers/net/wireless/ath/ar5523/ar5523_hw.h
@@ -0,0 +1,431 @@
+/*
+ * Copyright (c) 2006 Damien Bergamini <damien.bergamini@free.fr>
+ * Copyright (c) 2006 Sam Leffler, Errno Consulting
+ * Copyright (c) 2007 Christoph Hellwig <hch@lst.de>
+ * Copyright (c) 2008-2009 Weongyo Jeong <weongyo@freebsd.org>
+ * Copyright (c) 2012 Pontus Fuchs <pontus.fuchs@gmail.com>
+ *
+ * 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.
+ */
+
+/* all fields are big endian */
+struct ar5523_fwblock {
+        __be32          flags;
+#define AR5523_WRITE_BLOCK      (1 << 4)
+
+        __be32  len;
+#define AR5523_MAX_FWBLOCK_SIZE 2048
+
+        __be32          total;
+        __be32          remain;
+        __be32          rxtotal;
+        __be32          pad[123];
+} __packed;
+
+#define AR5523_MAX_RXCMDSZ      1024
+#define AR5523_MAX_TXCMDSZ      1024
+
+struct ar5523_cmd_hdr {
+        __be32          len;
+        __be32          code;
+/* NB: these are defined for rev 1.5 firmware; rev 1.6 is different */
+/* messages from Host -> Target */
+#define WDCMSG_HOST_AVAILABLE           0x01
+#define WDCMSG_BIND                     0x02
+#define WDCMSG_TARGET_RESET             0x03
+#define WDCMSG_TARGET_GET_CAPABILITY    0x04
+#define WDCMSG_TARGET_SET_CONFIG        0x05
+#define WDCMSG_TARGET_GET_STATUS        0x06
+#define WDCMSG_TARGET_GET_STATS         0x07
+#define WDCMSG_TARGET_START             0x08
+#define WDCMSG_TARGET_STOP              0x09
+#define WDCMSG_TARGET_ENABLE            0x0a
+#define WDCMSG_TARGET_DISABLE           0x0b
+#define WDCMSG_CREATE_CONNECTION        0x0c
+#define WDCMSG_UPDATE_CONNECT_ATTR      0x0d
+#define WDCMSG_DELETE_CONNECT           0x0e
+#define WDCMSG_SEND                     0x0f
+#define WDCMSG_FLUSH                    0x10
+/* messages from Target -> Host */
+#define WDCMSG_STATS_UPDATE             0x11
+#define WDCMSG_BMISS                    0x12
+#define WDCMSG_DEVICE_AVAIL             0x13
+#define WDCMSG_SEND_COMPLETE            0x14
+#define WDCMSG_DATA_AVAIL               0x15
+#define WDCMSG_SET_PWR_MODE             0x16
+#define WDCMSG_BMISS_ACK                0x17
+#define WDCMSG_SET_LED_STEADY           0x18
+#define WDCMSG_SET_LED_BLINK            0x19
+/* more messages */
+#define WDCMSG_SETUP_BEACON_DESC        0x1a
+#define WDCMSG_BEACON_INIT              0x1b
+#define WDCMSG_RESET_KEY_CACHE          0x1c
+#define WDCMSG_RESET_KEY_CACHE_ENTRY    0x1d
+#define WDCMSG_SET_KEY_CACHE_ENTRY      0x1e
+#define WDCMSG_SET_DECOMP_MASK          0x1f
+#define WDCMSG_SET_REGULATORY_DOMAIN    0x20
+#define WDCMSG_SET_LED_STATE            0x21
+#define WDCMSG_WRITE_ASSOCID            0x22
+#define WDCMSG_SET_STA_BEACON_TIMERS    0x23
+#define WDCMSG_GET_TSF                  0x24
+#define WDCMSG_RESET_TSF                0x25
+#define WDCMSG_SET_ADHOC_MODE           0x26
+#define WDCMSG_SET_BASIC_RATE           0x27
+#define WDCMSG_MIB_CONTROL              0x28
+#define WDCMSG_GET_CHANNEL_DATA         0x29
+#define WDCMSG_GET_CUR_RSSI             0x2a
+#define WDCMSG_SET_ANTENNA_SWITCH       0x2b
+#define WDCMSG_USE_SHORT_SLOT_TIME      0x2f
+#define WDCMSG_SET_POWER_MODE           0x30
+#define WDCMSG_SETUP_PSPOLL_DESC        0x31
+#define WDCMSG_SET_RX_MULTICAST_FILTER  0x32
+#define WDCMSG_RX_FILTER                0x33
+#define WDCMSG_PER_CALIBRATION          0x34
+#define WDCMSG_RESET                    0x35
+#define WDCMSG_DISABLE                  0x36
+#define WDCMSG_PHY_DISABLE              0x37
+#define WDCMSG_SET_TX_POWER_LIMIT       0x38
+#define WDCMSG_SET_TX_QUEUE_PARAMS      0x39
+#define WDCMSG_SETUP_TX_QUEUE           0x3a
+#define WDCMSG_RELEASE_TX_QUEUE         0x3b
+#define WDCMSG_SET_DEFAULT_KEY          0x43
+
+        __u32           priv;   /* driver private data,
+                                   don't care about endianess */
+        __be32          magic;
+        __be32          reserved2[4];
+};
+
+struct ar5523_cmd_host_available {
+        __be32  sw_ver_major;
+        __be32  sw_ver_minor;
+        __be32  sw_ver_patch;
+        __be32  sw_ver_build;
+} __packed;
+
+#define ATH_SW_VER_MAJOR        1
+#define ATH_SW_VER_MINOR        5
+#define ATH_SW_VER_PATCH        0
+#define ATH_SW_VER_BUILD        9999
+
+struct ar5523_chunk {
+        u8              seqnum;         /* sequence number for ordering */
+        u8              flags;
+#define UATH_CFLAGS_FINAL       0x01    /* final chunk of a msg */
+#define UATH_CFLAGS_RXMSG       0x02    /* chunk contains rx completion */
+#define UATH_CFLAGS_DEBUG       0x04    /* for debugging */
+        __be16          length;         /* chunk size in bytes */
+        /* chunk data follows */
+} __packed;
+
+/*
+ * Message format for a WDCMSG_DATA_AVAIL message from Target to Host.
+ */
+struct ar5523_rx_desc {
+        __be32  len;            /* msg length including header */
+        __be32  code;           /* WDCMSG_DATA_AVAIL */
+        __be32  gennum;         /* generation number */
+        __be32  status;         /* start of RECEIVE_INFO */
+#define UATH_STATUS_OK                  0
+#define UATH_STATUS_STOP_IN_PROGRESS    1
+#define UATH_STATUS_CRC_ERR             2
+#define UATH_STATUS_PHY_ERR             3
+#define UATH_STATUS_DECRYPT_CRC_ERR     4
+#define UATH_STATUS_DECRYPT_MIC_ERR     5
+#define UATH_STATUS_DECOMP_ERR          6
+#define UATH_STATUS_KEY_ERR             7
+#define UATH_STATUS_ERR                 8
+        __be32  tstamp_low;     /* low-order 32-bits of rx timestamp */
+        __be32  tstamp_high;    /* high-order 32-bits of rx timestamp */
+        __be32  framelen;       /* frame length */
+        __be32  rate;           /* rx rate code */
+        __be32  antenna;
+        __be32  rssi;
+        __be32  channel;
+        __be32  phyerror;
+        __be32  connix;         /* key table ix for bss traffic */
+        __be32  decrypterror;
+        __be32  keycachemiss;
+        __be32  pad;            /* XXX? */
+} __packed;
+
+struct ar5523_tx_desc {
+        __be32  msglen;
+        __be32  msgid;          /* msg id (supplied by host) */
+        __be32  type;           /* opcode: WDMSG_SEND or WDCMSG_FLUSH */
+        __be32  txqid;          /* tx queue id and flags */
+#define UATH_TXQID_MASK         0x0f
+#define UATH_TXQID_MINRATE      0x10    /* use min tx rate */
+#define UATH_TXQID_FF           0x20    /* content is fast frame */
+        __be32  connid;         /* tx connection id */
+#define UATH_ID_INVALID 0xffffffff      /* for sending prior to connection */
+        __be32  flags;          /* non-zero if response desired */
+#define UATH_TX_NOTIFY  (1 << 24)       /* f/w will send a UATH_NOTIF_TX */
+        __be32  buflen;         /* payload length */
+} __packed;
+
+
+#define AR5523_ID_BSS           2
+#define AR5523_ID_BROADCAST     0xffffffff
+
+/* structure for command UATH_CMD_WRITE_MAC */
+struct ar5523_write_mac {
+        __be32  reg;
+        __be32  len;
+        u8              data[32];
+} __packed;
+
+struct ar5523_cmd_rateset {
+        __u8            length;
+#define AR5523_MAX_NRATES       32
+        __u8            set[AR5523_MAX_NRATES];
+};
+
+struct ar5523_cmd_set_associd {         /* AR5523_WRITE_ASSOCID */
+        __be32  defaultrateix;
+        __be32  associd;
+        __be32  timoffset;
+        __be32  turboprime;
+        __u8    bssid[6];
+} __packed;
+
+/* structure for command WDCMSG_RESET */
+struct ar5523_cmd_reset {
+        __be32  flags;          /* channel flags */
+#define UATH_CHAN_TURBO 0x0100
+#define UATH_CHAN_CCK   0x0200
+#define UATH_CHAN_OFDM  0x0400
+#define UATH_CHAN_2GHZ  0x1000
+#define UATH_CHAN_5GHZ  0x2000
+        __be32  freq;           /* channel frequency */
+        __be32  maxrdpower;
+        __be32  cfgctl;
+        __be32  twiceantennareduction;
+        __be32  channelchange;
+        __be32  keeprccontent;
+} __packed;
+
+/* structure for command WDCMSG_SET_BASIC_RATE */
+struct ar5523_cmd_rates {
+        __be32  connid;
+        __be32  keeprccontent;
+        __be32  size;
+        struct ar5523_cmd_rateset rateset;
+} __packed;
+
+enum {
+        WLAN_MODE_NONE = 0,
+        WLAN_MODE_11b,
+        WLAN_MODE_11a,
+        WLAN_MODE_11g,
+        WLAN_MODE_11a_TURBO,
+        WLAN_MODE_11g_TURBO,
+        WLAN_MODE_11a_TURBO_PRIME,
+        WLAN_MODE_11g_TURBO_PRIME,
+        WLAN_MODE_11a_XR,
+        WLAN_MODE_11g_XR,
+};
+
+struct ar5523_cmd_connection_attr {
+        __be32  longpreambleonly;
+        struct ar5523_cmd_rateset       rateset;
+        __be32  wlanmode;
+} __packed;
+
+/* structure for command AR5523_CREATE_CONNECTION */
+struct ar5523_cmd_create_connection {
+        __be32  connid;
+        __be32  bssid;
+        __be32  size;
+        struct ar5523_cmd_connection_attr       connattr;
+} __packed;
+
+struct ar5523_cmd_ledsteady {           /* WDCMSG_SET_LED_STEADY */
+        __be32  lednum;
+#define UATH_LED_LINK           0
+#define UATH_LED_ACTIVITY       1
+        __be32  ledmode;
+#define UATH_LED_OFF    0
+#define UATH_LED_ON     1
+} __packed;
+
+struct ar5523_cmd_ledblink {            /* WDCMSG_SET_LED_BLINK */
+        __be32  lednum;
+        __be32  ledmode;
+        __be32  blinkrate;
+        __be32  slowmode;
+} __packed;
+
+struct ar5523_cmd_ledstate {            /* WDCMSG_SET_LED_STATE */
+        __be32  connected;
+} __packed;
+
+struct ar5523_cmd_txq_attr {
+        __be32  priority;
+        __be32  aifs;
+        __be32  logcwmin;
+        __be32  logcwmax;
+        __be32  bursttime;
+        __be32  mode;
+        __be32  qflags;
+} __packed;
+
+struct ar5523_cmd_txq_setup {           /* WDCMSG_SETUP_TX_QUEUE */
+        __be32  qid;
+        __be32  len;
+        struct ar5523_cmd_txq_attr attr;
+} __packed;
+
+struct ar5523_cmd_rx_filter {           /* WDCMSG_RX_FILTER */
+        __be32  bits;
+#define UATH_FILTER_RX_UCAST            0x00000001
+#define UATH_FILTER_RX_MCAST            0x00000002
+#define UATH_FILTER_RX_BCAST            0x00000004
+#define UATH_FILTER_RX_CONTROL          0x00000008
+#define UATH_FILTER_RX_BEACON           0x00000010      /* beacon frames */
+#define UATH_FILTER_RX_PROM             0x00000020      /* promiscuous mode */
+#define UATH_FILTER_RX_PHY_ERR          0x00000040      /* phy errors */
+#define UATH_FILTER_RX_PHY_RADAR        0x00000080      /* radar phy errors */
+#define UATH_FILTER_RX_XR_POOL          0x00000400      /* XR group polls */
+#define UATH_FILTER_RX_PROBE_REQ        0x00000800
+        __be32  op;
+#define UATH_FILTER_OP_INIT             0x0
+#define UATH_FILTER_OP_SET              0x1
+#define UATH_FILTER_OP_CLEAR            0x2
+#define UATH_FILTER_OP_TEMP             0x3
+#define UATH_FILTER_OP_RESTORE          0x4
+} __packed;
+
+enum {
+        CFG_NONE,                       /* Sentinal to indicate "no config" */
+        CFG_REG_DOMAIN,                 /* Regulatory Domain */
+        CFG_RATE_CONTROL_ENABLE,
+        CFG_DEF_XMIT_DATA_RATE,         /* NB: if rate control is not enabled */
+        CFG_HW_TX_RETRIES,
+        CFG_SW_TX_RETRIES,
+        CFG_SLOW_CLOCK_ENABLE,
+        CFG_COMP_PROC,
+        CFG_USER_RTS_THRESHOLD,
+        CFG_XR2NORM_RATE_THRESHOLD,
+        CFG_XRMODE_SWITCH_COUNT,
+        CFG_PROTECTION_TYPE,
+        CFG_BURST_SEQ_THRESHOLD,
+        CFG_ABOLT,
+        CFG_IQ_LOG_COUNT_MAX,
+        CFG_MODE_CTS,
+        CFG_WME_ENABLED,
+        CFG_GPRS_CBR_PERIOD,
+        CFG_SERVICE_TYPE,
+        /* MAC Address to use.  Overrides EEPROM */
+        CFG_MAC_ADDR,
+        CFG_DEBUG_EAR,
+        CFG_INIT_REGS,
+        /* An ID for use in error & debug messages */
+        CFG_DEBUG_ID,
+        CFG_COMP_WIN_SZ,
+        CFG_DIVERSITY_CTL,
+        CFG_TP_SCALE,
+        CFG_TPC_HALF_DBM5,
+        CFG_TPC_HALF_DBM2,
+        CFG_OVERRD_TX_POWER,
+        CFG_USE_32KHZ_CLOCK,
+        CFG_GMODE_PROTECTION,
+        CFG_GMODE_PROTECT_RATE_INDEX,
+        CFG_GMODE_NON_ERP_PREAMBLE,
+        CFG_WDC_TRANSPORT_CHUNK_SIZE,
+};
+
+enum {
+        /* Sentinal to indicate "no capability" */
+        CAP_NONE,
+        CAP_ALL,                        /* ALL capabilities */
+        CAP_TARGET_VERSION,
+        CAP_TARGET_REVISION,
+        CAP_MAC_VERSION,
+        CAP_MAC_REVISION,
+        CAP_PHY_REVISION,
+        CAP_ANALOG_5GHz_REVISION,
+        CAP_ANALOG_2GHz_REVISION,
+        /* Target supports WDC message debug features */
+        CAP_DEBUG_WDCMSG_SUPPORT,
+
+        CAP_REG_DOMAIN,
+        CAP_COUNTRY_CODE,
+        CAP_REG_CAP_BITS,
+
+        CAP_WIRELESS_MODES,
+        CAP_CHAN_SPREAD_SUPPORT,
+        CAP_SLEEP_AFTER_BEACON_BROKEN,
+        CAP_COMPRESS_SUPPORT,
+        CAP_BURST_SUPPORT,
+        CAP_FAST_FRAMES_SUPPORT,
+        CAP_CHAP_TUNING_SUPPORT,
+        CAP_TURBOG_SUPPORT,
+        CAP_TURBO_PRIME_SUPPORT,
+        CAP_DEVICE_TYPE,
+        CAP_XR_SUPPORT,
+        CAP_WME_SUPPORT,
+        CAP_TOTAL_QUEUES,
+        CAP_CONNECTION_ID_MAX,          /* Should absorb CAP_KEY_CACHE_SIZE */
+
+        CAP_LOW_5GHZ_CHAN,
+        CAP_HIGH_5GHZ_CHAN,
+        CAP_LOW_2GHZ_CHAN,
+        CAP_HIGH_2GHZ_CHAN,
+
+        CAP_MIC_AES_CCM,
+        CAP_MIC_CKIP,
+        CAP_MIC_TKIP,
+        CAP_MIC_TKIP_WME,
+        CAP_CIPHER_AES_CCM,
+        CAP_CIPHER_CKIP,
+        CAP_CIPHER_TKIP,
+
+        CAP_TWICE_ANTENNAGAIN_5G,
+        CAP_TWICE_ANTENNAGAIN_2G,
+};
+
+enum {
+        ST_NONE,                    /* Sentinal to indicate "no status" */
+        ST_ALL,
+        ST_SERVICE_TYPE,
+        ST_WLAN_MODE,
+        ST_FREQ,
+        ST_BAND,
+        ST_LAST_RSSI,
+        ST_PS_FRAMES_DROPPED,
+        ST_CACHED_DEF_ANT,
+        ST_COUNT_OTHER_RX_ANT,
+        ST_USE_FAST_DIVERSITY,
+        ST_MAC_ADDR,
+        ST_RX_GENERATION_NUM,
+        ST_TX_QUEUE_DEPTH,
+        ST_SERIAL_NUMBER,
+        ST_WDC_TRANSPORT_CHUNK_SIZE,
+};
+
+enum {
+        TARGET_DEVICE_AWAKE,
+        TARGET_DEVICE_SLEEP,
+        TARGET_DEVICE_PWRDN,
+        TARGET_DEVICE_PWRSAVE,
+        TARGET_DEVICE_SUSPEND,
+        TARGET_DEVICE_RESUME,
+};
+
+/* this is in net/ieee80211.h, but that conflicts with the mac80211 headers */
+#define IEEE80211_2ADDR_LEN     16
+
+#define AR5523_MIN_RXBUFSZ                              \
+        (((sizeof(__be32) + IEEE80211_2ADDR_LEN +       \
+           sizeof(struct ar5523_rx_desc)) + 3) & ~3)
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
index 89bf94d4d8a1..6f7cf49eff4d 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_2p2_initvals.h
@@ -534,107 +534,107 @@ static const u32 ar9300_2p2_baseband_core[][2] = {
 
 static const u32 ar9300Modes_high_power_tx_gain_table_2p2[][5] = {
         /* Addr      5G_HT20     5G_HT40     2G_HT40     2G_HT20   */
-        {0x0000a2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000a2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000a2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000a2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000a2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000a2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000a2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
         {0x0000a410, 0x000050d9, 0x000050d9, 0x000050d9, 0x000050d9},
         {0x0000a500, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
         {0x0000a504, 0x06000003, 0x06000003, 0x04000002, 0x04000002},
         {0x0000a508, 0x0a000020, 0x0a000020, 0x08000004, 0x08000004},
         {0x0000a50c, 0x10000023, 0x10000023, 0x0b000200, 0x0b000200},
-        {0x0000a510, 0x15000028, 0x15000028, 0x0f000202, 0x0f000202},
-        {0x0000a514, 0x1b00002b, 0x1b00002b, 0x12000400, 0x12000400},
-        {0x0000a518, 0x1f020028, 0x1f020028, 0x16000402, 0x16000402},
-        {0x0000a51c, 0x2502002b, 0x2502002b, 0x19000404, 0x19000404},
-        {0x0000a520, 0x2a04002a, 0x2a04002a, 0x1c000603, 0x1c000603},
-        {0x0000a524, 0x2e06002a, 0x2e06002a, 0x21000a02, 0x21000a02},
-        {0x0000a528, 0x3302202d, 0x3302202d, 0x25000a04, 0x25000a04},
-        {0x0000a52c, 0x3804202c, 0x3804202c, 0x28000a20, 0x28000a20},
-        {0x0000a530, 0x3c06202c, 0x3c06202c, 0x2c000e20, 0x2c000e20},
-        {0x0000a534, 0x4108202d, 0x4108202d, 0x30000e22, 0x30000e22},
-        {0x0000a538, 0x4506402d, 0x4506402d, 0x34000e24, 0x34000e24},
-        {0x0000a53c, 0x4906222d, 0x4906222d, 0x38001640, 0x38001640},
-        {0x0000a540, 0x4d062231, 0x4d062231, 0x3c001660, 0x3c001660},
-        {0x0000a544, 0x50082231, 0x50082231, 0x3f001861, 0x3f001861},
-        {0x0000a548, 0x5608422e, 0x5608422e, 0x43001a81, 0x43001a81},
-        {0x0000a54c, 0x5a08442e, 0x5a08442e, 0x47001a83, 0x47001a83},
-        {0x0000a550, 0x5e0a4431, 0x5e0a4431, 0x4a001c84, 0x4a001c84},
-        {0x0000a554, 0x640a4432, 0x640a4432, 0x4e001ce3, 0x4e001ce3},
-        {0x0000a558, 0x680a4434, 0x680a4434, 0x52001ce5, 0x52001ce5},
-        {0x0000a55c, 0x6c0a6434, 0x6c0a6434, 0x56001ce9, 0x56001ce9},
-        {0x0000a560, 0x6f0a6633, 0x6f0a6633, 0x5a001ceb, 0x5a001ceb},
-        {0x0000a564, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a568, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a56c, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a570, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a574, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a578, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
-        {0x0000a57c, 0x730c6634, 0x730c6634, 0x5d001eec, 0x5d001eec},
+        {0x0000a510, 0x16000220, 0x16000220, 0x0f000202, 0x0f000202},
+        {0x0000a514, 0x1c000223, 0x1c000223, 0x12000400, 0x12000400},
+        {0x0000a518, 0x21002220, 0x21002220, 0x16000402, 0x16000402},
+        {0x0000a51c, 0x27002223, 0x27002223, 0x19000404, 0x19000404},
+        {0x0000a520, 0x2b022220, 0x2b022220, 0x1c000603, 0x1c000603},
+        {0x0000a524, 0x2f022222, 0x2f022222, 0x21000a02, 0x21000a02},
+        {0x0000a528, 0x34022225, 0x34022225, 0x25000a04, 0x25000a04},
+        {0x0000a52c, 0x3a02222a, 0x3a02222a, 0x28000a20, 0x28000a20},
+        {0x0000a530, 0x3e02222c, 0x3e02222c, 0x2c000e20, 0x2c000e20},
+        {0x0000a534, 0x4202242a, 0x4202242a, 0x30000e22, 0x30000e22},
+        {0x0000a538, 0x4702244a, 0x4702244a, 0x34000e24, 0x34000e24},
+        {0x0000a53c, 0x4b02244c, 0x4b02244c, 0x38001640, 0x38001640},
+        {0x0000a540, 0x4e02246c, 0x4e02246c, 0x3c001660, 0x3c001660},
+        {0x0000a544, 0x52022470, 0x52022470, 0x3f001861, 0x3f001861},
+        {0x0000a548, 0x55022490, 0x55022490, 0x43001a81, 0x43001a81},
+        {0x0000a54c, 0x59022492, 0x59022492, 0x47001a83, 0x47001a83},
+        {0x0000a550, 0x5d022692, 0x5d022692, 0x4a001c84, 0x4a001c84},
+        {0x0000a554, 0x61022892, 0x61022892, 0x4e001ce3, 0x4e001ce3},
+        {0x0000a558, 0x65024890, 0x65024890, 0x52001ce5, 0x52001ce5},
+        {0x0000a55c, 0x69024892, 0x69024892, 0x56001ce9, 0x56001ce9},
+        {0x0000a560, 0x6e024c92, 0x6e024c92, 0x5a001ceb, 0x5a001ceb},
+        {0x0000a564, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a568, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a56c, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a570, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a574, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a578, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
+        {0x0000a57c, 0x74026e92, 0x74026e92, 0x5d001eec, 0x5d001eec},
         {0x0000a580, 0x00800000, 0x00800000, 0x00800000, 0x00800000},
         {0x0000a584, 0x06800003, 0x06800003, 0x04800002, 0x04800002},
         {0x0000a588, 0x0a800020, 0x0a800020, 0x08800004, 0x08800004},
         {0x0000a58c, 0x10800023, 0x10800023, 0x0b800200, 0x0b800200},
-        {0x0000a590, 0x15800028, 0x15800028, 0x0f800202, 0x0f800202},
-        {0x0000a594, 0x1b80002b, 0x1b80002b, 0x12800400, 0x12800400},
-        {0x0000a598, 0x1f820028, 0x1f820028, 0x16800402, 0x16800402},
-        {0x0000a59c, 0x2582002b, 0x2582002b, 0x19800404, 0x19800404},
-        {0x0000a5a0, 0x2a84002a, 0x2a84002a, 0x1c800603, 0x1c800603},
-        {0x0000a5a4, 0x2e86002a, 0x2e86002a, 0x21800a02, 0x21800a02},
-        {0x0000a5a8, 0x3382202d, 0x3382202d, 0x25800a04, 0x25800a04},
-        {0x0000a5ac, 0x3884202c, 0x3884202c, 0x28800a20, 0x28800a20},
-        {0x0000a5b0, 0x3c86202c, 0x3c86202c, 0x2c800e20, 0x2c800e20},
-        {0x0000a5b4, 0x4188202d, 0x4188202d, 0x30800e22, 0x30800e22},
-        {0x0000a5b8, 0x4586402d, 0x4586402d, 0x34800e24, 0x34800e24},
-        {0x0000a5bc, 0x4986222d, 0x4986222d, 0x38801640, 0x38801640},
-        {0x0000a5c0, 0x4d862231, 0x4d862231, 0x3c801660, 0x3c801660},
-        {0x0000a5c4, 0x50882231, 0x50882231, 0x3f801861, 0x3f801861},
-        {0x0000a5c8, 0x5688422e, 0x5688422e, 0x43801a81, 0x43801a81},
-        {0x0000a5cc, 0x5a88442e, 0x5a88442e, 0x47801a83, 0x47801a83},
-        {0x0000a5d0, 0x5e8a4431, 0x5e8a4431, 0x4a801c84, 0x4a801c84},
-        {0x0000a5d4, 0x648a4432, 0x648a4432, 0x4e801ce3, 0x4e801ce3},
-        {0x0000a5d8, 0x688a4434, 0x688a4434, 0x52801ce5, 0x52801ce5},
-        {0x0000a5dc, 0x6c8a6434, 0x6c8a6434, 0x56801ce9, 0x56801ce9},
-        {0x0000a5e0, 0x6f8a6633, 0x6f8a6633, 0x5a801ceb, 0x5a801ceb},
-        {0x0000a5e4, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5e8, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5ec, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f0, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f4, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5f8, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
-        {0x0000a5fc, 0x738c6634, 0x738c6634, 0x5d801eec, 0x5d801eec},
+        {0x0000a590, 0x16800220, 0x16800220, 0x0f800202, 0x0f800202},
+        {0x0000a594, 0x1c800223, 0x1c800223, 0x12800400, 0x12800400},
+        {0x0000a598, 0x21802220, 0x21802220, 0x16800402, 0x16800402},
+        {0x0000a59c, 0x27802223, 0x27802223, 0x19800404, 0x19800404},
+        {0x0000a5a0, 0x2b822220, 0x2b822220, 0x1c800603, 0x1c800603},
+        {0x0000a5a4, 0x2f822222, 0x2f822222, 0x21800a02, 0x21800a02},
+        {0x0000a5a8, 0x34822225, 0x34822225, 0x25800a04, 0x25800a04},
+        {0x0000a5ac, 0x3a82222a, 0x3a82222a, 0x28800a20, 0x28800a20},
+        {0x0000a5b0, 0x3e82222c, 0x3e82222c, 0x2c800e20, 0x2c800e20},
+        {0x0000a5b4, 0x4282242a, 0x4282242a, 0x30800e22, 0x30800e22},
+        {0x0000a5b8, 0x4782244a, 0x4782244a, 0x34800e24, 0x34800e24},
+        {0x0000a5bc, 0x4b82244c, 0x4b82244c, 0x38801640, 0x38801640},
+        {0x0000a5c0, 0x4e82246c, 0x4e82246c, 0x3c801660, 0x3c801660},
+        {0x0000a5c4, 0x52822470, 0x52822470, 0x3f801861, 0x3f801861},
+        {0x0000a5c8, 0x55822490, 0x55822490, 0x43801a81, 0x43801a81},
+        {0x0000a5cc, 0x59822492, 0x59822492, 0x47801a83, 0x47801a83},
+        {0x0000a5d0, 0x5d822692, 0x5d822692, 0x4a801c84, 0x4a801c84},
+        {0x0000a5d4, 0x61822892, 0x61822892, 0x4e801ce3, 0x4e801ce3},
+        {0x0000a5d8, 0x65824890, 0x65824890, 0x52801ce5, 0x52801ce5},
+        {0x0000a5dc, 0x69824892, 0x69824892, 0x56801ce9, 0x56801ce9},
+        {0x0000a5e0, 0x6e824c92, 0x6e824c92, 0x5a801ceb, 0x5a801ceb},
+        {0x0000a5e4, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5e8, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5ec, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f0, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f4, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5f8, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
+        {0x0000a5fc, 0x74826e92, 0x74826e92, 0x5d801eec, 0x5d801eec},
         {0x0000a600, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
         {0x0000a604, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
-        {0x0000a608, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a60c, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a610, 0x01804601, 0x01804601, 0x00000000, 0x00000000},
-        {0x0000a614, 0x01804601, 0x01804601, 0x01404000, 0x01404000},
-        {0x0000a618, 0x01804601, 0x01804601, 0x01404501, 0x01404501},
-        {0x0000a61c, 0x01804601, 0x01804601, 0x02008501, 0x02008501},
-        {0x0000a620, 0x03408d02, 0x03408d02, 0x0280ca03, 0x0280ca03},
-        {0x0000a624, 0x0300cc03, 0x0300cc03, 0x03010c04, 0x03010c04},
-        {0x0000a628, 0x03410d04, 0x03410d04, 0x04014c04, 0x04014c04},
-        {0x0000a62c, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a630, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a634, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a638, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000a63c, 0x03410d04, 0x03410d04, 0x04015005, 0x04015005},
-        {0x0000b2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000b2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000b2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000a608, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a60c, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a610, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
+        {0x0000a614, 0x02004000, 0x02004000, 0x01404000, 0x01404000},
+        {0x0000a618, 0x02004801, 0x02004801, 0x01404501, 0x01404501},
+        {0x0000a61c, 0x02808a02, 0x02808a02, 0x02008501, 0x02008501},
+        {0x0000a620, 0x0380ce03, 0x0380ce03, 0x0280ca03, 0x0280ca03},
+        {0x0000a624, 0x04411104, 0x04411104, 0x03010c04, 0x03010c04},
+        {0x0000a628, 0x04411104, 0x04411104, 0x04014c04, 0x04014c04},
+        {0x0000a62c, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a630, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a634, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a638, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000a63c, 0x04411104, 0x04411104, 0x04015005, 0x04015005},
+        {0x0000b2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000b2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000b2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000b2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
-        {0x0000c2dc, 0x000cfff0, 0x000cfff0, 0x03aaa352, 0x03aaa352},
-        {0x0000c2e0, 0x000f0000, 0x000f0000, 0x03ccc584, 0x03ccc584},
-        {0x0000c2e4, 0x03f00000, 0x03f00000, 0x03f0f800, 0x03f0f800},
+        {0x0000c2dc, 0x00033800, 0x00033800, 0x03aaa352, 0x03aaa352},
+        {0x0000c2e0, 0x0003c000, 0x0003c000, 0x03ccc584, 0x03ccc584},
+        {0x0000c2e4, 0x03fc0000, 0x03fc0000, 0x03f0f800, 0x03f0f800},
         {0x0000c2e8, 0x00000000, 0x00000000, 0x03ff0000, 0x03ff0000},
         {0x00016044, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016048, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016048, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016068, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
         {0x00016444, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016448, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016448, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016468, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
         {0x00016844, 0x012492d4, 0x012492d4, 0x012492d4, 0x012492d4},
-        {0x00016848, 0x61200001, 0x61200001, 0x66480001, 0x66480001},
+        {0x00016848, 0x66480001, 0x66480001, 0x66480001, 0x66480001},
         {0x00016868, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c, 0x6db6db6c},
 };
 
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_calib.c b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
index 84b558d126ca..162401f22f8c 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_calib.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_calib.c
@@ -276,6 +276,11 @@ static void ar9003_hw_iqcalibrate(struct ath_hw *ah, u8 numChains)
                                 offset_array[i],
                                 REG_READ(ah, offset_array[i]));
 
+                        if (AR_SREV_9565(ah) &&
+                            (iCoff == 63 || qCoff == 63 ||
+                             iCoff == -63 || qCoff == -63))
+                                return;
+
                         REG_RMW_FIELD(ah, offset_array[i],
                                       AR_PHY_RX_IQCAL_CORR_IQCORR_Q_I_COFF,
                                       iCoff);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
index 5bbe5057ba18..c86cb6400040 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_eeprom.c
@@ -18,6 +18,7 @@
 #include "hw.h"
 #include "ar9003_phy.h"
 #include "ar9003_eeprom.h"
+#include "ar9003_mci.h"
 
 #define COMP_HDR_LEN 4
 #define COMP_CKSUM_LEN 2
@@ -41,7 +42,6 @@
 static int ar9003_hw_power_interpolate(int32_t x,
                                        int32_t *px, int32_t *py, u_int16_t np);
 
-
 static const struct ar9300_eeprom ar9300_default = {
         .eepromVersion = 2,
         .templateVersion = 2,
@@ -2989,7 +2989,7 @@ static u32 ath9k_hw_ar9300_get_eeprom(struct ath_hw *ah,
         case EEP_PAPRD:
                 if (AR_SREV_9462(ah))
                         return false;
-                if (!ah->config.enable_paprd);
+                if (!ah->config.enable_paprd)
                         return false;
                 return !!(pBase->featureEnable & BIT(5));
         case EEP_CHAIN_MASK_REDUCE:
@@ -3601,7 +3601,7 @@ static void ar9003_hw_ant_ctrl_apply(struct ath_hw *ah, bool is2ghz)
          *   7:4 R/W  SWITCH_TABLE_COM_SPDT_WLAN_IDLE
          * SWITCH_TABLE_COM_SPDT_WLAN_IDLE
          */
-        if (AR_SREV_9462_20_OR_LATER(ah)) {
+        if (AR_SREV_9462_20(ah) || AR_SREV_9565(ah)) {
                 value = ar9003_switch_com_spdt_get(ah, is2ghz);
                 REG_RMW_FIELD(ah, AR_PHY_GLB_CONTROL,
                                 AR_SWITCH_TABLE_COM_SPDT_ALL, value);
@@ -5037,16 +5037,28 @@ static void ar9003_hw_set_power_per_rate_table(struct ath_hw *ah,
                 case CTL_5GHT20:
                 case CTL_2GHT20:
                         for (i = ALL_TARGET_HT20_0_8_16;
-                             i <= ALL_TARGET_HT20_23; i++)
+                             i <= ALL_TARGET_HT20_23; i++) {
                                 pPwrArray[i] = (u8)min((u16)pPwrArray[i],
                                                        minCtlPower);
+                                if (ath9k_hw_mci_is_enabled(ah))
+                                        pPwrArray[i] =
+                                                (u8)min((u16)pPwrArray[i],
+                                                ar9003_mci_get_max_txpower(ah,
+                                                        pCtlMode[ctlMode]));
+                        }
                         break;
                 case CTL_5GHT40:
                 case CTL_2GHT40:
                         for (i = ALL_TARGET_HT40_0_8_16;
-                             i <= ALL_TARGET_HT40_23; i++)
+                             i <= ALL_TARGET_HT40_23; i++) {
                                 pPwrArray[i] = (u8)min((u16)pPwrArray[i],
                                                        minCtlPower);
+                                if (ath9k_hw_mci_is_enabled(ah))
+                                        pPwrArray[i] =
+                                                (u8)min((u16)pPwrArray[i],
+                                                ar9003_mci_get_max_txpower(ah,
+                                                        pCtlMode[ctlMode]));
+                        }
                         break;
                 default:
                         break;
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_hw.c b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
index 1a36fa262639..0693cd95b746 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_hw.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_hw.c
@@ -219,10 +219,10 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
 
                 /* Awake -> Sleep Setting */
                 INIT_INI_ARRAY(&ah->iniPcieSerdes,
-                               ar9462_pciephy_pll_on_clkreq_disable_L1_2p0);
+                               ar9462_pciephy_clkreq_disable_L1_2p0);
                 /* Sleep -> Awake Setting */
                 INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-                               ar9462_pciephy_pll_on_clkreq_disable_L1_2p0);
+                               ar9462_pciephy_clkreq_disable_L1_2p0);
 
                 /* Fast clock modal settings */
                 INIT_INI_ARRAY(&ah->iniModesFastClock,
@@ -328,9 +328,9 @@ static void ar9003_hw_init_mode_regs(struct ath_hw *ah)
                                ar9565_1p0_Modes_lowest_ob_db_tx_gain_table);
 
                 INIT_INI_ARRAY(&ah->iniPcieSerdes,
-                               ar9565_1p0_pciephy_pll_on_clkreq_disable_L1);
+                               ar9565_1p0_pciephy_clkreq_disable_L1);
                 INIT_INI_ARRAY(&ah->iniPcieSerdesLowPower,
-                               ar9565_1p0_pciephy_pll_on_clkreq_disable_L1);
+                               ar9565_1p0_pciephy_clkreq_disable_L1);
 
                 INIT_INI_ARRAY(&ah->iniModesFastClock,
                                 ar9565_1p0_modes_fast_clock);
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.c b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
index 44c202ce6c66..42b4412d6794 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.c
@@ -750,6 +750,9 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 
         mci_hw->bt_state = MCI_BT_AWAKE;
 
+        REG_CLR_BIT(ah, AR_PHY_TIMING4,
+                    1 << AR_PHY_TIMING_CONTROL4_DO_GAIN_DC_IQ_CAL_SHIFT);
+
         if (caldata) {
                 caldata->done_txiqcal_once = false;
                 caldata->done_txclcal_once = false;
@@ -759,6 +762,9 @@ int ar9003_mci_end_reset(struct ath_hw *ah, struct ath9k_channel *chan,
         if (!ath9k_hw_init_cal(ah, chan))
                 return -EIO;
 
+        REG_SET_BIT(ah, AR_PHY_TIMING4,
+                    1 << AR_PHY_TIMING_CONTROL4_DO_GAIN_DC_IQ_CAL_SHIFT);
+
 exit:
         ar9003_mci_enable_interrupt(ah);
         return 0;
@@ -799,6 +805,9 @@ static void ar9003_mci_osla_setup(struct ath_hw *ah, bool enable)
         REG_RMW_FIELD(ah, AR_MCI_SCHD_TABLE_2,
                       AR_MCI_SCHD_TABLE_2_MEM_BASED, 1);
 
+        if (AR_SREV_9565(ah))
+                REG_RMW_FIELD(ah, AR_MCI_MISC, AR_MCI_MISC_HW_FIX_EN, 1);
+
         if (!(mci->config & ATH_MCI_CONFIG_DISABLE_AGGR_THRESH)) {
                 thresh = MS(mci->config, ATH_MCI_CONFIG_AGGR_THRESH);
                 REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
@@ -818,7 +827,7 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
 {
         struct ath_common *common = ath9k_hw_common(ah);
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
-        u32 regval;
+        u32 regval, i;
 
         ath_dbg(common, MCI, "MCI Reset (full_sleep = %d, is_2g = %d)\n",
                 is_full_sleep, is_2g);
@@ -847,11 +856,18 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
                  SM(1, AR_BTCOEX_CTRL_WBTIMER_EN) |
                  SM(1, AR_BTCOEX_CTRL_PA_SHARED) |
                  SM(1, AR_BTCOEX_CTRL_LNA_SHARED) |
-                 SM(2, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
-                 SM(3, AR_BTCOEX_CTRL_RX_CHAIN_MASK) |
                  SM(0, AR_BTCOEX_CTRL_1_CHAIN_ACK) |
                  SM(0, AR_BTCOEX_CTRL_1_CHAIN_BCN) |
                  SM(0, AR_BTCOEX_CTRL_ONE_STEP_LOOK_AHEAD_EN);
+        if (AR_SREV_9565(ah)) {
+                regval |= SM(1, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
+                          SM(1, AR_BTCOEX_CTRL_RX_CHAIN_MASK);
+                REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
+                              AR_BTCOEX_CTRL2_TX_CHAIN_MASK, 0x1);
+        } else {
+                regval |= SM(2, AR_BTCOEX_CTRL_NUM_ANTENNAS) |
+                          SM(3, AR_BTCOEX_CTRL_RX_CHAIN_MASK);
+        }
 
         REG_WRITE(ah, AR_BTCOEX_CTRL, regval);
 
@@ -865,9 +881,24 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
         REG_RMW_FIELD(ah, AR_BTCOEX_CTRL3,
                       AR_BTCOEX_CTRL3_CONT_INFO_TIMEOUT, 20);
 
-        REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2, AR_BTCOEX_CTRL2_RX_DEWEIGHT, 1);
+        REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2, AR_BTCOEX_CTRL2_RX_DEWEIGHT, 0);
         REG_RMW_FIELD(ah, AR_PCU_MISC, AR_PCU_BT_ANT_PREVENT_RX, 0);
 
+        /* Set the time out to 3.125ms (5 BT slots) */
+        REG_RMW_FIELD(ah, AR_BTCOEX_WL_LNA, AR_BTCOEX_WL_LNA_TIMEOUT, 0x3D090);
+
+        /* concurrent tx priority */
+        if (mci->config & ATH_MCI_CONFIG_CONCUR_TX) {
+                REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
+                              AR_BTCOEX_CTRL2_DESC_BASED_TXPWR_ENABLE, 0);
+                REG_RMW_FIELD(ah, AR_BTCOEX_CTRL2,
+                              AR_BTCOEX_CTRL2_TXPWR_THRESH, 0x7f);
+                REG_RMW_FIELD(ah, AR_BTCOEX_CTRL,
+                              AR_BTCOEX_CTRL_REDUCE_TXPWR, 0);
+                for (i = 0; i < 8; i++)
+                        REG_WRITE(ah, AR_BTCOEX_MAX_TXPWR(i), 0x7f7f7f7f);
+        }
+
         regval = MS(mci->config, ATH_MCI_CONFIG_CLK_DIV);
         REG_RMW_FIELD(ah, AR_MCI_TX_CTRL, AR_MCI_TX_CTRL_CLK_DIV, regval);
         REG_SET_BIT(ah, AR_BTCOEX_CTRL, AR_BTCOEX_CTRL_MCI_MODE_EN);
@@ -910,6 +941,9 @@ int ar9003_mci_reset(struct ath_hw *ah, bool en_int, bool is_2g,
         mci->ready = true;
         ar9003_mci_prep_interface(ah);
 
+        if (AR_SREV_9565(ah))
+                REG_RMW_FIELD(ah, AR_MCI_DBG_CNT_CTRL,
+                              AR_MCI_DBG_CNT_CTRL_ENABLE, 0);
         if (en_int)
                 ar9003_mci_enable_interrupt(ah);
 
@@ -1028,7 +1062,9 @@ void ar9003_mci_2g5g_switch(struct ath_hw *ah, bool force)
 
                 if (!(mci->config & ATH_MCI_CONFIG_DISABLE_OSLA))
                         ar9003_mci_osla_setup(ah, true);
-                REG_WRITE(ah, AR_SELFGEN_MASK, 0x02);
+
+                if (AR_SREV_9462(ah))
+                        REG_WRITE(ah, AR_SELFGEN_MASK, 0x02);
         } else {
                 ar9003_mci_send_lna_take(ah, true);
                 udelay(5);
@@ -1170,7 +1206,7 @@ EXPORT_SYMBOL(ar9003_mci_cleanup);
 u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
 {
         struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
-        u32 value = 0;
+        u32 value = 0, tsf;
         u8 query_type;
 
         switch (state_type) {
@@ -1228,6 +1264,14 @@ u32 ar9003_mci_state(struct ath_hw *ah, u32 state_type)
                 ar9003_mci_send_coex_bt_status_query(ah, true, query_type);
                 break;
         case MCI_STATE_RECOVER_RX:
+                tsf = ath9k_hw_gettsf32(ah);
+                if ((tsf - mci->last_recovery) <= MCI_RECOVERY_DUR_TSF) {
+                        ath_dbg(ath9k_hw_common(ah), MCI,
+                                "(MCI) ignore Rx recovery\n");
+                        break;
+                }
+                ath_dbg(ath9k_hw_common(ah), MCI, "(MCI) RECOVER RX\n");
+                mci->last_recovery = tsf;
                 ar9003_mci_prep_interface(ah);
                 mci->query_bt = true;
                 mci->need_flush_btinfo = true;
@@ -1426,3 +1470,17 @@ void ar9003_mci_send_wlan_channels(struct ath_hw *ah)
         ar9003_mci_send_coex_wlan_channels(ah, true);
 }
 EXPORT_SYMBOL(ar9003_mci_send_wlan_channels);
+
+u16 ar9003_mci_get_max_txpower(struct ath_hw *ah, u8 ctlmode)
+{
+        if (!ah->btcoex_hw.mci.concur_tx)
+                goto out;
+
+        if (ctlmode == CTL_2GHT20)
+                return ATH_BTCOEX_HT20_MAX_TXPOWER;
+        else if (ctlmode == CTL_2GHT40)
+                return ATH_BTCOEX_HT40_MAX_TXPOWER;
+
+out:
+        return -1;
+}
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mci.h b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
index 2a2d01889613..66d7ab9f920d 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mci.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mci.h
@@ -18,6 +18,7 @@
 #define AR9003_MCI_H
 
 #define MCI_FLAG_DISABLE_TIMESTAMP      0x00000001      /* Disable time stamp */
+#define MCI_RECOVERY_DUR_TSF            (100 * 1000)    /* 100 ms */
 
 /* Default remote BT device MCI COEX version */
 #define MCI_GPM_COEX_MAJOR_VERSION_DEFAULT  3
@@ -125,6 +126,7 @@ enum ath_mci_gpm_coex_profile_type {
         MCI_GPM_COEX_PROFILE_HID,
         MCI_GPM_COEX_PROFILE_BNEP,
         MCI_GPM_COEX_PROFILE_VOICE,
+        MCI_GPM_COEX_PROFILE_A2DPVO,
         MCI_GPM_COEX_PROFILE_MAX
 };
 
@@ -196,7 +198,6 @@ enum mci_state_type {
         MCI_STATE_SEND_WLAN_COEX_VERSION,
         MCI_STATE_SEND_VERSION_QUERY,
         MCI_STATE_SEND_STATUS_QUERY,
-        MCI_STATE_SET_CONCUR_TX_PRI,
         MCI_STATE_RECOVER_RX,
         MCI_STATE_NEED_FTP_STOMP,
         MCI_STATE_DEBUG,
@@ -278,6 +279,7 @@ 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);
+u16 ar9003_mci_get_max_txpower(struct ath_hw *ah, u8 ctlmode);
 
 #else
 
@@ -324,6 +326,10 @@ static inline void ar9003_mci_set_power_awake(struct ath_hw *ah)
 static inline void ar9003_mci_check_gpm_offset(struct ath_hw *ah)
 {
 }
+static inline u16 ar9003_mci_get_max_txpower(struct ath_hw *ah, u8 ctlmode)
+{
+        return -1;
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.h b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
index 9a48e3d2f231..8f585233a788 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.h
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.h
@@ -32,6 +32,7 @@
 #define AR_PHY_SPUR_REG     (AR_CHAN_BASE + 0x1c)
 #define AR_PHY_RX_IQCAL_CORR_B0    (AR_CHAN_BASE + 0xdc)
 #define AR_PHY_TX_IQCAL_CONTROL_3  (AR_CHAN_BASE + 0xb0)
+#define AR_PHY_TIMING_CONTROL4_DO_GAIN_DC_IQ_CAL_SHIFT 16
 
 #define AR_PHY_TIMING11_SPUR_FREQ_SD    0x3FF00000
 #define AR_PHY_TIMING11_SPUR_FREQ_SD_S  20
diff --git a/drivers/net/wireless/ath/ath9k/ar9565_1p0_initvals.h b/drivers/net/wireless/ath/ath9k/ar9565_1p0_initvals.h
index 843e79f67ff2..0c2ac0c6dc89 100644
--- a/drivers/net/wireless/ath/ath9k/ar9565_1p0_initvals.h
+++ b/drivers/net/wireless/ath/ath9k/ar9565_1p0_initvals.h
@@ -768,9 +768,9 @@ static const u32 ar9565_1p0_Modes_lowest_ob_db_tx_gain_table[][5] = {
         {0x00016054, 0x00000000, 0x00000000, 0x00000000, 0x00000000},
 };
 
-static const u32 ar9565_1p0_pciephy_pll_on_clkreq_disable_L1[][2] = {
+static const u32 ar9565_1p0_pciephy_clkreq_disable_L1[][2] = {
         /* Addr      allmodes  */
-        {0x00018c00, 0x18212ede},
+        {0x00018c00, 0x18213ede},
         {0x00018c04, 0x000801d8},
         {0x00018c08, 0x0003780c},
 };
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index dfe6a4707fd2..4e125d8904a0 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -437,6 +437,7 @@ void ath9k_set_beacon(struct ath_softc *sc);
 #define ATH_LONG_CALINTERVAL_INT  1000    /* 1000 ms */
 #define ATH_LONG_CALINTERVAL      30000   /* 30 seconds */
 #define ATH_RESTART_CALINTERVAL   1200000 /* 20 minutes */
+#define ATH_ANI_MAX_SKIP_COUNT  10
 
 #define ATH_PAPRD_TIMEOUT       100 /* msecs */
 #define ATH_PLL_WORK_INTERVAL   100
@@ -478,6 +479,7 @@ struct ath_btcoex {
         u32 btscan_no_stomp; /* in usec */
         u32 duty_cycle;
         u32 bt_wait_time;
+        int rssi_count;
         struct ath_gen_timer *no_stomp_timer; /* Timer for no BT stomping */
         struct ath_mci_profile mci;
 };
@@ -492,6 +494,7 @@ void ath9k_btcoex_timer_pause(struct ath_softc *sc);
 void ath9k_btcoex_handle_interrupt(struct ath_softc *sc, u32 status);
 u16 ath9k_btcoex_aggr_limit(struct ath_softc *sc, u32 max_4ms_framelen);
 void ath9k_btcoex_stop_gen_timer(struct ath_softc *sc);
+int ath9k_dump_btcoex(struct ath_softc *sc, u8 *buf, u32 len, u32 size);
 #else
 static inline int ath9k_init_btcoex(struct ath_softc *sc)
 {
@@ -518,6 +521,11 @@ static inline u16 ath9k_btcoex_aggr_limit(struct ath_softc *sc,
 static inline void ath9k_btcoex_stop_gen_timer(struct ath_softc *sc)
 {
 }
+static inline int ath9k_dump_btcoex(struct ath_softc *sc, u8 *buf,
+                                    u32 len, u32 size)
+{
+        return 0;
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
 struct ath9k_wow_pattern {
@@ -642,6 +650,7 @@ enum sc_op_flags {
 #define PS_WAIT_FOR_PSPOLL_DATA   BIT(2)
 #define PS_WAIT_FOR_TX_ACK        BIT(3)
 #define PS_BEACON_SYNC            BIT(4)
+#define PS_WAIT_FOR_ANI           BIT(5)
 
 struct ath_rate_table;
 
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.c b/drivers/net/wireless/ath/ath9k/btcoex.c
index 419e9a3f2fed..c90e9bc4b026 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.c
+++ b/drivers/net/wireless/ath/ath9k/btcoex.c
@@ -195,7 +195,7 @@ void ath9k_hw_btcoex_init_mci(struct ath_hw *ah)
         ah->btcoex_hw.mci.need_flush_btinfo = false;
         ah->btcoex_hw.mci.wlan_cal_seq = 0;
         ah->btcoex_hw.mci.wlan_cal_done = 0;
-        ah->btcoex_hw.mci.config = 0x2201;
+        ah->btcoex_hw.mci.config = (AR_SREV_9462(ah)) ? 0x2201 : 0xa4c1;
 }
 EXPORT_SYMBOL(ath9k_hw_btcoex_init_mci);
 
@@ -218,27 +218,45 @@ void ath9k_hw_btcoex_set_weight(struct ath_hw *ah,
                                 enum ath_stomp_type stomp_type)
 {
         struct ath_btcoex_hw *btcoex_hw = &ah->btcoex_hw;
+        struct ath9k_hw_mci *mci_hw = &ah->btcoex_hw.mci;
+        u8 txprio_shift[] = { 24, 16, 16, 0 }; /* tx priority weight */
+        bool concur_tx = (mci_hw->concur_tx && btcoex_hw->tx_prio[stomp_type]);
+        const u32 *weight = ar9003_wlan_weights[stomp_type];
+        int i;
 
-        if (AR_SREV_9300_20_OR_LATER(ah)) {
-                const u32 *weight = ar9003_wlan_weights[stomp_type];
-                int i;
-
-                if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
-                        if ((stomp_type == ATH_BTCOEX_STOMP_LOW) &&
-                            btcoex_hw->mci.stomp_ftp)
-                                stomp_type = ATH_BTCOEX_STOMP_LOW_FTP;
-                        weight = mci_wlan_weights[stomp_type];
-                }
-
-                for (i = 0; i < AR9300_NUM_WLAN_WEIGHTS; i++) {
-                        btcoex_hw->bt_weight[i] = AR9300_BT_WGHT;
-                        btcoex_hw->wlan_weight[i] = weight[i];
-                }
-        } else {
+        if (!AR_SREV_9300_20_OR_LATER(ah)) {
                 btcoex_hw->bt_coex_weights =
                         SM(bt_weight, AR_BTCOEX_BT_WGHT) |
                         SM(wlan_weight, AR_BTCOEX_WL_WGHT);
+                return;
+        }
+
+        if (AR_SREV_9462(ah) || AR_SREV_9565(ah)) {
+                enum ath_stomp_type stype =
+                        ((stomp_type == ATH_BTCOEX_STOMP_LOW) &&
+                         btcoex_hw->mci.stomp_ftp) ?
+                        ATH_BTCOEX_STOMP_LOW_FTP : stomp_type;
+                weight = mci_wlan_weights[stype];
         }
+
+        for (i = 0; i < AR9300_NUM_WLAN_WEIGHTS; i++) {
+                btcoex_hw->bt_weight[i] = AR9300_BT_WGHT;
+                btcoex_hw->wlan_weight[i] = weight[i];
+                if (concur_tx && i) {
+                        btcoex_hw->wlan_weight[i] &=
+                                ~(0xff << txprio_shift[i-1]);
+                        btcoex_hw->wlan_weight[i] |=
+                                (btcoex_hw->tx_prio[stomp_type] <<
+                                 txprio_shift[i-1]);
+                }
+        }
+        /* Last WLAN weight has to be adjusted wrt tx priority */
+        if (concur_tx) {
+                btcoex_hw->wlan_weight[i-1] &= ~(0xff << txprio_shift[i-1]);
+                btcoex_hw->wlan_weight[i-1] |= (btcoex_hw->tx_prio[stomp_type]
+                                                      << txprio_shift[i-1]);
+        }
+
 }
 EXPORT_SYMBOL(ath9k_hw_btcoex_set_weight);
 
@@ -385,3 +403,13 @@ void ath9k_hw_btcoex_bt_stomp(struct ath_hw *ah,
         }
 }
 EXPORT_SYMBOL(ath9k_hw_btcoex_bt_stomp);
+
+void ath9k_hw_btcoex_set_concur_txprio(struct ath_hw *ah, u8 *stomp_txprio)
+{
+        struct ath_btcoex_hw *btcoex = &ah->btcoex_hw;
+        int i;
+
+        for (i = 0; i < ATH_BTCOEX_STOMP_MAX; i++)
+                btcoex->tx_prio[i] = stomp_txprio[i];
+}
+EXPORT_SYMBOL(ath9k_hw_btcoex_set_concur_txprio);
diff --git a/drivers/net/wireless/ath/ath9k/btcoex.h b/drivers/net/wireless/ath/ath9k/btcoex.h
index 385197ad79b0..2f84ab273d0c 100644
--- a/drivers/net/wireless/ath/ath9k/btcoex.h
+++ b/drivers/net/wireless/ath/ath9k/btcoex.h
@@ -39,6 +39,9 @@
 #define ATH_BTCOEX_RX_WAIT_TIME       100
 #define ATH_BTCOEX_STOMP_FTP_THRESH   5
 
+#define ATH_BTCOEX_HT20_MAX_TXPOWER   0x14
+#define ATH_BTCOEX_HT40_MAX_TXPOWER   0x10
+
 #define AR9300_NUM_BT_WEIGHTS   4
 #define AR9300_NUM_WLAN_WEIGHTS 4
 /* Defines the BT AR_BT_COEX_WGHT used */
@@ -84,6 +87,8 @@ struct ath9k_hw_mci {
         u8 bt_ver_minor;
         u8 bt_state;
         u8 stomp_ftp;
+        bool concur_tx;
+        u32 last_recovery;
 };
 
 struct ath_btcoex_hw {
@@ -98,6 +103,7 @@ struct ath_btcoex_hw {
         u32 bt_coex_mode2;      /* Register setting for AR_BT_COEX_MODE2 */
         u32 bt_weight[AR9300_NUM_BT_WEIGHTS];
         u32 wlan_weight[AR9300_NUM_WLAN_WEIGHTS];
+        u8 tx_prio[ATH_BTCOEX_STOMP_MAX];
 };
 
 void ath9k_hw_btcoex_init_scheme(struct ath_hw *ah);
@@ -112,5 +118,6 @@ void ath9k_hw_btcoex_set_weight(struct ath_hw *ah,
 void ath9k_hw_btcoex_disable(struct ath_hw *ah);
 void ath9k_hw_btcoex_bt_stomp(struct ath_hw *ah,
                               enum ath_stomp_type stomp_type);
+void ath9k_hw_btcoex_set_concur_txprio(struct ath_hw *ah, u8 *stomp_txprio);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/calib.c b/drivers/net/wireless/ath/ath9k/calib.c
index e5cceb077574..f3448a032e6f 100644
--- a/drivers/net/wireless/ath/ath9k/calib.c
+++ b/drivers/net/wireless/ath/ath9k/calib.c
@@ -410,6 +410,7 @@ void ath9k_init_nfcal_hist_buffer(struct ath_hw *ah,
 
         ah->caldata->channel = chan->channel;
         ah->caldata->channelFlags = chan->channelFlags & ~CHANNEL_CW_INT;
+        ah->caldata->chanmode = chan->chanmode;
         h = ah->caldata->nfCalHist;
         default_nf = ath9k_hw_get_default_nf(ah, chan);
         for (i = 0; i < NUM_NF_READINGS; i++) {
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 6727b566d294..a8be94b2a53a 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -1586,6 +1586,35 @@ static const struct file_operations fops_samps = {
 
 #endif
 
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+static ssize_t read_file_btcoex(struct file *file, char __user *user_buf,
+                                size_t count, loff_t *ppos)
+{
+        struct ath_softc *sc = file->private_data;
+        u32 len = 0, size = 1500;
+        char *buf;
+        size_t retval;
+
+        buf = kzalloc(size, GFP_KERNEL);
+        if (buf == NULL)
+                return -ENOMEM;
+
+        len = ath9k_dump_btcoex(sc, buf, len, size);
+
+        retval = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+        kfree(buf);
+
+        return retval;
+}
+
+static const struct file_operations fops_btcoex = {
+        .read = read_file_btcoex,
+        .open = simple_open,
+        .owner = THIS_MODULE,
+        .llseek = default_llseek,
+};
+#endif
+
 int ath9k_init_debug(struct ath_hw *ah)
 {
         struct ath_common *common = ath9k_hw_common(ah);
@@ -1658,6 +1687,9 @@ int ath9k_init_debug(struct ath_hw *ah)
                            sc->debug.debugfs_phy, &sc->sc_ah->gpio_val);
         debugfs_create_file("diversity", S_IRUSR | S_IWUSR,
                             sc->debug.debugfs_phy, sc, &fops_ant_diversity);
-
+#ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
+        debugfs_create_file("btcoex", S_IRUSR, sc->debug.debugfs_phy, sc,
+                            &fops_btcoex);
+#endif
         return 0;
 }
diff --git a/drivers/net/wireless/ath/ath9k/gpio.c b/drivers/net/wireless/ath/ath9k/gpio.c
index d9ed141a053e..a8ea57b9f49c 100644
--- a/drivers/net/wireless/ath/ath9k/gpio.c
+++ b/drivers/net/wireless/ath/ath9k/gpio.c
@@ -187,6 +187,24 @@ static void ath9k_gen_timer_stop(struct ath_hw *ah, struct ath_gen_timer *timer)
         }
 }
 
+static void ath_mci_ftp_adjust(struct ath_softc *sc)
+{
+        struct ath_btcoex *btcoex = &sc->btcoex;
+        struct ath_mci_profile *mci = &btcoex->mci;
+        struct ath_hw *ah = sc->sc_ah;
+
+        if (btcoex->bt_wait_time > ATH_BTCOEX_RX_WAIT_TIME) {
+                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);
+                else
+                        ah->btcoex_hw.mci.stomp_ftp = false;
+                btcoex->bt_wait_time = 0;
+                sc->rx.num_pkts = 0;
+        }
+}
+
 /*
  * This is the master bt coex timer which runs for every
  * 45ms, bt traffic will be given priority during 55% of this
@@ -197,41 +215,43 @@ static void ath_btcoex_period_timer(unsigned long data)
         struct ath_softc *sc = (struct ath_softc *) data;
         struct ath_hw *ah = sc->sc_ah;
         struct ath_btcoex *btcoex = &sc->btcoex;
-        struct ath_mci_profile *mci = &btcoex->mci;
+        enum ath_stomp_type stomp_type;
         u32 timer_period;
-        bool is_btscan;
         unsigned long flags;
 
         spin_lock_irqsave(&sc->sc_pm_lock, flags);
         if (sc->sc_ah->power_mode == ATH9K_PM_NETWORK_SLEEP) {
+                btcoex->bt_wait_time += btcoex->btcoex_period;
                 spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
                 goto skip_hw_wakeup;
         }
         spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
+        ath9k_mci_update_rssi(sc);
+
         ath9k_ps_wakeup(sc);
+
         if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI))
                 ath_detect_bt_priority(sc);
-        is_btscan = test_bit(BT_OP_SCAN, &btcoex->op_flags);
 
-        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) &&
-                    (mci->num_pan || mci->num_other_acl))
-                        ah->btcoex_hw.mci.stomp_ftp =
-                                (sc->rx.num_pkts < ATH_BTCOEX_STOMP_FTP_THRESH);
-                else
-                        ah->btcoex_hw.mci.stomp_ftp = false;
-                btcoex->bt_wait_time = 0;
-                sc->rx.num_pkts = 0;
-        }
+        if (ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
+                ath_mci_ftp_adjust(sc);
 
         spin_lock_bh(&btcoex->btcoex_lock);
 
-        ath9k_hw_btcoex_bt_stomp(ah, is_btscan ? ATH_BTCOEX_STOMP_ALL :
-                              btcoex->bt_stomp_type);
+        stomp_type = btcoex->bt_stomp_type;
+        timer_period = btcoex->btcoex_no_stomp;
+
+        if (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI)) {
+                if (test_bit(BT_OP_SCAN, &btcoex->op_flags)) {
+                        stomp_type = ATH_BTCOEX_STOMP_ALL;
+                        timer_period = btcoex->btscan_no_stomp;
+                }
+        }
 
+        ath9k_hw_btcoex_bt_stomp(ah, stomp_type);
         ath9k_hw_btcoex_enable(ah);
+
         spin_unlock_bh(&btcoex->btcoex_lock);
 
         /*
@@ -243,17 +263,16 @@ static void ath_btcoex_period_timer(unsigned long data)
                 if (btcoex->hw_timer_enabled)
                         ath9k_gen_timer_stop(ah, btcoex->no_stomp_timer);
 
-                timer_period = is_btscan ? btcoex->btscan_no_stomp :
-                                           btcoex->btcoex_no_stomp;
                 ath9k_gen_timer_start(ah, btcoex->no_stomp_timer, timer_period,
                                       timer_period * 10);
                 btcoex->hw_timer_enabled = true;
         }
 
         ath9k_ps_restore(sc);
+
 skip_hw_wakeup:
-        timer_period = btcoex->btcoex_period;
-        mod_timer(&btcoex->period_timer, jiffies + msecs_to_jiffies(timer_period));
+        mod_timer(&btcoex->period_timer,
+                  jiffies + msecs_to_jiffies(btcoex->btcoex_period));
 }
 
 /*
@@ -273,7 +292,8 @@ static void ath_btcoex_no_stomp_timer(void *arg)
         spin_lock_bh(&btcoex->btcoex_lock);
 
         if (btcoex->bt_stomp_type == ATH_BTCOEX_STOMP_LOW ||
-            test_bit(BT_OP_SCAN, &btcoex->op_flags))
+            (!(ah->caps.hw_caps & ATH9K_HW_CAP_MCI) &&
+             test_bit(BT_OP_SCAN, &btcoex->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);
@@ -474,4 +494,52 @@ int ath9k_init_btcoex(struct ath_softc *sc)
         return 0;
 }
 
+int ath9k_dump_btcoex(struct ath_softc *sc, u8 *buf, u32 len, u32 size)
+{
+#define ATH_DUMP_BTCOEX(_s, _val)                                \
+        do {                                                     \
+                len += snprintf(buf + len, size - len,           \
+                                "%20s : %10d\n", _s, (_val));    \
+        } while (0)
+
+        struct ath_btcoex *btcoex = &sc->btcoex;
+        struct ath_mci_profile *mci = &btcoex->mci;
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath_btcoex_hw *btcoex_hw = &ah->btcoex_hw;
+        int i;
+
+        ATH_DUMP_BTCOEX("Total BT profiles", NUM_PROF(mci));
+        ATH_DUMP_BTCOEX("Number of MGMT", mci->num_mgmt);
+        ATH_DUMP_BTCOEX("Number of SCO", mci->num_sco);
+        ATH_DUMP_BTCOEX("Number of A2DP", mci->num_a2dp);
+        ATH_DUMP_BTCOEX("Number of HID", mci->num_hid);
+        ATH_DUMP_BTCOEX("Number of PAN", mci->num_pan);
+        ATH_DUMP_BTCOEX("Number of ACL", mci->num_other_acl);
+        ATH_DUMP_BTCOEX("Number of BDR", mci->num_bdr);
+        ATH_DUMP_BTCOEX("Aggr. Limit", mci->aggr_limit);
+        ATH_DUMP_BTCOEX("Stomp Type", btcoex->bt_stomp_type);
+        ATH_DUMP_BTCOEX("BTCoex Period (msec)", btcoex->btcoex_period);
+        ATH_DUMP_BTCOEX("Duty Cycle", btcoex->duty_cycle);
+        ATH_DUMP_BTCOEX("BT Wait time", btcoex->bt_wait_time);
+        ATH_DUMP_BTCOEX("Concurrent Tx", btcoex_hw->mci.concur_tx);
+        ATH_DUMP_BTCOEX("Concur RSSI count", btcoex->rssi_count);
+        len += snprintf(buf + len, size - len, "BT Weights: ");
+        for (i = 0; i < AR9300_NUM_BT_WEIGHTS; i++)
+                len += snprintf(buf + len, size - len, "%08x ",
+                                btcoex_hw->bt_weight[i]);
+        len += snprintf(buf + len, size - len, "\n");
+        len += snprintf(buf + len, size - len, "WLAN Weights: ");
+        for (i = 0; i < AR9300_NUM_BT_WEIGHTS; i++)
+                len += snprintf(buf + len, size - len, "%08x ",
+                                btcoex_hw->wlan_weight[i]);
+        len += snprintf(buf + len, size - len, "\n");
+        len += snprintf(buf + len, size - len, "Tx Priorities: ");
+        for (i = 0; i < ATH_BTCOEX_STOMP_MAX; i++)
+                len += snprintf(buf + len, size - len, "%08x ",
+                                btcoex_hw->tx_prio[i]);
+        len += snprintf(buf + len, size - len, "\n");
+#undef ATH_DUMP_BTCOEX
+
+        return len;
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
diff --git a/drivers/net/wireless/ath/ath9k/hif_usb.c b/drivers/net/wireless/ath/ath9k/hif_usb.c
index 924c4616c3d9..f5dda84176c3 100644
--- a/drivers/net/wireless/ath/ath9k/hif_usb.c
+++ b/drivers/net/wireless/ath/ath9k/hif_usb.c
@@ -38,6 +38,7 @@ static struct usb_device_id ath9k_hif_usb_ids[] = {
         { USB_DEVICE(0x04CA, 0x4605) }, /* Liteon */
         { USB_DEVICE(0x040D, 0x3801) }, /* VIA */
         { USB_DEVICE(0x0cf3, 0xb003) }, /* Ubiquiti WifiStation Ext */
+        { USB_DEVICE(0x0cf3, 0xb002) }, /* Ubiquiti WifiStation */
         { USB_DEVICE(0x057c, 0x8403) }, /* AVM FRITZ!WLAN 11N v2 USB */
 
         { USB_DEVICE(0x0cf3, 0x7015),
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index d98255eb1b9a..5ecf1287dddd 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -694,6 +694,20 @@ err_hw:
         return ret;
 }
 
+static const struct ieee80211_iface_limit if_limits[] = {
+        { .max = 2,     .types = BIT(NL80211_IFTYPE_STATION) |
+                                 BIT(NL80211_IFTYPE_P2P_CLIENT) },
+        { .max = 2,     .types = BIT(NL80211_IFTYPE_AP) |
+                                 BIT(NL80211_IFTYPE_P2P_GO) },
+};
+
+static const struct ieee80211_iface_combination if_comb = {
+        .limits = if_limits,
+        .n_limits = ARRAY_SIZE(if_limits),
+        .max_interfaces = 2,
+        .num_different_channels = 1,
+};
+
 static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
                                struct ieee80211_hw *hw)
 {
@@ -716,6 +730,9 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
                 BIT(NL80211_IFTYPE_P2P_GO) |
                 BIT(NL80211_IFTYPE_P2P_CLIENT);
 
+        hw->wiphy->iface_combinations = &if_comb;
+        hw->wiphy->n_iface_combinations = 1;
+
         hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
         hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN |
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index ca78e33ca23e..66f6a74c508e 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -1036,26 +1036,6 @@ static int ath9k_htc_add_interface(struct ieee80211_hw *hw,
 
         mutex_lock(&priv->mutex);
 
-        if (priv->nvifs >= ATH9K_HTC_MAX_VIF) {
-                mutex_unlock(&priv->mutex);
-                return -ENOBUFS;
-        }
-
-        if (priv->num_ibss_vif ||
-            (priv->nvifs && vif->type == NL80211_IFTYPE_ADHOC)) {
-                ath_err(common, "IBSS coexistence with other modes is not allowed\n");
-                mutex_unlock(&priv->mutex);
-                return -ENOBUFS;
-        }
-
-        if (((vif->type == NL80211_IFTYPE_AP) ||
-             (vif->type == NL80211_IFTYPE_ADHOC)) &&
-            ((priv->num_ap_vif + priv->num_ibss_vif) >= ATH9K_HTC_MAX_BCN_VIF)) {
-                ath_err(common, "Max. number of beaconing interfaces reached\n");
-                mutex_unlock(&priv->mutex);
-                return -ENOBUFS;
-        }
-
         ath9k_htc_ps_wakeup(priv);
         memset(&hvif, 0, sizeof(struct ath9k_htc_target_vif));
         memcpy(&hvif.myaddr, vif->addr, ETH_ALEN);
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 8e1559aba495..71cd9f0c96af 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -2153,9 +2153,6 @@ 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)
@@ -2171,6 +2168,9 @@ static bool ath9k_hw_set_power_awake(struct ath_hw *ah)
                 return false;
         }
 
+        if (ath9k_hw_mci_is_enabled(ah))
+                ar9003_mci_set_power_awake(ah);
+
         REG_CLR_BIT(ah, AR_STA_ID1, AR_STA_ID1_PWR_SAV);
 
         return true;
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index dbc1b7a4cbfd..3e73bfe2315e 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -401,6 +401,7 @@ enum ath9k_int {
 struct ath9k_hw_cal_data {
         u16 channel;
         u32 channelFlags;
+        u32 chanmode;
         int32_t CalValid;
         int8_t iCoff;
         int8_t qCoff;
@@ -834,6 +835,7 @@ struct ath_hw {
         int coarse_low[5];
         int firpwr[5];
         enum ath9k_ani_cmd ani_function;
+        u32 ani_skip_count;
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
         struct ath_btcoex_hw btcoex_hw;
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index fad3ccd5cd91..546bae93647b 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -687,6 +687,7 @@ static const struct ieee80211_iface_combination if_comb = {
         .n_limits = ARRAY_SIZE(if_limits),
         .max_interfaces = 2048,
         .num_different_channels = 1,
+        .beacon_int_infra_match = true,
 };
 
 void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 7b88b9c39ccd..223b9693527e 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -350,8 +350,18 @@ void ath_ani_calibrate(unsigned long data)
                 ATH_AP_SHORT_CALINTERVAL : ATH_STA_SHORT_CALINTERVAL;
 
         /* Only calibrate if awake */
-        if (sc->sc_ah->power_mode != ATH9K_PM_AWAKE)
+        if (sc->sc_ah->power_mode != ATH9K_PM_AWAKE) {
+                if (++ah->ani_skip_count >= ATH_ANI_MAX_SKIP_COUNT) {
+                        spin_lock_irqsave(&sc->sc_pm_lock, flags);
+                        sc->ps_flags |= PS_WAIT_FOR_ANI;
+                        spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
+                }
                 goto set_timer;
+        }
+        ah->ani_skip_count = 0;
+        spin_lock_irqsave(&sc->sc_pm_lock, flags);
+        sc->ps_flags &= ~PS_WAIT_FOR_ANI;
+        spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
         ath9k_ps_wakeup(sc);
 
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index dd45edfa6bae..578a7234aa56 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -131,7 +131,8 @@ void ath9k_ps_restore(struct ath_softc *sc)
                    !(sc->ps_flags & (PS_WAIT_FOR_BEACON |
                                      PS_WAIT_FOR_CAB |
                                      PS_WAIT_FOR_PSPOLL_DATA |
-                                     PS_WAIT_FOR_TX_ACK))) {
+                                     PS_WAIT_FOR_TX_ACK |
+                                     PS_WAIT_FOR_ANI))) {
                 mode = ATH9K_PM_NETWORK_SLEEP;
                 if (ath9k_hw_btcoex_is_enabled(sc->sc_ah))
                         ath9k_btcoex_stop_gen_timer(sc);
@@ -292,6 +293,10 @@ static int ath_reset_internal(struct ath_softc *sc, struct ath9k_channel *hchan,
                 goto out;
         }
 
+        if (ath9k_hw_mci_is_enabled(sc->sc_ah) &&
+            (sc->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL))
+                ath9k_mci_set_txpower(sc, true, false);
+
         if (!ath_complete_reset(sc, true))
                 r = -EIO;
 
@@ -1449,6 +1454,9 @@ static void ath9k_set_assoc_state(struct ath_softc *sc,
         sc->ps_flags |= PS_BEACON_SYNC | PS_WAIT_FOR_BEACON;
         spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 
+        if (ath9k_hw_mci_is_enabled(sc->sc_ah))
+                ath9k_mci_update_wlan_channels(sc, false);
+
         ath_dbg(common, CONFIG,
                 "Primary Station interface: %pM, BSSID: %pM\n",
                 vif->addr, common->curbssid);
@@ -1505,6 +1513,8 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
                         memset(common->curbssid, 0, ETH_ALEN);
                         common->curaid = 0;
                         ath9k_hw_write_associd(sc->sc_ah);
+                        if (ath9k_hw_mci_is_enabled(sc->sc_ah))
+                                ath9k_mci_update_wlan_channels(sc, true);
                 }
         }
 
diff --git a/drivers/net/wireless/ath/ath9k/mci.c b/drivers/net/wireless/ath/ath9k/mci.c
index ec2d7c807567..0dd2cbb52d65 100644
--- a/drivers/net/wireless/ath/ath9k/mci.c
+++ b/drivers/net/wireless/ath/ath9k/mci.c
@@ -43,6 +43,7 @@ static bool ath_mci_add_profile(struct ath_common *common,
                                 struct ath_mci_profile_info *info)
 {
         struct ath_mci_profile_info *entry;
+        u8 voice_priority[] = { 110, 110, 110, 112, 110, 110, 114, 116, 118 };
 
         if ((mci->num_sco == ATH_MCI_MAX_SCO_PROFILE) &&
             (info->type == MCI_GPM_COEX_PROFILE_VOICE))
@@ -59,6 +60,12 @@ static bool ath_mci_add_profile(struct ath_common *common,
         memcpy(entry, info, 10);
         INC_PROF(mci, info);
         list_add_tail(&entry->list, &mci->info);
+        if (info->type == MCI_GPM_COEX_PROFILE_VOICE) {
+                if (info->voice_type < sizeof(voice_priority))
+                        mci->voice_priority = voice_priority[info->voice_type];
+                else
+                        mci->voice_priority = 110;
+        }
 
         return true;
 }
@@ -150,7 +157,7 @@ static void ath_mci_update_scheme(struct ath_softc *sc)
                          * For single PAN/FTP profile, allocate 35% for BT
                          * to improve WLAN throughput.
                          */
-                        btcoex->duty_cycle = 35;
+                        btcoex->duty_cycle = AR_SREV_9565(sc->sc_ah) ? 40 : 35;
                         btcoex->btcoex_period = 53;
                         ath_dbg(common, MCI,
                                 "Single PAN/FTP bt period %d ms dutycycle %d\n",
@@ -250,6 +257,57 @@ static void ath9k_mci_work(struct work_struct *work)
         ath_mci_update_scheme(sc);
 }
 
+static void ath_mci_update_stomp_txprio(u8 cur_txprio, u8 *stomp_prio)
+{
+        if (cur_txprio < stomp_prio[ATH_BTCOEX_STOMP_NONE])
+                stomp_prio[ATH_BTCOEX_STOMP_NONE] = cur_txprio;
+
+        if (cur_txprio > stomp_prio[ATH_BTCOEX_STOMP_ALL])
+                stomp_prio[ATH_BTCOEX_STOMP_ALL] = cur_txprio;
+
+        if ((cur_txprio > ATH_MCI_HI_PRIO) &&
+            (cur_txprio < stomp_prio[ATH_BTCOEX_STOMP_LOW]))
+                stomp_prio[ATH_BTCOEX_STOMP_LOW] = cur_txprio;
+}
+
+static void ath_mci_set_concur_txprio(struct ath_softc *sc)
+{
+        struct ath_btcoex *btcoex = &sc->btcoex;
+        struct ath_mci_profile *mci = &btcoex->mci;
+        u8 stomp_txprio[] = { 0, 0, 0, 0 }; /* all, low, none, low_ftp */
+
+        if (mci->num_mgmt) {
+                stomp_txprio[ATH_BTCOEX_STOMP_ALL] = ATH_MCI_INQUIRY_PRIO;
+                if (!mci->num_pan && !mci->num_other_acl)
+                        stomp_txprio[ATH_BTCOEX_STOMP_NONE] =
+                                ATH_MCI_INQUIRY_PRIO;
+        } else {
+                u8 prof_prio[] = { 50, 90, 94, 52 };/* RFCOMM, A2DP, HID, PAN */
+
+                stomp_txprio[ATH_BTCOEX_STOMP_LOW] =
+                stomp_txprio[ATH_BTCOEX_STOMP_NONE] = 0xff;
+
+                if (mci->num_sco)
+                        ath_mci_update_stomp_txprio(mci->voice_priority,
+                                                    stomp_txprio);
+                if (mci->num_other_acl)
+                        ath_mci_update_stomp_txprio(prof_prio[0], stomp_txprio);
+                if (mci->num_a2dp)
+                        ath_mci_update_stomp_txprio(prof_prio[1], stomp_txprio);
+                if (mci->num_hid)
+                        ath_mci_update_stomp_txprio(prof_prio[2], stomp_txprio);
+                if (mci->num_pan)
+                        ath_mci_update_stomp_txprio(prof_prio[3], stomp_txprio);
+
+                if (stomp_txprio[ATH_BTCOEX_STOMP_NONE] == 0xff)
+                        stomp_txprio[ATH_BTCOEX_STOMP_NONE] = 0;
+
+                if (stomp_txprio[ATH_BTCOEX_STOMP_LOW] == 0xff)
+                        stomp_txprio[ATH_BTCOEX_STOMP_LOW] = 0;
+        }
+        ath9k_hw_btcoex_set_concur_txprio(sc->sc_ah, stomp_txprio);
+}
+
 static u8 ath_mci_process_profile(struct ath_softc *sc,
                                   struct ath_mci_profile_info *info)
 {
@@ -281,6 +339,7 @@ static u8 ath_mci_process_profile(struct ath_softc *sc,
         } else
                 ath_mci_del_profile(common, mci, entry);
 
+        ath_mci_set_concur_txprio(sc);
         return 1;
 }
 
@@ -314,6 +373,7 @@ static u8 ath_mci_process_status(struct ath_softc *sc,
                         mci->num_mgmt++;
         } while (++i < ATH_MCI_MAX_PROFILE);
 
+        ath_mci_set_concur_txprio(sc);
         if (old_num_mgmt != mci->num_mgmt)
                 return 1;
 
@@ -600,3 +660,112 @@ void ath_mci_enable(struct ath_softc *sc)
         if (sc->sc_ah->caps.hw_caps & ATH9K_HW_CAP_MCI)
                 sc->sc_ah->imask |= ATH9K_INT_MCI;
 }
+
+void ath9k_mci_update_wlan_channels(struct ath_softc *sc, bool allow_all)
+{
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath9k_hw_mci *mci = &ah->btcoex_hw.mci;
+        struct ath9k_channel *chan = ah->curchan;
+        u32 channelmap[] = {0x00000000, 0xffff0000, 0xffffffff, 0x7fffffff};
+        int i;
+        s16 chan_start, chan_end;
+        u16 wlan_chan;
+
+        if (!chan || !IS_CHAN_2GHZ(chan))
+                return;
+
+        if (allow_all)
+                goto send_wlan_chan;
+
+        wlan_chan = chan->channel - 2402;
+
+        chan_start = wlan_chan - 10;
+        chan_end = wlan_chan + 10;
+
+        if (chan->chanmode == CHANNEL_G_HT40PLUS)
+                chan_end += 20;
+        else if (chan->chanmode == CHANNEL_G_HT40MINUS)
+                chan_start -= 20;
+
+        /* adjust side band */
+        chan_start -= 7;
+        chan_end += 7;
+
+        if (chan_start <= 0)
+                chan_start = 0;
+        if (chan_end >= ATH_MCI_NUM_BT_CHANNELS)
+                chan_end = ATH_MCI_NUM_BT_CHANNELS - 1;
+
+        ath_dbg(ath9k_hw_common(ah), MCI,
+                "WLAN current channel %d mask BT channel %d - %d\n",
+                wlan_chan, chan_start, chan_end);
+
+        for (i = chan_start; i < chan_end; i++)
+                MCI_GPM_CLR_CHANNEL_BIT(&channelmap, i);
+
+send_wlan_chan:
+        /* update and send wlan channels info to BT */
+        for (i = 0; i < 4; i++)
+                mci->wlan_channels[i] = channelmap[i];
+        ar9003_mci_send_wlan_channels(ah);
+        ar9003_mci_state(ah, MCI_STATE_SEND_VERSION_QUERY);
+}
+
+void ath9k_mci_set_txpower(struct ath_softc *sc, bool setchannel,
+                           bool concur_tx)
+{
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath9k_hw_mci *mci_hw = &sc->sc_ah->btcoex_hw.mci;
+        bool old_concur_tx = mci_hw->concur_tx;
+
+        if (!(mci_hw->config & ATH_MCI_CONFIG_CONCUR_TX)) {
+                mci_hw->concur_tx = false;
+                return;
+        }
+
+        if (!IS_CHAN_2GHZ(ah->curchan))
+                return;
+
+        if (setchannel) {
+                struct ath9k_hw_cal_data *caldata = &sc->caldata;
+                if ((caldata->chanmode == CHANNEL_G_HT40PLUS) &&
+                    (ah->curchan->channel > caldata->channel) &&
+                    (ah->curchan->channel <= caldata->channel + 20))
+                        return;
+                if ((caldata->chanmode == CHANNEL_G_HT40MINUS) &&
+                    (ah->curchan->channel < caldata->channel) &&
+                    (ah->curchan->channel >= caldata->channel - 20))
+                        return;
+                mci_hw->concur_tx = false;
+        } else
+                mci_hw->concur_tx = concur_tx;
+
+        if (old_concur_tx != mci_hw->concur_tx)
+                ath9k_hw_set_txpowerlimit(ah, sc->config.txpowlimit, false);
+}
+
+void ath9k_mci_update_rssi(struct ath_softc *sc)
+{
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath_btcoex *btcoex = &sc->btcoex;
+        struct ath9k_hw_mci *mci_hw = &sc->sc_ah->btcoex_hw.mci;
+
+        if (!(mci_hw->config & ATH_MCI_CONFIG_CONCUR_TX))
+                return;
+
+        if (ah->stats.avgbrssi >= 40) {
+                if (btcoex->rssi_count < 0)
+                        btcoex->rssi_count = 0;
+                if (++btcoex->rssi_count >= ATH_MCI_CONCUR_TX_SWITCH) {
+                        btcoex->rssi_count = 0;
+                        ath9k_mci_set_txpower(sc, false, true);
+                }
+        } else {
+                if (btcoex->rssi_count > 0)
+                        btcoex->rssi_count = 0;
+                if (--btcoex->rssi_count <= -ATH_MCI_CONCUR_TX_SWITCH) {
+                        btcoex->rssi_count = 0;
+                        ath9k_mci_set_txpower(sc, false, false);
+                }
+        }
+}
diff --git a/drivers/net/wireless/ath/ath9k/mci.h b/drivers/net/wireless/ath/ath9k/mci.h
index fc14eea034eb..06958837620c 100644
--- a/drivers/net/wireless/ath/ath9k/mci.h
+++ b/drivers/net/wireless/ath/ath9k/mci.h
@@ -32,6 +32,27 @@
 #define ATH_MCI_MAX_PROFILE             (ATH_MCI_MAX_ACL_PROFILE +\
                                          ATH_MCI_MAX_SCO_PROFILE)
 
+#define ATH_MCI_INQUIRY_PRIO         62
+#define ATH_MCI_HI_PRIO              60
+#define ATH_MCI_NUM_BT_CHANNELS      79
+#define ATH_MCI_CONCUR_TX_SWITCH      5
+
+#define MCI_GPM_SET_CHANNEL_BIT(_p_gpm, _bt_chan)                         \
+        do {                                                              \
+                if (_bt_chan < ATH_MCI_NUM_BT_CHANNELS) {                 \
+                        *(((u8 *)(_p_gpm)) + MCI_GPM_COEX_B_CHANNEL_MAP + \
+                                (_bt_chan / 8)) |= (1 << (_bt_chan & 7)); \
+                }                                                         \
+        } while (0)
+
+#define MCI_GPM_CLR_CHANNEL_BIT(_p_gpm, _bt_chan)                         \
+        do {                                                              \
+                if (_bt_chan < ATH_MCI_NUM_BT_CHANNELS) {                 \
+                        *(((u8 *)(_p_gpm)) + MCI_GPM_COEX_B_CHANNEL_MAP + \
+                                (_bt_chan / 8)) &= ~(1 << (_bt_chan & 7));\
+                }                                                         \
+        } while (0)
+
 #define INC_PROF(_mci, _info) do {               \
                 switch (_info->type) {           \
                 case MCI_GPM_COEX_PROFILE_RFCOMM:\
@@ -49,6 +70,7 @@
                         _mci->num_pan++;         \
                         break;                   \
                 case MCI_GPM_COEX_PROFILE_VOICE: \
+                case MCI_GPM_COEX_PROFILE_A2DPVO:\
                         _mci->num_sco++;         \
                         break;                   \
                 default:                         \
@@ -73,6 +95,7 @@
                         _mci->num_pan--;         \
                         break;                   \
                 case MCI_GPM_COEX_PROFILE_VOICE: \
+                case MCI_GPM_COEX_PROFILE_A2DPVO:\
                         _mci->num_sco--;         \
                         break;                   \
                 default:                         \
@@ -113,6 +136,7 @@ struct ath_mci_profile {
         u8 num_pan;
         u8 num_other_acl;
         u8 num_bdr;
+        u8 voice_priority;
 };
 
 struct ath_mci_buf {
@@ -130,13 +154,25 @@ void ath_mci_flush_profile(struct ath_mci_profile *mci);
 int ath_mci_setup(struct ath_softc *sc);
 void ath_mci_cleanup(struct ath_softc *sc);
 void ath_mci_intr(struct ath_softc *sc);
+void ath9k_mci_update_rssi(struct ath_softc *sc);
 
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 void ath_mci_enable(struct ath_softc *sc);
+void ath9k_mci_update_wlan_channels(struct ath_softc *sc, bool allow_all);
+void ath9k_mci_set_txpower(struct ath_softc *sc, bool setchannel,
+                           bool concur_tx);
 #else
 static inline void ath_mci_enable(struct ath_softc *sc)
 {
 }
+static inline void ath9k_mci_update_wlan_channels(struct ath_softc *sc,
+                                                  bool allow_all)
+{
+}
+static inline void ath9k_mci_set_txpower(struct ath_softc *sc, bool setchannel,
+                                         bool concur_tx)
+{
+}
 #endif /* CONFIG_ATH9K_BTCOEX_SUPPORT */
 
 #endif /* MCI_H*/
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 83d16e7ed272..a04028bce28b 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -1105,7 +1105,10 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
                 else
                         rs.is_mybeacon = false;
 
-                sc->rx.num_pkts++;
+                if (ieee80211_is_data_present(hdr->frame_control) &&
+                    !ieee80211_is_qos_nullfunc(hdr->frame_control))
+                        sc->rx.num_pkts++;
+
                 ath_debug_stat_rx(sc, &rs);
 
                 /*
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index 4e6760f8596d..ad3c82c09177 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -907,10 +907,6 @@
         (((_ah)->hw_version.macVersion == AR_SREV_VERSION_9462) && \
         ((_ah)->hw_version.macRev == AR_SREV_REVISION_9462_20))
 
-#define AR_SREV_9462_20_OR_LATER(_ah) \
-        (((_ah)->hw_version.macVersion == AR_SREV_VERSION_9462) && \
-        ((_ah)->hw_version.macRev >= AR_SREV_REVISION_9462_20))
-
 #define AR_SREV_9565(_ah) \
         (((_ah)->hw_version.macVersion == AR_SREV_VERSION_9565))
 
@@ -2315,6 +2311,8 @@ enum {
 #define AR_BTCOEX_MAX_TXPWR(_x)                         (0x18c0 + ((_x) << 2))
 #define AR_BTCOEX_WL_LNA                                0x1940
 #define AR_BTCOEX_RFGAIN_CTRL                           0x1944
+#define AR_BTCOEX_WL_LNA_TIMEOUT                        0x003FFFFF
+#define AR_BTCOEX_WL_LNA_TIMEOUT_S                      0
 
 #define AR_BTCOEX_CTRL2                                 0x1948
 #define AR_BTCOEX_CTRL2_TXPWR_THRESH                    0x0007F800
@@ -2360,4 +2358,11 @@ enum {
 #define AR_GLB_SWREG_DISCONT_MODE         0x2002c
 #define AR_GLB_SWREG_DISCONT_EN_BT_WLAN   0x3
 
+#define AR_MCI_MISC                    0x1a74
+#define AR_MCI_MISC_HW_FIX_EN          0x00000001
+#define AR_MCI_MISC_HW_FIX_EN_S        0
+#define AR_MCI_DBG_CNT_CTRL            0x1a78
+#define AR_MCI_DBG_CNT_CTRL_ENABLE     0x00000001
+#define AR_MCI_DBG_CNT_CTRL_ENABLE_S   0
+
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/wow.c b/drivers/net/wireless/ath/ath9k/wow.c
index a483d518758c..9f8563091bea 100644
--- a/drivers/net/wireless/ath/ath9k/wow.c
+++ b/drivers/net/wireless/ath/ath9k/wow.c
@@ -118,7 +118,7 @@ static void ath9k_wow_create_keep_alive_pattern(struct ath_hw *ah)
                        (ap_mac_addr[1] << 8) | (ap_mac_addr[0]);
         data_word[5] = (ap_mac_addr[5] << 8) | (ap_mac_addr[4]);
 
-        if (AR_SREV_9462_20_OR_LATER(ah)) {
+        if (AR_SREV_9462_20(ah)) {
                 /* AR9462 2.0 has an extra descriptor word (time based
                  * discard) compared to other chips */
                 REG_WRITE(ah, (AR_WOW_KA_DESC_WORD2 + (12 * 4)), 0);
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 378bd70256b2..1ffca7511fa8 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -312,6 +312,7 @@ static struct ath_buf *ath_tx_get_buffer(struct ath_softc *sc)
         }
 
         bf = list_first_entry(&sc->tx.txbuf, struct ath_buf, list);
+        bf->bf_next = NULL;
         list_del(&bf->list);
 
         spin_unlock_bh(&sc->tx.txbuflock);
@@ -1774,6 +1775,7 @@ static void ath_tx_send_normal(struct ath_softc *sc, struct ath_txq *txq,
         list_add_tail(&bf->list, &bf_head);
         bf->bf_state.bf_type = 0;
 
+        bf->bf_next = NULL;
         bf->bf_lastbf = bf;
         ath_tx_fill_desc(sc, bf, txq, fi->framelen);
         ath_tx_txqaddbuf(sc, txq, &bf_head, false);
diff --git a/drivers/net/wireless/ath/carl9170/mac.c b/drivers/net/wireless/ath/carl9170/mac.c
index e3b1b6e87760..24d75ab94f0d 100644
--- a/drivers/net/wireless/ath/carl9170/mac.c
+++ b/drivers/net/wireless/ath/carl9170/mac.c
@@ -343,7 +343,24 @@ int carl9170_set_operating_mode(struct ar9170 *ar)
                         break;
                 }
         } else {
-                mac_addr = NULL;
+                /*
+                 * Enable monitor mode
+                 *
+                 * rx_ctrl |= AR9170_MAC_RX_CTRL_ACK_IN_SNIFFER;
+                 * sniffer |= AR9170_MAC_SNIFFER_ENABLE_PROMISC;
+                 *
+                 * When the hardware is in SNIFFER_PROMISC mode,
+                 * it generates spurious ACKs for every incoming
+                 * frame. This confuses every peer in the
+                 * vicinity and the network throughput will suffer
+                 * badly.
+                 *
+                 * Hence, the hardware will be put into station
+                 * mode and just the rx filters are disabled.
+                 */
+                cam_mode |= AR9170_MAC_CAM_STA;
+                rx_ctrl |= AR9170_MAC_RX_CTRL_PASS_TO_HOST;
+                mac_addr = common->macaddr;
                 bssid = NULL;
         }
         rcu_read_unlock();
@@ -355,8 +372,6 @@ int carl9170_set_operating_mode(struct ar9170 *ar)
                 enc_mode |= AR9170_MAC_ENCRYPTION_RX_SOFTWARE;
 
         if (ar->sniffer_enabled) {
-                rx_ctrl |= AR9170_MAC_RX_CTRL_ACK_IN_SNIFFER;
-                sniffer |= AR9170_MAC_SNIFFER_ENABLE_PROMISC;
                 enc_mode |= AR9170_MAC_ENCRYPTION_RX_SOFTWARE;
         }
 
diff --git a/drivers/net/wireless/ath/carl9170/rx.c b/drivers/net/wireless/ath/carl9170/rx.c
index a0b723078547..6d22382875bc 100644
--- a/drivers/net/wireless/ath/carl9170/rx.c
+++ b/drivers/net/wireless/ath/carl9170/rx.c
@@ -164,9 +164,6 @@ void carl9170_handle_command_response(struct ar9170 *ar, void *buf, u32 len)
         struct carl9170_rsp *cmd = buf;
         struct ieee80211_vif *vif;
 
-        if (carl9170_check_sequence(ar, cmd->hdr.seq))
-                return;
-
         if ((cmd->hdr.cmd & CARL9170_RSP_FLAG) != CARL9170_RSP_FLAG) {
                 if (!(cmd->hdr.cmd & CARL9170_CMD_ASYNC_FLAG))
                         carl9170_cmd_callback(ar, len, buf);
@@ -663,6 +660,35 @@ static bool carl9170_ampdu_check(struct ar9170 *ar, u8 *buf, u8 ms,
         return false;
 }
 
+static int carl9170_handle_mpdu(struct ar9170 *ar, u8 *buf, int len,
+                                struct ieee80211_rx_status *status)
+{
+        struct sk_buff *skb;
+
+        /* (driver) frame trap handler
+         *
+         * Because power-saving mode handing has to be implemented by
+         * the driver/firmware. We have to check each incoming beacon
+         * from the associated AP, if there's new data for us (either
+         * broadcast/multicast or unicast) we have to react quickly.
+         *
+         * So, if you have you want to add additional frame trap
+         * handlers, this would be the perfect place!
+         */
+
+        carl9170_ps_beacon(ar, buf, len);
+
+        carl9170_ba_check(ar, buf, len);
+
+        skb = carl9170_rx_copy_data(buf, len);
+        if (!skb)
+                return -ENOMEM;
+
+        memcpy(IEEE80211_SKB_RXCB(skb), &status, sizeof(status));
+        ieee80211_rx(ar->hw, skb);
+        return 0;
+}
+
 /*
  * If the frame alignment is right (or the kernel has
  * CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS), and there
@@ -672,14 +698,12 @@ static bool carl9170_ampdu_check(struct ar9170 *ar, u8 *buf, u8 ms,
  * mode, and we need to observe the proper ordering,
  * this is non-trivial.
  */
-
-static void carl9170_handle_mpdu(struct ar9170 *ar, u8 *buf, int len)
+static void carl9170_rx_untie_data(struct ar9170 *ar, u8 *buf, int len)
 {
         struct ar9170_rx_head *head;
         struct ar9170_rx_macstatus *mac;
         struct ar9170_rx_phystatus *phy = NULL;
         struct ieee80211_rx_status status;
-        struct sk_buff *skb;
         int mpdu_len;
         u8 mac_status;
 
@@ -791,18 +815,10 @@ static void carl9170_handle_mpdu(struct ar9170 *ar, u8 *buf, int len)
         if (phy)
                 carl9170_rx_phy_status(ar, phy, &status);
 
-        carl9170_ps_beacon(ar, buf, mpdu_len);
-
-        carl9170_ba_check(ar, buf, mpdu_len);
-
-        skb = carl9170_rx_copy_data(buf, mpdu_len);
-        if (!skb)
+        if (carl9170_handle_mpdu(ar, buf, mpdu_len, &status))
                 goto drop;
 
-        memcpy(IEEE80211_SKB_RXCB(skb), &status, sizeof(status));
-        ieee80211_rx(ar->hw, skb);
         return;
-
 drop:
         ar->rx_dropped++;
 }
@@ -820,6 +836,9 @@ static void carl9170_rx_untie_cmds(struct ar9170 *ar, const u8 *respbuf,
                 if (unlikely(i > resplen))
                         break;
 
+                if (carl9170_check_sequence(ar, cmd->hdr.seq))
+                        break;
+
                 carl9170_handle_command_response(ar, cmd, cmd->hdr.len + 4);
         }
 
@@ -851,7 +870,7 @@ static void __carl9170_rx(struct ar9170 *ar, u8 *buf, unsigned int len)
         if (i == 12)
                 carl9170_rx_untie_cmds(ar, buf, len);
         else
-                carl9170_handle_mpdu(ar, buf, len);
+                carl9170_rx_untie_data(ar, buf, len);
 }
 
 static void carl9170_rx_stream(struct ar9170 *ar, void *buf, unsigned int len)
diff --git a/drivers/net/wireless/ath/carl9170/usb.c b/drivers/net/wireless/ath/carl9170/usb.c
index 888152ce3eca..307bc0ddff99 100644
--- a/drivers/net/wireless/ath/carl9170/usb.c
+++ b/drivers/net/wireless/ath/carl9170/usb.c
@@ -295,6 +295,13 @@ static void carl9170_usb_rx_irq_complete(struct urb *urb)
                 goto resubmit;
         }
 
+        /*
+         * While the carl9170 firmware does not use this EP, the
+         * firmware loader in the EEPROM unfortunately does.
+         * Therefore we need to be ready to handle out-of-band
+         * responses and traps in case the firmware crashed and
+         * the loader took over again.
+         */
         carl9170_handle_command_response(ar, urb->transfer_buffer,
                                          urb->actual_length);
 
diff --git a/drivers/net/wireless/ath/hw.c b/drivers/net/wireless/ath/hw.c
index 19befb331073..39e8a590d7fc 100644
--- a/drivers/net/wireless/ath/hw.c
+++ b/drivers/net/wireless/ath/hw.c
@@ -20,8 +20,8 @@
 #include "ath.h"
 #include "reg.h"
 
-#define REG_READ        (common->ops->read)
-#define REG_WRITE       (common->ops->write)
+#define REG_READ                        (common->ops->read)
+#define REG_WRITE(_ah, _reg, _val)      (common->ops->write)(_ah, _val, _reg)
 
 /**
  * ath_hw_set_bssid_mask - filter out bssids we listen
@@ -119,8 +119,8 @@ void ath_hw_setbssidmask(struct ath_common *common)
 {
         void *ah = common->ah;
 
-        REG_WRITE(ah, get_unaligned_le32(common->bssidmask), AR_BSSMSKL);
-        REG_WRITE(ah, get_unaligned_le16(common->bssidmask + 4), AR_BSSMSKU);
+        REG_WRITE(ah, AR_BSSMSKL, get_unaligned_le32(common->bssidmask));
+        REG_WRITE(ah, AR_BSSMSKU, get_unaligned_le16(common->bssidmask + 4));
 }
 EXPORT_SYMBOL(ath_hw_setbssidmask);
 
@@ -139,7 +139,7 @@ void ath_hw_cycle_counters_update(struct ath_common *common)
         void *ah = common->ah;
 
         /* freeze */
-        REG_WRITE(ah, AR_MIBC_FMC, AR_MIBC);
+        REG_WRITE(ah, AR_MIBC, AR_MIBC_FMC);
 
         /* read */
         cycles = REG_READ(ah, AR_CCCNT);
@@ -148,13 +148,13 @@ void ath_hw_cycle_counters_update(struct ath_common *common)
         tx = REG_READ(ah, AR_TFCNT);
 
         /* clear */
-        REG_WRITE(ah, 0, AR_CCCNT);
-        REG_WRITE(ah, 0, AR_RFCNT);
-        REG_WRITE(ah, 0, AR_RCCNT);
-        REG_WRITE(ah, 0, AR_TFCNT);
+        REG_WRITE(ah, AR_CCCNT, 0);
+        REG_WRITE(ah, AR_RFCNT, 0);
+        REG_WRITE(ah, AR_RCCNT, 0);
+        REG_WRITE(ah, AR_TFCNT, 0);
 
         /* unfreeze */
-        REG_WRITE(ah, 0, AR_MIBC);
+        REG_WRITE(ah, AR_MIBC, 0);
 
         /* update all cycle counters here */
         common->cc_ani.cycles += cycles;
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 73730e94e0ac..ddd6a4f78097 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -4652,7 +4652,7 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
         switch (dev->dev->bus_type) {
 #ifdef CONFIG_B43_BCMA
         case B43_BUS_BCMA:
-                bcma_core_pci_irq_ctl(&dev->dev->bdev->bus->drv_pci,
+                bcma_core_pci_irq_ctl(&dev->dev->bdev->bus->drv_pci[0],
                                       dev->dev->bdev, true);
                 break;
 #endif
@@ -5404,6 +5404,8 @@ static void b43_bcma_remove(struct bcma_device *core)
         cancel_work_sync(&wldev->restart_work);
 
         B43_WARN_ON(!wl);
+        if (!wldev->fw.ucode.data)
+                return;                 /* NULL if firmware never loaded */
         if (wl->current_dev == wldev && wl->hw_registred) {
                 b43_leds_stop(wldev);
                 ieee80211_unregister_hw(wl->hw);
@@ -5478,6 +5480,8 @@ static void b43_ssb_remove(struct ssb_device *sdev)
         cancel_work_sync(&wldev->restart_work);
 
         B43_WARN_ON(!wl);
+        if (!wldev->fw.ucode.data)
+                return;                 /* NULL if firmware never loaded */
         if (wl->current_dev == wldev && wl->hw_registred) {
                 b43_leds_stop(wldev);
                 ieee80211_unregister_hw(wl->hw);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index 9d5170b6df50..fe80b637c519 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -24,6 +24,7 @@ ccflags-y += -D__CHECK_ENDIAN__
 obj-$(CONFIG_BRCMFMAC) += brcmfmac.o
 brcmfmac-objs += \
                 wl_cfg80211.o \
+                fwil.o \
                 dhd_cdc.o \
                 dhd_common.o \
                 dhd_linux.o
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
index 17e7ae73e008..8704daa2758f 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd.h
@@ -318,6 +318,12 @@ struct brcmf_event {
 #define BRCMF_E_LINK_ASSOC_REC                  3
 #define BRCMF_E_LINK_BSSCFG_DIS                 4
 
+/* Small, medium and maximum buffer size for dcmd
+ */
+#define BRCMF_DCMD_SMLEN        256
+#define BRCMF_DCMD_MEDLEN       1536
+#define BRCMF_DCMD_MAXLEN       8192
+
 /* Pattern matching filter. Specifies an offset within received packets to
  * start matching, the pattern to match, the size of the pattern, and a bitmask
  * that indicates which bits within the pattern should be matched.
@@ -623,7 +629,6 @@ struct brcmf_pub {
         u8 wme_dp;              /* wme discard priority */
 
         /* Dongle media info */
-        bool iswl;              /* Dongle-resident driver is wl */
         unsigned long drv_version;      /* Version of dongle-resident driver */
         u8 mac[ETH_ALEN];               /* MAC address obtained from dongle */
 
@@ -651,16 +656,10 @@ struct brcmf_pub {
         int in_suspend;         /* flag set to 1 when early suspend called */
         int dtim_skip;          /* dtim skip , default 0 means wake each dtim */
 
-        /* Pkt filter defination */
-        char *pktfilter[100];
-        int pktfilter_count;
-
-        u8 country_code[BRCM_CNTRY_BUF_SZ];
-        char eventmask[BRCMF_EVENTING_MASK_LEN];
-
         struct brcmf_if *iflist[BRCMF_MAX_IFS];
 
         struct mutex proto_block;
+        unsigned char proto_buf[BRCMF_DCMD_MAXLEN];
 
         struct work_struct setmacaddr_work;
         struct work_struct multicast_work;
@@ -671,6 +670,11 @@ struct brcmf_pub {
 #endif
 };
 
+struct bcmevent_name {
+        uint event;
+        const char *name;
+};
+
 struct brcmf_if_event {
         u8 ifidx;
         u8 action;
@@ -678,47 +682,60 @@ struct brcmf_if_event {
         u8 bssidx;
 };
 
-struct bcmevent_name {
-        uint event;
-        const char *name;
+/* forward declaration */
+struct brcmf_cfg80211_vif;
+
+/**
+ * struct brcmf_if - interface control information.
+ *
+ * @drvr: points to device related information.
+ * @vif: points to cfg80211 specific interface information.
+ * @ndev: associated network device.
+ * @stats: interface specific network statistics.
+ * @idx: interface index in device firmware.
+ * @bssidx: index of bss associated with this interface.
+ * @mac_addr: assigned mac address.
+ */
+struct brcmf_if {
+        struct brcmf_pub *drvr;
+        struct brcmf_cfg80211_vif *vif;
+        struct net_device *ndev;
+        struct net_device_stats stats;
+        int idx;
+        s32 bssidx;
+        u8 mac_addr[ETH_ALEN];
 };
 
+static inline s32 brcmf_ndev_bssidx(struct net_device *ndev)
+{
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        return ifp->bssidx;
+}
+
 extern const struct bcmevent_name bcmevent_names[];
 
 extern uint brcmf_c_mkiovar(char *name, char *data, uint datalen,
                           char *buf, uint len);
-extern uint brcmf_c_mkiovar_bsscfg(char *name, char *data, uint datalen,
-                                   char *buf, uint buflen, s32 bssidx);
 
 extern int brcmf_netdev_wait_pend8021x(struct net_device *ndev);
 
-extern s32 brcmf_exec_dcmd(struct net_device *dev, u32 cmd, void *arg, u32 len);
-extern int brcmf_netlink_dcmd(struct net_device *ndev, struct brcmf_dcmd *dcmd);
-
 /* Return pointer to interface name */
 extern char *brcmf_ifname(struct brcmf_pub *drvr, int idx);
 
 /* Query dongle */
 extern int brcmf_proto_cdc_query_dcmd(struct brcmf_pub *drvr, int ifidx,
                                        uint cmd, void *buf, uint len);
-
-#ifdef DEBUG
-extern int brcmf_write_to_file(struct brcmf_pub *drvr, const u8 *buf, int size);
-#endif                          /* DEBUG */
+extern int brcmf_proto_cdc_set_dcmd(struct brcmf_pub *drvr, int ifidx, uint cmd,
+                                    void *buf, uint len);
 
 extern int brcmf_ifname2idx(struct brcmf_pub *drvr, char *name);
 extern int brcmf_c_host_event(struct brcmf_pub *drvr, int *idx,
                               void *pktdata, struct brcmf_event_msg *,
                               void **data_ptr);
 
+extern int brcmf_net_attach(struct brcmf_if *ifp);
+extern struct brcmf_if *brcmf_add_if(struct device *dev, int ifidx, s32 bssidx,
+                                     char *name, u8 *mac_addr);
 extern void brcmf_del_if(struct brcmf_pub *drvr, int ifidx);
 
-extern void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg);
-extern void brcmf_c_pktfilter_offload_enable(struct brcmf_pub *drvr, char *arg,
-                                             int enable, int master_mode);
-
-#define BRCMF_DCMD_SMLEN        256     /* "small" cmd buffer required */
-#define BRCMF_DCMD_MEDLEN       1536    /* "med" cmd buffer required */
-#define BRCMF_DCMD_MAXLEN       8192    /* max length cmd buffer required */
-
 #endif                          /* _BRCMF_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
index 9b8ee19ea55d..265580f5b270 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
@@ -111,9 +111,6 @@ extern void brcmf_txcomplete(struct device *dev, struct sk_buff *txp,
 
 extern int brcmf_bus_start(struct device *dev);
 
-extern int brcmf_add_if(struct device *dev, int ifidx,
-                        char *name, u8 *mac_addr);
-
 #ifdef CONFIG_BRCMFMAC_SDIO
 extern void brcmf_sdio_exit(void);
 extern void brcmf_sdio_init(void);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
index a5c15cac5e7d..b9d8a5adfd43 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_cdc.c
@@ -458,35 +458,6 @@ void brcmf_proto_detach(struct brcmf_pub *drvr)
         drvr->prot = NULL;
 }
 
-int brcmf_proto_init(struct brcmf_pub *drvr)
-{
-        int ret = 0;
-        char buf[128];
-
-        brcmf_dbg(TRACE, "Enter\n");
-
-        mutex_lock(&drvr->proto_block);
-
-        /* Get the device MAC address */
-        strcpy(buf, "cur_etheraddr");
-        ret = brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR,
-                                          buf, sizeof(buf));
-        if (ret < 0) {
-                mutex_unlock(&drvr->proto_block);
-                return ret;
-        }
-        memcpy(drvr->mac, buf, ETH_ALEN);
-
-        mutex_unlock(&drvr->proto_block);
-
-        ret = brcmf_c_preinit_dcmds(drvr);
-
-        /* Always assumes wl for now */
-        drvr->iswl = true;
-
-        return ret;
-}
-
 void brcmf_proto_stop(struct brcmf_pub *drvr)
 {
         /* Nothing to do for CDC */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
index 15c5db5752d1..866b66995bb0 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_common.c
@@ -28,12 +28,17 @@
 #include "dhd_bus.h"
 #include "dhd_proto.h"
 #include "dhd_dbg.h"
+#include "fwil.h"
 
 #define BRCM_OUI                        "\x00\x10\x18"
 #define DOT11_OUI_LEN                   3
 #define BCMILCP_BCM_SUBTYPE_EVENT       1
-#define PKTFILTER_BUF_SIZE              2048
+#define PKTFILTER_BUF_SIZE              128
 #define BRCMF_ARPOL_MODE                0xb     /* agent|snoop|peer_autoreply */
+#define BRCMF_DEFAULT_BCN_TIMEOUT       3
+#define BRCMF_DEFAULT_SCAN_CHANNEL_TIME 40
+#define BRCMF_DEFAULT_SCAN_UNASSOC_TIME 40
+#define BRCMF_DEFAULT_PACKET_FILTER     "100 0 0 0 0x01 0x00"
 
 #define MSGTRACE_VERSION        1
 
@@ -88,52 +93,6 @@ brcmf_c_mkiovar(char *name, char *data, uint datalen, char *buf, uint buflen)
         return len;
 }
 
-uint
-brcmf_c_mkiovar_bsscfg(char *name, char *data, uint datalen,
-                       char *buf, uint buflen, s32 bssidx)
-{
-        const s8 *prefix = "bsscfg:";
-        s8 *p;
-        u32 prefixlen;
-        u32 namelen;
-        u32 iolen;
-        __le32 bssidx_le;
-
-        if (bssidx == 0)
-                return brcmf_c_mkiovar(name, data, datalen, buf, buflen);
-
-        prefixlen = (u32) strlen(prefix); /* lengh of bsscfg prefix */
-        namelen = (u32) strlen(name) + 1; /* lengh of iovar  name + null */
-        iolen = prefixlen + namelen + sizeof(bssidx_le) + datalen;
-
-        if (buflen < 0 || iolen > (u32)buflen) {
-                brcmf_dbg(ERROR, "buffer is too short\n");
-                return 0;
-        }
-
-        p = buf;
-
-        /* copy prefix, no null */
-        memcpy(p, prefix, prefixlen);
-        p += prefixlen;
-
-        /* copy iovar name including null */
-        memcpy(p, name, namelen);
-        p += namelen;
-
-        /* bss config index as first data */
-        bssidx_le = cpu_to_le32(bssidx);
-        memcpy(p, &bssidx_le, sizeof(bssidx_le));
-        p += sizeof(bssidx_le);
-
-        /* parameter buffer follows */
-        if (datalen)
-                memcpy(p, data, datalen);
-
-        return iolen;
-
-}
-
 bool brcmf_c_prec_enq(struct device *dev, struct pktq *q,
                       struct sk_buff *pkt, int prec)
 {
@@ -490,6 +449,7 @@ brcmf_c_host_event(struct brcmf_pub *drvr, int *ifidx, void *pktdata,
         /* check whether packet is a BRCM event pkt */
         struct brcmf_event *pvt_data = (struct brcmf_event *) pktdata;
         struct brcmf_if_event *ifevent;
+        struct brcmf_if *ifp;
         char *event_data;
         u32 type, status;
         u16 flags;
@@ -525,12 +485,17 @@ brcmf_c_host_event(struct brcmf_pub *drvr, int *ifidx, void *pktdata,
                 brcmf_dbg(TRACE, "if event\n");
 
                 if (ifevent->ifidx > 0 && ifevent->ifidx < BRCMF_MAX_IFS) {
-                        if (ifevent->action == BRCMF_E_IF_ADD)
-                                brcmf_add_if(drvr->dev, ifevent->ifidx,
-                                             event->ifname,
-                                             pvt_data->eth.h_dest);
-                        else
+                        if (ifevent->action == BRCMF_E_IF_ADD) {
+                                ifp = brcmf_add_if(drvr->dev, ifevent->ifidx,
+                                                   ifevent->bssidx,
+                                                   event->ifname,
+                                                   pvt_data->eth.h_dest);
+                                if (IS_ERR(ifp))
+                                        return PTR_ERR(ifp);
+                                brcmf_net_attach(ifp);
+                        } else {
                                 brcmf_del_if(drvr, ifevent->ifidx);
+                        }
                 } else {
                         brcmf_dbg(ERROR, "Invalid ifidx %d for %s\n",
                                   ifevent->ifidx, event->ifname);
@@ -603,90 +568,57 @@ static int brcmf_c_pattern_atoh(char *src, char *dst)
         return i;
 }
 
-void
-brcmf_c_pktfilter_offload_enable(struct brcmf_pub *drvr, char *arg, int enable,
-                             int master_mode)
+static void
+brcmf_c_pktfilter_offload_enable(struct brcmf_if *ifp, char *arg, int enable,
+                                 int master_mode)
 {
         unsigned long res;
-        char *argv[8];
-        int i = 0;
-        const char *str;
-        int buf_len;
-        int str_len;
+        char *argv;
         char *arg_save = NULL, *arg_org = NULL;
-        int rc;
-        char buf[128];
+        s32 err;
         struct brcmf_pkt_filter_enable_le enable_parm;
-        struct brcmf_pkt_filter_enable_le *pkt_filterp;
-        __le32 mmode_le;
 
-        arg_save = kmalloc(strlen(arg) + 1, GFP_ATOMIC);
+        arg_save = kstrdup(arg, GFP_ATOMIC);
         if (!arg_save)
                 goto fail;
 
         arg_org = arg_save;
-        memcpy(arg_save, arg, strlen(arg) + 1);
 
-        argv[i] = strsep(&arg_save, " ");
+        argv = strsep(&arg_save, " ");
 
-        i = 0;
-        if (NULL == argv[i]) {
+        if (argv == NULL) {
                 brcmf_dbg(ERROR, "No args provided\n");
                 goto fail;
         }
 
-        str = "pkt_filter_enable";
-        str_len = strlen(str);
-        strncpy(buf, str, str_len);
-        buf[str_len] = '\0';
-        buf_len = str_len + 1;
-
-        pkt_filterp = (struct brcmf_pkt_filter_enable_le *) (buf + str_len + 1);
-
         /* Parse packet filter id. */
         enable_parm.id = 0;
-        if (!kstrtoul(argv[i], 0, &res))
+        if (!kstrtoul(argv, 0, &res))
                 enable_parm.id = cpu_to_le32((u32)res);
 
-        /* Parse enable/disable value. */
+        /* Enable/disable the specified filter. */
         enable_parm.enable = cpu_to_le32(enable);
 
-        buf_len += sizeof(enable_parm);
-        memcpy((char *)pkt_filterp, &enable_parm, sizeof(enable_parm));
+        err = brcmf_fil_iovar_data_set(ifp, "pkt_filter_enable", &enable_parm,
+                                       sizeof(enable_parm));
+        if (err)
+                brcmf_dbg(ERROR, "Set pkt_filter_enable error (%d)\n", err);
 
-        /* Enable/disable the specified filter. */
-        rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
-        rc = rc >= 0 ? 0 : rc;
-        if (rc)
-                brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
-                          arg, rc);
-        else
-                brcmf_dbg(TRACE, "successfully added pktfilter %s\n", arg);
-
-        /* Contorl the master mode */
-        mmode_le = cpu_to_le32(master_mode);
-        brcmf_c_mkiovar("pkt_filter_mode", (char *)&mmode_le, 4, buf,
-                    sizeof(buf));
-        rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf,
-                                       sizeof(buf));
-        rc = rc >= 0 ? 0 : rc;
-        if (rc)
-                brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
-                          arg, rc);
+        /* Control the master mode */
+        err = brcmf_fil_iovar_int_set(ifp, "pkt_filter_mode", master_mode);
+        if (err)
+                brcmf_dbg(ERROR, "Set pkt_filter_mode error (%d)\n", err);
 
 fail:
         kfree(arg_org);
 }
 
-void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
+static void brcmf_c_pktfilter_offload_set(struct brcmf_if *ifp, char *arg)
 {
-        const char *str;
-        struct brcmf_pkt_filter_le pkt_filter;
-        struct brcmf_pkt_filter_le *pkt_filterp;
+        struct brcmf_pkt_filter_le *pkt_filter;
         unsigned long res;
         int buf_len;
-        int str_len;
-        int rc;
+        s32 err;
         u32 mask_size;
         u32 pattern_size;
         char *argv[8], *buf = NULL;
@@ -704,104 +636,64 @@ void brcmf_c_pktfilter_offload_set(struct brcmf_pub *drvr, char *arg)
                 goto fail;
 
         argv[i] = strsep(&arg_save, " ");
-        while (argv[i++])
+        while (argv[i]) {
+                i++;
+                if (i >= 8) {
+                        brcmf_dbg(ERROR, "Too many parameters\n");
+                        goto fail;
+                }
                 argv[i] = strsep(&arg_save, " ");
+        }
 
-        i = 0;
-        if (NULL == argv[i]) {
-                brcmf_dbg(ERROR, "No args provided\n");
+        if (i != 6) {
+                brcmf_dbg(ERROR, "Not enough args provided %d\n", i);
                 goto fail;
         }
 
-        str = "pkt_filter_add";
-        strcpy(buf, str);
-        str_len = strlen(str);
-        buf_len = str_len + 1;
-
-        pkt_filterp = (struct brcmf_pkt_filter_le *) (buf + str_len + 1);
+        pkt_filter = (struct brcmf_pkt_filter_le *)buf;
 
         /* Parse packet filter id. */
-        pkt_filter.id = 0;
-        if (!kstrtoul(argv[i], 0, &res))
-                pkt_filter.id = cpu_to_le32((u32)res);
-
-        if (NULL == argv[++i]) {
-                brcmf_dbg(ERROR, "Polarity not provided\n");
-                goto fail;
-        }
+        pkt_filter->id = 0;
+        if (!kstrtoul(argv[0], 0, &res))
+                pkt_filter->id = cpu_to_le32((u32)res);
 
         /* Parse filter polarity. */
-        pkt_filter.negate_match = 0;
-        if (!kstrtoul(argv[i], 0, &res))
-                pkt_filter.negate_match = cpu_to_le32((u32)res);
-
-        if (NULL == argv[++i]) {
-                brcmf_dbg(ERROR, "Filter type not provided\n");
-                goto fail;
-        }
+        pkt_filter->negate_match = 0;
+        if (!kstrtoul(argv[1], 0, &res))
+                pkt_filter->negate_match = cpu_to_le32((u32)res);
 
         /* Parse filter type. */
-        pkt_filter.type = 0;
-        if (!kstrtoul(argv[i], 0, &res))
-                pkt_filter.type = cpu_to_le32((u32)res);
-
-        if (NULL == argv[++i]) {
-                brcmf_dbg(ERROR, "Offset not provided\n");
-                goto fail;
-        }
+        pkt_filter->type = 0;
+        if (!kstrtoul(argv[2], 0, &res))
+                pkt_filter->type = cpu_to_le32((u32)res);
 
         /* Parse pattern filter offset. */
-        pkt_filter.u.pattern.offset = 0;
-        if (!kstrtoul(argv[i], 0, &res))
-                pkt_filter.u.pattern.offset = cpu_to_le32((u32)res);
-
-        if (NULL == argv[++i]) {
-                brcmf_dbg(ERROR, "Bitmask not provided\n");
-                goto fail;
-        }
+        pkt_filter->u.pattern.offset = 0;
+        if (!kstrtoul(argv[3], 0, &res))
+                pkt_filter->u.pattern.offset = cpu_to_le32((u32)res);
 
         /* Parse pattern filter mask. */
-        mask_size =
-            brcmf_c_pattern_atoh
-                   (argv[i], (char *)pkt_filterp->u.pattern.mask_and_pattern);
-
-        if (NULL == argv[++i]) {
-                brcmf_dbg(ERROR, "Pattern not provided\n");
-                goto fail;
-        }
+        mask_size = brcmf_c_pattern_atoh(argv[4],
+                        (char *)pkt_filter->u.pattern.mask_and_pattern);
 
         /* Parse pattern filter pattern. */
-        pattern_size =
-            brcmf_c_pattern_atoh(argv[i],
-                                   (char *)&pkt_filterp->u.pattern.
-                                   mask_and_pattern[mask_size]);
+        pattern_size = brcmf_c_pattern_atoh(argv[5],
+                (char *)&pkt_filter->u.pattern.mask_and_pattern[mask_size]);
 
         if (mask_size != pattern_size) {
                 brcmf_dbg(ERROR, "Mask and pattern not the same size\n");
                 goto fail;
         }
 
-        pkt_filter.u.pattern.size_bytes = cpu_to_le32(mask_size);
-        buf_len += BRCMF_PKT_FILTER_FIXED_LEN;
-        buf_len += (BRCMF_PKT_FILTER_PATTERN_FIXED_LEN + 2 * mask_size);
-
-        /* Keep-alive attributes are set in local
-         * variable (keep_alive_pkt), and
-         ** then memcpy'ed into buffer (keep_alive_pktp) since there is no
-         ** guarantee that the buffer is properly aligned.
-         */
-        memcpy((char *)pkt_filterp,
-               &pkt_filter,
-               BRCMF_PKT_FILTER_FIXED_LEN + BRCMF_PKT_FILTER_PATTERN_FIXED_LEN);
-
-        rc = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, buf, buf_len);
-        rc = rc >= 0 ? 0 : rc;
+        pkt_filter->u.pattern.size_bytes = cpu_to_le32(mask_size);
+        buf_len = sizeof(*pkt_filter);
+        buf_len -= sizeof(pkt_filter->u.pattern.mask_and_pattern);
+        buf_len += mask_size + pattern_size;
 
-        if (rc)
-                brcmf_dbg(TRACE, "failed to add pktfilter %s, retcode = %d\n",
-                          arg, rc);
-        else
-                brcmf_dbg(TRACE, "successfully added pktfilter %s\n", arg);
+        err = brcmf_fil_iovar_data_set(ifp, "pkt_filter_add", pkt_filter,
+                                       buf_len);
+        if (err)
+                brcmf_dbg(ERROR, "Set pkt_filter_add error (%d)\n", err);
 
 fail:
         kfree(arg_org);
@@ -809,130 +701,125 @@ fail:
         kfree(buf);
 }
 
-static void brcmf_c_arp_offload_set(struct brcmf_pub *drvr, int arp_mode)
+int brcmf_c_preinit_dcmds(struct brcmf_if *ifp)
 {
-        char iovbuf[32];
-        int retcode;
-        __le32 arp_mode_le;
-
-        arp_mode_le = cpu_to_le32(arp_mode);
-        brcmf_c_mkiovar("arp_ol", (char *)&arp_mode_le, 4, iovbuf,
-                        sizeof(iovbuf));
-        retcode = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
-                                   iovbuf, sizeof(iovbuf));
-        retcode = retcode >= 0 ? 0 : retcode;
-        if (retcode)
-                brcmf_dbg(TRACE, "failed to set ARP offload mode to 0x%x, retcode = %d\n",
-                          arp_mode, retcode);
-        else
-                brcmf_dbg(TRACE, "successfully set ARP offload mode to 0x%x\n",
-                          arp_mode);
-}
-
-static void brcmf_c_arp_offload_enable(struct brcmf_pub *drvr, int arp_enable)
-{
-        char iovbuf[32];
-        int retcode;
-        __le32 arp_enable_le;
-
-        arp_enable_le = cpu_to_le32(arp_enable);
-
-        brcmf_c_mkiovar("arpoe", (char *)&arp_enable_le, 4,
-                        iovbuf, sizeof(iovbuf));
-        retcode = brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR,
-                                   iovbuf, sizeof(iovbuf));
-        retcode = retcode >= 0 ? 0 : retcode;
-        if (retcode)
-                brcmf_dbg(TRACE, "failed to enable ARP offload to %d, retcode = %d\n",
-                          arp_enable, retcode);
-        else
-                brcmf_dbg(TRACE, "successfully enabled ARP offload to %d\n",
-                          arp_enable);
-}
-
-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;
-        __le32 roaming_le = cpu_to_le32(1);
-        __le32 bcn_timeout_le = cpu_to_le32(3);
-        __le32 scan_assoc_time_le = cpu_to_le32(40);
-        __le32 scan_unassoc_time_le = cpu_to_le32(40);
-        int i;
+        s8 eventmask[BRCMF_EVENTING_MASK_LEN];
+        u8 buf[BRCMF_DCMD_SMLEN];
+        char *ptr;
+        s32 err;
         struct brcmf_bus_dcmd *cmdlst;
         struct list_head *cur, *q;
 
-        mutex_lock(&drvr->proto_block);
-
-        /* Set Country code */
-        if (drvr->country_code[0] != 0) {
-                if (brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_COUNTRY,
-                                              drvr->country_code,
-                                              sizeof(drvr->country_code)) < 0)
-                        brcmf_dbg(ERROR, "country code setting failed\n");
+        /* retreive mac address */
+        err = brcmf_fil_iovar_data_get(ifp, "cur_etheraddr", ifp->mac_addr,
+                                       sizeof(ifp->mac_addr));
+        if (err < 0) {
+                brcmf_dbg(ERROR, "Retreiving cur_etheraddr failed, %d\n",
+                          err);
+                goto done;
         }
+        memcpy(ifp->drvr->mac, ifp->mac_addr, sizeof(ifp->drvr->mac));
 
         /* query for 'ver' to get version info from firmware */
         memset(buf, 0, sizeof(buf));
-        ptr = buf;
-        brcmf_c_mkiovar("ver", NULL, 0, buf, sizeof(buf));
-        brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR, buf, sizeof(buf));
+        strcpy(buf, "ver");
+        err = brcmf_fil_iovar_data_get(ifp, "ver", buf, sizeof(buf));
+        if (err < 0) {
+                brcmf_dbg(ERROR, "Retreiving version information failed, %d\n",
+                          err);
+                goto done;
+        }
+        ptr = (char *)buf;
         strsep(&ptr, "\n");
         /* Print fw version info */
         brcmf_dbg(ERROR, "Firmware version = %s\n", buf);
 
-        /* Setup timeout if Beacons are lost and roam is off to report
-                 link down */
-        brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_timeout_le, 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
+         */
+        err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout",
+                                      BRCMF_DEFAULT_BCN_TIMEOUT);
+        if (err) {
+                brcmf_dbg(ERROR, "bcn_timeout error (%d)\n", err);
+                goto done;
+        }
 
         /* Enable/Disable build-in roaming to allowed ext supplicant to take
-                 of romaing */
-        brcmf_c_mkiovar("roam_off", (char *)&roaming_le, 4,
-                      iovbuf, sizeof(iovbuf));
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-                                  sizeof(iovbuf));
-
-        /* Setup event_msgs */
-        brcmf_c_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
-                      iovbuf, sizeof(iovbuf));
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_VAR, iovbuf,
-                                  sizeof(iovbuf));
-
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_CHANNEL_TIME,
-                 (char *)&scan_assoc_time_le, sizeof(scan_assoc_time_le));
-        brcmf_proto_cdc_set_dcmd(drvr, 0, BRCMF_C_SET_SCAN_UNASSOC_TIME,
-                 (char *)&scan_unassoc_time_le, sizeof(scan_unassoc_time_le));
-
-        /* Set and enable ARP offload feature */
-        brcmf_c_arp_offload_set(drvr, BRCMF_ARPOL_MODE);
-        brcmf_c_arp_offload_enable(drvr, true);
-
-        /* Set up pkt filter */
-        for (i = 0; i < drvr->pktfilter_count; i++) {
-                brcmf_c_pktfilter_offload_set(drvr, drvr->pktfilter[i]);
-                brcmf_c_pktfilter_offload_enable(drvr, drvr->pktfilter[i],
-                                                 0, true);
+         * of romaing
+         */
+        err = brcmf_fil_iovar_int_set(ifp, "roam_off", 1);
+        if (err) {
+                brcmf_dbg(ERROR, "roam_off error (%d)\n", err);
+                goto done;
+        }
+
+        /* Setup event_msgs, enable E_IF */
+        err = brcmf_fil_iovar_data_get(ifp, "event_msgs", eventmask,
+                                       BRCMF_EVENTING_MASK_LEN);
+        if (err) {
+                brcmf_dbg(ERROR, "Get event_msgs error (%d)\n", err);
+                goto done;
+        }
+        setbit(eventmask, BRCMF_E_IF);
+        err = brcmf_fil_iovar_data_set(ifp, "event_msgs", eventmask,
+                                       BRCMF_EVENTING_MASK_LEN);
+        if (err) {
+                brcmf_dbg(ERROR, "Set event_msgs error (%d)\n", err);
+                goto done;
+        }
+
+        /* Setup default scan channel time */
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_CHANNEL_TIME,
+                                    BRCMF_DEFAULT_SCAN_CHANNEL_TIME);
+        if (err) {
+                brcmf_dbg(ERROR, "BRCMF_C_SET_SCAN_CHANNEL_TIME error (%d)\n",
+                          err);
+                goto done;
         }
 
+        /* Setup default scan unassoc time */
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_UNASSOC_TIME,
+                                    BRCMF_DEFAULT_SCAN_UNASSOC_TIME);
+        if (err) {
+                brcmf_dbg(ERROR, "BRCMF_C_SET_SCAN_UNASSOC_TIME error (%d)\n",
+                          err);
+                goto done;
+        }
+
+        /* Try to set and enable ARP offload feature, this may fail */
+        err = brcmf_fil_iovar_int_set(ifp, "arp_ol", BRCMF_ARPOL_MODE);
+        if (err) {
+                brcmf_dbg(TRACE, "failed to set ARP offload mode to 0x%x, err = %d\n",
+                          BRCMF_ARPOL_MODE, err);
+                err = 0;
+        } else {
+                err = brcmf_fil_iovar_int_set(ifp, "arpoe", 1);
+                if (err) {
+                        brcmf_dbg(TRACE, "failed to enable ARP offload err = %d\n",
+                                  err);
+                        err = 0;
+                } else
+                        brcmf_dbg(TRACE, "successfully enabled ARP offload to 0x%x\n",
+                                  BRCMF_ARPOL_MODE);
+        }
+
+        /* Setup packet filter */
+        brcmf_c_pktfilter_offload_set(ifp, BRCMF_DEFAULT_PACKET_FILTER);
+        brcmf_c_pktfilter_offload_enable(ifp, BRCMF_DEFAULT_PACKET_FILTER,
+                                         0, true);
+
         /* set bus specific command if there is any */
-        list_for_each_safe(cur, q, &drvr->bus_if->dcmd_list) {
+        list_for_each_safe(cur, q, &ifp->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));
+                        brcmf_fil_iovar_data_set(ifp, cmdlst->name,
+                                                 cmdlst->param,
+                                                 cmdlst->param_len);
                 }
                 list_del(cur);
                 kfree(cmdlst);
         }
-
-        mutex_unlock(&drvr->proto_block);
-
-        return 0;
+done:
+        return err;
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
index 7f89540b56da..862d2acb7a16 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.c
@@ -16,8 +16,10 @@
 #include <linux/debugfs.h>
 #include <linux/if_ether.h>
 #include <linux/if.h>
+#include <linux/netdevice.h>
 #include <linux/ieee80211.h>
 #include <linux/module.h>
+#include <linux/netdevice.h>
 
 #include <defs.h>
 #include <brcmu_wifi.h>
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
index fb508c2256dd..eefa6c2560cc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_dbg.h
@@ -31,6 +31,7 @@
 #define BRCMF_EVENT_VAL 0x0800
 #define BRCMF_BTA_VAL   0x1000
 #define BRCMF_ISCAN_VAL 0x2000
+#define BRCMF_FIL_VAL   0x4000
 
 #if defined(DEBUG)
 
@@ -56,6 +57,7 @@ do {									\
 #define BRCMF_BYTES_ON()        (brcmf_msg_level & BRCMF_BYTES_VAL)
 #define BRCMF_GLOM_ON()         (brcmf_msg_level & BRCMF_GLOM_VAL)
 #define BRCMF_EVENT_ON()        (brcmf_msg_level & BRCMF_EVENT_VAL)
+#define BRCMF_FIL_ON()          (brcmf_msg_level & BRCMF_FIL_VAL)
 
 #else   /* (defined DEBUG) || (defined DEBUG) */
 
@@ -67,6 +69,7 @@ do {									\
 #define BRCMF_BYTES_ON()        0
 #define BRCMF_GLOM_ON()         0
 #define BRCMF_EVENT_ON()        0
+#define BRCMF_FIL_ON()          0
 
 #endif                          /* defined(DEBUG) */
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
index d7c76ce9d8cb..297652339fda 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_linux.c
@@ -52,16 +52,6 @@ MODULE_SUPPORTED_DEVICE("Broadcom 802.11n WLAN fullmac cards");
 MODULE_LICENSE("Dual BSD/GPL");
 
 
-/* Interface control information */
-struct brcmf_if {
-        struct brcmf_pub *drvr; /* back pointer to brcmf_pub */
-        /* OS/stack specifics */
-        struct net_device *ndev;
-        struct net_device_stats stats;
-        int idx;                /* iface idx in dongle */
-        u8 mac_addr[ETH_ALEN];  /* assigned MAC address */
-};
-
 /* Error bits */
 int brcmf_msg_level = BRCMF_ERROR_VAL;
 module_param(brcmf_msg_level, int, 0);
@@ -629,12 +619,9 @@ static int brcmf_ethtool(struct brcmf_pub *drvr, void __user *uaddr)
                         brcmf_dbg(ERROR, "dongle is not up\n");
                         return -ENODEV;
                 }
-
                 /* finally, report dongle driver type */
-                else if (drvr->iswl)
-                        sprintf(info.driver, "wl");
                 else
-                        sprintf(info.driver, "xx");
+                        sprintf(info.driver, "wl");
 
                 sprintf(info.version, "%lu", drvr->drv_version);
                 if (copy_to_user(uaddr, &info, sizeof(info)))
@@ -719,65 +706,6 @@ static int brcmf_netdev_ioctl_entry(struct net_device *ndev, struct ifreq *ifr,
         return -EOPNOTSUPP;
 }
 
-/* called only from within this driver. Sends a command to the dongle. */
-s32 brcmf_exec_dcmd(struct net_device *ndev, u32 cmd, void *arg, u32 len)
-{
-        struct brcmf_dcmd dcmd;
-        s32 err = 0;
-        int buflen = 0;
-        bool is_set_key_cmd;
-        struct brcmf_if *ifp = netdev_priv(ndev);
-        struct brcmf_pub *drvr = ifp->drvr;
-
-        memset(&dcmd, 0, sizeof(dcmd));
-        dcmd.cmd = cmd;
-        dcmd.buf = arg;
-        dcmd.len = len;
-
-        if (dcmd.buf != NULL)
-                buflen = min_t(uint, dcmd.len, BRCMF_DCMD_MAXLEN);
-
-        /* send to dongle (must be up, and wl) */
-        if ((drvr->bus_if->state != BRCMF_BUS_DATA)) {
-                brcmf_dbg(ERROR, "DONGLE_DOWN\n");
-                err = -EIO;
-                goto done;
-        }
-
-        if (!drvr->iswl) {
-                err = -EIO;
-                goto done;
-        }
-
-        /*
-         * Intercept BRCMF_C_SET_KEY CMD - serialize M4 send and
-         * set key CMD to prevent M4 encryption.
-         */
-        is_set_key_cmd = ((dcmd.cmd == BRCMF_C_SET_KEY) ||
-                          ((dcmd.cmd == BRCMF_C_SET_VAR) &&
-                           !(strncmp("wsec_key", dcmd.buf, 9))) ||
-                          ((dcmd.cmd == BRCMF_C_SET_VAR) &&
-                           !(strncmp("bsscfg:wsec_key", dcmd.buf, 15))));
-        if (is_set_key_cmd)
-                brcmf_netdev_wait_pend8021x(ndev);
-
-        err = brcmf_proto_dcmd(drvr, ifp->idx, &dcmd, buflen);
-
-done:
-        if (err > 0)
-                err = 0;
-
-        return err;
-}
-
-int brcmf_netlink_dcmd(struct net_device *ndev, struct brcmf_dcmd *dcmd)
-{
-        brcmf_dbg(TRACE, "enter: cmd %x buf %p len %d\n",
-                  dcmd->cmd, dcmd->buf, dcmd->len);
-
-        return brcmf_exec_dcmd(ndev, dcmd->cmd, dcmd->buf, dcmd->len);
-}
-
 static int brcmf_netdev_stop(struct net_device *ndev)
 {
         struct brcmf_if *ifp = netdev_priv(ndev);
@@ -851,7 +779,7 @@ static const struct net_device_ops brcmf_netdev_ops_pri = {
         .ndo_set_rx_mode = brcmf_netdev_set_multicast_list
 };
 
-static int brcmf_net_attach(struct brcmf_if *ifp)
+int brcmf_net_attach(struct brcmf_if *ifp)
 {
         struct brcmf_pub *drvr = ifp->drvr;
         struct net_device *ndev;
@@ -885,15 +813,6 @@ static int brcmf_net_attach(struct brcmf_if *ifp)
 
         memcpy(ndev->dev_addr, temp_addr, ETH_ALEN);
 
-        /* attach to cfg80211 for primary interface */
-        if (!ifp->idx) {
-                drvr->config = brcmf_cfg80211_attach(ndev, drvr->dev, drvr);
-                if (drvr->config == NULL) {
-                        brcmf_dbg(ERROR, "wl_cfg80211_attach failed\n");
-                        goto fail;
-                }
-        }
-
         if (register_netdev(ndev) != 0) {
                 brcmf_dbg(ERROR, "couldn't register the net device\n");
                 goto fail;
@@ -905,11 +824,12 @@ static int brcmf_net_attach(struct brcmf_if *ifp)
 
 fail:
         ndev->netdev_ops = NULL;
+        free_netdev(ndev);
         return -EBADE;
 }
 
-int
-brcmf_add_if(struct device *dev, int ifidx, char *name, u8 *mac_addr)
+struct brcmf_if *brcmf_add_if(struct device *dev, int ifidx, s32 bssidx,
+                              char *name, u8 *mac_addr)
 {
         struct brcmf_if *ifp;
         struct net_device *ndev;
@@ -936,7 +856,7 @@ brcmf_add_if(struct device *dev, int ifidx, char *name, u8 *mac_addr)
         ndev = alloc_netdev(sizeof(struct brcmf_if), name, ether_setup);
         if (!ndev) {
                 brcmf_dbg(ERROR, "OOM - alloc_netdev\n");
-                return -ENOMEM;
+                return ERR_PTR(-ENOMEM);
         }
 
         ifp = netdev_priv(ndev);
@@ -944,20 +864,14 @@ brcmf_add_if(struct device *dev, int ifidx, char *name, u8 *mac_addr)
         ifp->drvr = drvr;
         drvr->iflist[ifidx] = ifp;
         ifp->idx = ifidx;
+        ifp->bssidx = bssidx;
         if (mac_addr != NULL)
                 memcpy(&ifp->mac_addr, mac_addr, ETH_ALEN);
 
-        if (brcmf_net_attach(ifp)) {
-                brcmf_dbg(ERROR, "brcmf_net_attach failed");
-                free_netdev(ifp->ndev);
-                drvr->iflist[ifidx] = NULL;
-                return -EOPNOTSUPP;
-        }
-
         brcmf_dbg(TRACE, " ==== pid:%x, net_device for if:%s created ===\n",
                   current->pid, ifp->ndev->name);
 
-        return 0;
+        return ifp;
 }
 
 void brcmf_del_if(struct brcmf_pub *drvr, int ifidx)
@@ -1036,10 +950,9 @@ fail:
 int brcmf_bus_start(struct device *dev)
 {
         int ret = -1;
-        /* Room for "event_msgs" + '\0' + bitvec */
-        char iovbuf[BRCMF_EVENTING_MASK_LEN + 12];
         struct brcmf_bus *bus_if = dev_get_drvdata(dev);
         struct brcmf_pub *drvr = bus_if->drvr;
+        struct brcmf_if *ifp;
 
         brcmf_dbg(TRACE, "\n");
 
@@ -1050,49 +963,30 @@ int brcmf_bus_start(struct device *dev)
                 return ret;
         }
 
-        brcmf_c_mkiovar("event_msgs", drvr->eventmask, BRCMF_EVENTING_MASK_LEN,
-                      iovbuf, sizeof(iovbuf));
-        brcmf_proto_cdc_query_dcmd(drvr, 0, BRCMF_C_GET_VAR, iovbuf,
-                                    sizeof(iovbuf));
-        memcpy(drvr->eventmask, iovbuf, BRCMF_EVENTING_MASK_LEN);
-
-        setbit(drvr->eventmask, BRCMF_E_SET_SSID);
-        setbit(drvr->eventmask, BRCMF_E_PRUNE);
-        setbit(drvr->eventmask, BRCMF_E_AUTH);
-        setbit(drvr->eventmask, BRCMF_E_REASSOC);
-        setbit(drvr->eventmask, BRCMF_E_REASSOC_IND);
-        setbit(drvr->eventmask, BRCMF_E_DEAUTH_IND);
-        setbit(drvr->eventmask, BRCMF_E_DISASSOC_IND);
-        setbit(drvr->eventmask, BRCMF_E_DISASSOC);
-        setbit(drvr->eventmask, BRCMF_E_JOIN);
-        setbit(drvr->eventmask, BRCMF_E_ASSOC_IND);
-        setbit(drvr->eventmask, BRCMF_E_PSK_SUP);
-        setbit(drvr->eventmask, BRCMF_E_LINK);
-        setbit(drvr->eventmask, BRCMF_E_NDIS_LINK);
-        setbit(drvr->eventmask, BRCMF_E_MIC_ERROR);
-        setbit(drvr->eventmask, BRCMF_E_PMKID_CACHE);
-        setbit(drvr->eventmask, BRCMF_E_TXFAIL);
-        setbit(drvr->eventmask, BRCMF_E_JOIN_START);
-        setbit(drvr->eventmask, BRCMF_E_SCAN_COMPLETE);
-
-/* enable dongle roaming event */
-
-        drvr->pktfilter_count = 1;
-        /* Setup filter to allow only unicast */
-        drvr->pktfilter[0] = "100 0 0 0 0x01 0x00";
-
-        /* Bus is ready, do any protocol initialization */
-        ret = brcmf_proto_init(drvr);
+        /* add primary networking interface */
+        ifp = brcmf_add_if(dev, 0, 0, "wlan%d", NULL);
+        if (IS_ERR(ifp))
+                return PTR_ERR(ifp);
+
+        /* signal bus ready */
+        bus_if->state = BRCMF_BUS_DATA;
+
+        /* Bus is ready, do any initialization */
+        ret = brcmf_c_preinit_dcmds(ifp);
         if (ret < 0)
                 return ret;
 
-        /* add primary networking interface */
-        ret = brcmf_add_if(dev, 0, "wlan%d", drvr->mac);
-        if (ret < 0)
+        drvr->config = brcmf_cfg80211_attach(drvr);
+        if (drvr->config == NULL)
+                return -ENOMEM;
+
+        ret = brcmf_net_attach(ifp);
+        if (ret < 0) {
+                brcmf_dbg(ERROR, "brcmf_net_attach failed");
+                drvr->iflist[0] = NULL;
                 return ret;
+        }
 
-        /* signal bus ready */
-        bus_if->state = BRCMF_BUS_DATA;
         return 0;
 }
 
@@ -1163,42 +1057,6 @@ int brcmf_netdev_wait_pend8021x(struct net_device *ndev)
         return pend;
 }
 
-#ifdef DEBUG
-int brcmf_write_to_file(struct brcmf_pub *drvr, const u8 *buf, int size)
-{
-        int ret = 0;
-        struct file *fp;
-        mm_segment_t old_fs;
-        loff_t pos = 0;
-
-        /* change to KERNEL_DS address limit */
-        old_fs = get_fs();
-        set_fs(KERNEL_DS);
-
-        /* open file to write */
-        fp = filp_open("/tmp/mem_dump", O_WRONLY | O_CREAT, 0640);
-        if (!fp) {
-                brcmf_dbg(ERROR, "open file error\n");
-                ret = -1;
-                goto exit;
-        }
-
-        /* Write buf to file */
-        fp->f_op->write(fp, (char __user *)buf, size, &pos);
-
-exit:
-        /* free buf before return */
-        kfree(buf);
-        /* close file before return */
-        if (fp)
-                filp_close(fp, NULL);
-        /* restore previous address limit */
-        set_fs(old_fs);
-
-        return ret;
-}
-#endif                          /* DEBUG */
-
 static void brcmf_driver_init(struct work_struct *work)
 {
         brcmf_debugfs_init();
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
index 6bc4425a8b0f..7fe6779b90cf 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_proto.h
@@ -27,11 +27,6 @@ extern int brcmf_proto_attach(struct brcmf_pub *drvr);
 /* Unlink, frees allocated protocol memory (including brcmf_proto) */
 extern void brcmf_proto_detach(struct brcmf_pub *drvr);
 
-/* Initialize protocol: sync w/dongle state.
- * Sets dongle media info (iswl, drv_version, mac address).
- */
-extern int brcmf_proto_init(struct brcmf_pub *drvr);
-
 /* Stop protocol: sync w/dongle state. */
 extern void brcmf_proto_stop(struct brcmf_pub *drvr);
 
@@ -45,7 +40,8 @@ extern void brcmf_proto_hdrpush(struct brcmf_pub *, int ifidx,
 extern int brcmf_proto_dcmd(struct brcmf_pub *drvr, int ifidx,
                                 struct brcmf_dcmd *dcmd, int len);
 
-extern int brcmf_c_preinit_dcmds(struct brcmf_pub *drvr);
+/* Sets dongle media info (drv_version, mac address). */
+extern int brcmf_c_preinit_dcmds(struct brcmf_if *ifp);
 
 extern int brcmf_proto_cdc_set_dcmd(struct brcmf_pub *drvr, int ifidx,
                                      uint cmd, void *buf, uint len);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 3564686add9a..415f2be36375 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -614,6 +614,12 @@ static const uint max_roundup = 512;
 
 #define ALIGNMENT  4
 
+enum brcmf_sdio_frmtype {
+        BRCMF_SDIO_FT_NORMAL,
+        BRCMF_SDIO_FT_SUPER,
+        BRCMF_SDIO_FT_SUB,
+};
+
 static void pkt_align(struct sk_buff *p, int len, int align)
 {
         uint datalign;
@@ -1032,7 +1038,8 @@ static void brcmf_sdbrcm_free_glom(struct brcmf_sdio *bus)
 }
 
 static bool brcmf_sdio_hdparser(struct brcmf_sdio *bus, u8 *header,
-                                struct brcmf_sdio_read *rd)
+                                struct brcmf_sdio_read *rd,
+                                enum brcmf_sdio_frmtype type)
 {
         u16 len, checksum;
         u8 rx_seq, fc, tx_seq_max;
@@ -1059,6 +1066,15 @@ static bool brcmf_sdio_hdparser(struct brcmf_sdio *bus, u8 *header,
                 brcmf_dbg(ERROR, "HW header length error\n");
                 return false;
         }
+        if (type == BRCMF_SDIO_FT_SUPER &&
+            (roundup(len, bus->blocksize) != rd->len)) {
+                brcmf_dbg(ERROR, "HW superframe header length error\n");
+                return false;
+        }
+        if (type == BRCMF_SDIO_FT_SUB && len > rd->len) {
+                brcmf_dbg(ERROR, "HW subframe header length error\n");
+                return false;
+        }
         rd->len = len;
 
         /*
@@ -1071,9 +1087,16 @@ static bool brcmf_sdio_hdparser(struct brcmf_sdio *bus, u8 *header,
          * Byte 5: Maximum Sequence number allow for Tx
          * Byte 6~7: Reserved
          */
+        if (type == BRCMF_SDIO_FT_SUPER &&
+            SDPCM_GLOMDESC(&header[SDPCM_FRAMETAG_LEN])) {
+                brcmf_dbg(ERROR, "Glom descriptor found in superframe head\n");
+                rd->len = 0;
+                return false;
+        }
         rx_seq = SDPCM_PACKET_SEQUENCE(&header[SDPCM_FRAMETAG_LEN]);
         rd->channel = SDPCM_PACKET_CHANNEL(&header[SDPCM_FRAMETAG_LEN]);
-        if (len > MAX_RX_DATASZ && rd->channel != SDPCM_CONTROL_CHANNEL) {
+        if (len > MAX_RX_DATASZ && rd->channel != SDPCM_CONTROL_CHANNEL &&
+            type != BRCMF_SDIO_FT_SUPER) {
                 brcmf_dbg(ERROR, "HW header length too long\n");
                 bus->sdiodev->bus_if->dstats.rx_errors++;
                 bus->sdcnt.rx_toolong++;
@@ -1081,6 +1104,17 @@ static bool brcmf_sdio_hdparser(struct brcmf_sdio *bus, u8 *header,
                 rd->len = 0;
                 return false;
         }
+        if (type == BRCMF_SDIO_FT_SUPER && rd->channel != SDPCM_GLOM_CHANNEL) {
+                brcmf_dbg(ERROR, "Wrong channel for superframe\n");
+                rd->len = 0;
+                return false;
+        }
+        if (type == BRCMF_SDIO_FT_SUB && rd->channel != SDPCM_DATA_CHANNEL &&
+            rd->channel != SDPCM_EVENT_CHANNEL) {
+                brcmf_dbg(ERROR, "Wrong channel for subframe\n");
+                rd->len = 0;
+                return false;
+        }
         rd->dat_offset = SDPCM_DOFFSET_VALUE(&header[SDPCM_FRAMETAG_LEN]);
         if (rd->dat_offset < SDPCM_HDRLEN || rd->dat_offset > rd->len) {
                 brcmf_dbg(ERROR, "seq %d: bad data offset\n", rx_seq);
@@ -1095,6 +1129,9 @@ static bool brcmf_sdio_hdparser(struct brcmf_sdio *bus, u8 *header,
                 bus->sdcnt.rx_badseq++;
                 rd->seq_num = rx_seq;
         }
+        /* no need to check the reset for subframe */
+        if (type == BRCMF_SDIO_FT_SUB)
+                return true;
         rd->len_nxtfrm = header[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
         if (rd->len_nxtfrm << 4 > MAX_RX_DATASZ) {
                 /* only warm for NON glom packet */
@@ -1126,16 +1163,16 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
         u16 dlen, totlen;
         u8 *dptr, num = 0;
 
-        u16 sublen, check;
+        u16 sublen;
         struct sk_buff *pfirst, *pnext;
 
         int errcode;
-        u8 chan, seq, doff, sfdoff;
-        u8 txmax;
+        u8 doff, sfdoff;
 
         int ifidx = 0;
         bool usechain = bus->use_rxchain;
-        u16 next_len;
+
+        struct brcmf_sdio_read rd_new;
 
         /* If packets, issue read(s) and send up packet chain */
         /* Return sequence numbers consumed? */
@@ -1279,68 +1316,15 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                                    pfirst->data, min_t(int, pfirst->len, 48),
                                    "SUPERFRAME:\n");
 
-                /* Validate the superframe header */
-                dptr = (u8 *) (pfirst->data);
-                sublen = get_unaligned_le16(dptr);
-                check = get_unaligned_le16(dptr + sizeof(u16));
-
-                chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-                seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
-                next_len = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
-                if ((next_len << 4) > MAX_RX_DATASZ) {
-                        brcmf_dbg(INFO, "nextlen too large (%d) seq %d\n",
-                                  next_len, seq);
-                        next_len = 0;
-                }
-                bus->cur_read.len = next_len << 4;
-                doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-                txmax = SDPCM_WINDOW_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
-
-                errcode = 0;
-                if ((u16)~(sublen ^ check)) {
-                        brcmf_dbg(ERROR, "(superframe): HW hdr error: len/check 0x%04x/0x%04x\n",
-                                  sublen, check);
-                        errcode = -1;
-                } else if (roundup(sublen, bus->blocksize) != dlen) {
-                        brcmf_dbg(ERROR, "(superframe): len 0x%04x, rounded 0x%04x, expect 0x%04x\n",
-                                  sublen, roundup(sublen, bus->blocksize),
-                                  dlen);
-                        errcode = -1;
-                } else if (SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]) !=
-                           SDPCM_GLOM_CHANNEL) {
-                        brcmf_dbg(ERROR, "(superframe): bad channel %d\n",
-                                  SDPCM_PACKET_CHANNEL(
-                                          &dptr[SDPCM_FRAMETAG_LEN]));
-                        errcode = -1;
-                } else if (SDPCM_GLOMDESC(&dptr[SDPCM_FRAMETAG_LEN])) {
-                        brcmf_dbg(ERROR, "(superframe): got 2nd descriptor?\n");
-                        errcode = -1;
-                } else if ((doff < SDPCM_HDRLEN) ||
-                           (doff > (pfirst->len - SDPCM_HDRLEN))) {
-                        brcmf_dbg(ERROR, "(superframe): Bad data offset %d: HW %d pkt %d min %d\n",
-                                  doff, sublen, pfirst->len, SDPCM_HDRLEN);
-                        errcode = -1;
-                }
-
-                /* Check sequence number of superframe SW header */
-                if (rxseq != seq) {
-                        brcmf_dbg(INFO, "(superframe) rx_seq %d, expected %d\n",
-                                  seq, rxseq);
-                        bus->sdcnt.rx_badseq++;
-                        rxseq = seq;
-                }
-
-                /* Check window for sanity */
-                if ((u8) (txmax - bus->tx_seq) > 0x40) {
-                        brcmf_dbg(ERROR, "unlikely tx max %d with tx_seq %d\n",
-                                  txmax, bus->tx_seq);
-                        txmax = bus->tx_seq + 2;
-                }
-                bus->tx_max = txmax;
+                rd_new.seq_num = rxseq;
+                rd_new.len = dlen;
+                errcode = -!brcmf_sdio_hdparser(bus, pfirst->data, &rd_new,
+                                                   BRCMF_SDIO_FT_SUPER);
+                bus->cur_read.len = rd_new.len_nxtfrm << 4;
 
                 /* Remove superframe header, remember offset */
-                skb_pull(pfirst, doff);
-                sfdoff = doff;
+                skb_pull(pfirst, rd_new.dat_offset);
+                sfdoff = rd_new.dat_offset;
                 num = 0;
 
                 /* Validate all the subframe headers */
@@ -1349,34 +1333,14 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                         if (errcode)
                                 break;
 
-                        dptr = (u8 *) (pnext->data);
-                        dlen = (u16) (pnext->len);
-                        sublen = get_unaligned_le16(dptr);
-                        check = get_unaligned_le16(dptr + sizeof(u16));
-                        chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-                        doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
+                        rd_new.len = pnext->len;
+                        rd_new.seq_num = rxseq++;
+                        errcode = -!brcmf_sdio_hdparser(bus, pnext->data,
+                                                           &rd_new,
+                                                           BRCMF_SDIO_FT_SUB);
                         brcmf_dbg_hex_dump(BRCMF_GLOM_ON(),
-                                           dptr, 32, "subframe:\n");
+                                           pnext->data, 32, "subframe:\n");
 
-                        if ((u16)~(sublen ^ check)) {
-                                brcmf_dbg(ERROR, "(subframe %d): HW hdr error: len/check 0x%04x/0x%04x\n",
-                                          num, sublen, check);
-                                errcode = -1;
-                        } else if ((sublen > dlen) || (sublen < SDPCM_HDRLEN)) {
-                                brcmf_dbg(ERROR, "(subframe %d): length mismatch: len 0x%04x, expect 0x%04x\n",
-                                          num, sublen, dlen);
-                                errcode = -1;
-                        } else if ((chan != SDPCM_DATA_CHANNEL) &&
-                                   (chan != SDPCM_EVENT_CHANNEL)) {
-                                brcmf_dbg(ERROR, "(subframe %d): bad channel %d\n",
-                                          num, chan);
-                                errcode = -1;
-                        } else if ((doff < SDPCM_HDRLEN) || (doff > sublen)) {
-                                brcmf_dbg(ERROR, "(subframe %d): Bad data offset %d: HW %d min %d\n",
-                                          num, doff, sublen, SDPCM_HDRLEN);
-                                errcode = -1;
-                        }
-                        /* increase the subframe count */
                         num++;
                 }
 
@@ -1402,27 +1366,11 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                 skb_queue_walk_safe(&bus->glom, pfirst, pnext) {
                         dptr = (u8 *) (pfirst->data);
                         sublen = get_unaligned_le16(dptr);
-                        chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
-                        seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
                         doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
 
-                        brcmf_dbg(GLOM, "Get subframe %d, %p(%p/%d), sublen %d chan %d seq %d\n",
-                                  num, pfirst, pfirst->data,
-                                  pfirst->len, sublen, chan, seq);
-
-                        /* precondition: chan == SDPCM_DATA_CHANNEL ||
-                                         chan == SDPCM_EVENT_CHANNEL */
-
-                        if (rxseq != seq) {
-                                brcmf_dbg(GLOM, "rx_seq %d, expected %d\n",
-                                          seq, rxseq);
-                                bus->sdcnt.rx_badseq++;
-                                rxseq = seq;
-                        }
-                        rxseq++;
-
                         brcmf_dbg_hex_dump(BRCMF_BYTES_ON() && BRCMF_DATA_ON(),
-                                           dptr, dlen, "Rx Subframe Data:\n");
+                                           dptr, pfirst->len,
+                                           "Rx Subframe Data:\n");
 
                         __skb_trim(pfirst, sublen);
                         skb_pull(pfirst, doff);
@@ -1642,7 +1590,8 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
                                            bus->rxhdr, SDPCM_HDRLEN,
                                            "RxHdr:\n");
 
-                        if (!brcmf_sdio_hdparser(bus, bus->rxhdr, rd)) {
+                        if (!brcmf_sdio_hdparser(bus, bus->rxhdr, rd,
+                                                 BRCMF_SDIO_FT_NORMAL)) {
                                 if (!bus->rxpending)
                                         break;
                                 else
@@ -1701,7 +1650,8 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
                 } else {
                         memcpy(bus->rxhdr, pkt->data, SDPCM_HDRLEN);
                         rd_new.seq_num = rd->seq_num;
-                        if (!brcmf_sdio_hdparser(bus, bus->rxhdr, &rd_new)) {
+                        if (!brcmf_sdio_hdparser(bus, bus->rxhdr, &rd_new,
+                                                 BRCMF_SDIO_FT_NORMAL)) {
                                 rd->len = 0;
                                 brcmu_pkt_buf_free_skb(pkt);
                         }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
new file mode 100644
index 000000000000..4b272c3d237c
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
@@ -0,0 +1,336 @@
+/*
+ * 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.
+ */
+
+/* FWIL is the Firmware Interface Layer. In this module the support functions
+ * are located to set and get variables to and from the firmware.
+ */
+
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <defs.h>
+#include <brcmu_utils.h>
+#include <brcmu_wifi.h>
+#include "dhd.h"
+#include "dhd_bus.h"
+#include "dhd_dbg.h"
+#include "fwil.h"
+
+
+static s32
+brcmf_fil_cmd_data(struct brcmf_if *ifp, u32 cmd, void *data, u32 len, bool set)
+{
+        struct brcmf_pub *drvr = ifp->drvr;
+        s32 err;
+
+        if (drvr->bus_if->state == BRCMF_BUS_DOWN) {
+                brcmf_dbg(ERROR, "bus is down. we have nothing to do.\n");
+                return -EIO;
+        }
+
+        if (data != NULL)
+                len = min_t(uint, len, BRCMF_DCMD_MAXLEN);
+        if (set)
+                err = brcmf_proto_cdc_set_dcmd(drvr, ifp->idx, cmd, data, len);
+        else
+                err = brcmf_proto_cdc_query_dcmd(drvr, ifp->idx, cmd, data,
+                                                 len);
+
+        if (err >= 0)
+                err = 0;
+        else
+                brcmf_dbg(ERROR, "Failed err=%d\n", err);
+
+        return err;
+}
+
+s32
+brcmf_fil_cmd_data_set(struct brcmf_if *ifp, u32 cmd, void *data, u32 len)
+{
+        s32 err;
+
+        mutex_lock(&ifp->drvr->proto_block);
+
+        brcmf_dbg(FIL, "cmd=%d, len=%d\n", cmd, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        err = brcmf_fil_cmd_data(ifp, cmd, data, len, true);
+        mutex_unlock(&ifp->drvr->proto_block);
+
+        return err;
+}
+
+s32
+brcmf_fil_cmd_data_get(struct brcmf_if *ifp, u32 cmd, void *data, u32 len)
+{
+        s32 err;
+
+        mutex_lock(&ifp->drvr->proto_block);
+        err = brcmf_fil_cmd_data(ifp, cmd, data, len, false);
+
+        brcmf_dbg(FIL, "cmd=%d, len=%d\n", cmd, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        mutex_unlock(&ifp->drvr->proto_block);
+
+        return err;
+}
+
+
+s32
+brcmf_fil_cmd_int_set(struct brcmf_if *ifp, u32 cmd, u32 data)
+{
+        s32 err;
+        __le32 data_le = cpu_to_le32(data);
+
+        mutex_lock(&ifp->drvr->proto_block);
+        err = brcmf_fil_cmd_data(ifp, cmd, &data_le, sizeof(data_le), true);
+        mutex_unlock(&ifp->drvr->proto_block);
+
+        return err;
+}
+
+s32
+brcmf_fil_cmd_int_get(struct brcmf_if *ifp, u32 cmd, u32 *data)
+{
+        s32 err;
+        __le32 data_le = cpu_to_le32(*data);
+
+        mutex_lock(&ifp->drvr->proto_block);
+        err = brcmf_fil_cmd_data(ifp, cmd, &data_le, sizeof(data_le), false);
+        mutex_unlock(&ifp->drvr->proto_block);
+        *data = le32_to_cpu(data_le);
+
+        return err;
+}
+
+static u32
+brcmf_create_iovar(char *name, char *data, u32 datalen, char *buf, u32 buflen)
+{
+        u32 len;
+
+        len = strlen(name) + 1;
+
+        if ((len + datalen) > buflen)
+                return 0;
+
+        memcpy(buf, name, len);
+
+        /* append data onto the end of the name string */
+        if (data && datalen)
+                memcpy(&buf[len], data, datalen);
+
+        return len + datalen;
+}
+
+
+s32
+brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, void *data,
+                         u32 len)
+{
+        struct brcmf_pub *drvr = ifp->drvr;
+        s32 err;
+        u32 buflen;
+
+        mutex_lock(&drvr->proto_block);
+
+        brcmf_dbg(FIL, "name=%s, len=%d\n", name, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        buflen = brcmf_create_iovar(name, data, len, drvr->proto_buf,
+                                    sizeof(drvr->proto_buf));
+        if (buflen) {
+                err = brcmf_fil_cmd_data(ifp, BRCMF_C_SET_VAR, drvr->proto_buf,
+                                         buflen, true);
+        } else {
+                err = -EPERM;
+                brcmf_dbg(ERROR, "Creating iovar failed\n");
+        }
+
+        mutex_unlock(&drvr->proto_block);
+        return err;
+}
+
+s32
+brcmf_fil_iovar_data_get(struct brcmf_if *ifp, char *name, void *data,
+                         u32 len)
+{
+        struct brcmf_pub *drvr = ifp->drvr;
+        s32 err;
+        u32 buflen;
+
+        mutex_lock(&drvr->proto_block);
+
+        buflen = brcmf_create_iovar(name, data, len, drvr->proto_buf,
+                                    sizeof(drvr->proto_buf));
+        if (buflen) {
+                err = brcmf_fil_cmd_data(ifp, BRCMF_C_GET_VAR, drvr->proto_buf,
+                                         buflen, false);
+                if (err == 0)
+                        memcpy(data, drvr->proto_buf, len);
+        } else {
+                err = -EPERM;
+                brcmf_dbg(ERROR, "Creating iovar failed\n");
+        }
+
+        brcmf_dbg(FIL, "name=%s, len=%d\n", name, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        mutex_unlock(&drvr->proto_block);
+        return err;
+}
+
+s32
+brcmf_fil_iovar_int_set(struct brcmf_if *ifp, char *name, u32 data)
+{
+        __le32 data_le = cpu_to_le32(data);
+
+        return brcmf_fil_iovar_data_set(ifp, name, &data_le, sizeof(data_le));
+}
+
+s32
+brcmf_fil_iovar_int_get(struct brcmf_if *ifp, char *name, u32 *data)
+{
+        __le32 data_le = cpu_to_le32(*data);
+        s32 err;
+
+        err = brcmf_fil_iovar_data_get(ifp, name, &data_le, sizeof(data_le));
+        if (err == 0)
+                *data = le32_to_cpu(data_le);
+        return err;
+}
+
+static u32
+brcmf_create_bsscfg(s32 bssidx, char *name, char *data, u32 datalen, char *buf,
+                    u32 buflen)
+{
+        const s8 *prefix = "bsscfg:";
+        s8 *p;
+        u32 prefixlen;
+        u32 namelen;
+        u32 iolen;
+        __le32 bssidx_le;
+
+        if (bssidx == 0)
+                return brcmf_create_iovar(name, data, datalen, buf, buflen);
+
+        prefixlen = strlen(prefix);
+        namelen = strlen(name) + 1; /* lengh of iovar  name + null */
+        iolen = prefixlen + namelen + sizeof(bssidx_le) + datalen;
+
+        if (buflen < iolen) {
+                brcmf_dbg(ERROR, "buffer is too short\n");
+                return 0;
+        }
+
+        p = buf;
+
+        /* copy prefix, no null */
+        memcpy(p, prefix, prefixlen);
+        p += prefixlen;
+
+        /* copy iovar name including null */
+        memcpy(p, name, namelen);
+        p += namelen;
+
+        /* bss config index as first data */
+        bssidx_le = cpu_to_le32(bssidx);
+        memcpy(p, &bssidx_le, sizeof(bssidx_le));
+        p += sizeof(bssidx_le);
+
+        /* parameter buffer follows */
+        if (datalen)
+                memcpy(p, data, datalen);
+
+        return iolen;
+}
+
+s32
+brcmf_fil_bsscfg_data_set(struct brcmf_if *ifp, char *name,
+                          void *data, u32 len)
+{
+        struct brcmf_pub *drvr = ifp->drvr;
+        s32 err;
+        u32 buflen;
+
+        mutex_lock(&drvr->proto_block);
+
+        brcmf_dbg(FIL, "bssidx=%d, name=%s, len=%d\n", ifp->bssidx, name, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        buflen = brcmf_create_bsscfg(ifp->bssidx, name, data, len,
+                                     drvr->proto_buf, sizeof(drvr->proto_buf));
+        if (buflen) {
+                err = brcmf_fil_cmd_data(ifp, BRCMF_C_SET_VAR, drvr->proto_buf,
+                                         buflen, true);
+        } else {
+                err = -EPERM;
+                brcmf_dbg(ERROR, "Creating bsscfg failed\n");
+        }
+
+        mutex_unlock(&drvr->proto_block);
+        return err;
+}
+
+s32
+brcmf_fil_bsscfg_data_get(struct brcmf_if *ifp, char *name,
+                          void *data, u32 len)
+{
+        struct brcmf_pub *drvr = ifp->drvr;
+        s32 err;
+        u32 buflen;
+
+        mutex_lock(&drvr->proto_block);
+
+        buflen = brcmf_create_bsscfg(ifp->bssidx, name, NULL, len,
+                                     drvr->proto_buf, sizeof(drvr->proto_buf));
+        if (buflen) {
+                err = brcmf_fil_cmd_data(ifp, BRCMF_C_GET_VAR, drvr->proto_buf,
+                                         buflen, false);
+                if (err == 0)
+                        memcpy(data, drvr->proto_buf, len);
+        } else {
+                err = -EPERM;
+                brcmf_dbg(ERROR, "Creating bsscfg failed\n");
+        }
+        brcmf_dbg(FIL, "bssidx=%d, name=%s, len=%d\n", ifp->bssidx, name, len);
+        brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data, len, "data");
+
+        mutex_unlock(&drvr->proto_block);
+        return err;
+
+}
+
+s32
+brcmf_fil_bsscfg_int_set(struct brcmf_if *ifp, char *name, u32 data)
+{
+        __le32 data_le = cpu_to_le32(data);
+
+        return brcmf_fil_bsscfg_data_set(ifp, name, &data_le,
+                                         sizeof(data_le));
+}
+
+s32
+brcmf_fil_bsscfg_int_get(struct brcmf_if *ifp, char *name, u32 *data)
+{
+        __le32 data_le = cpu_to_le32(*data);
+        s32 err;
+
+        err = brcmf_fil_bsscfg_data_get(ifp, name, &data_le,
+                                        sizeof(data_le));
+        if (err == 0)
+                *data = le32_to_cpu(data_le);
+        return err;
+}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
new file mode 100644
index 000000000000..16eb8202fb1e
--- /dev/null
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+
+#ifndef _fwil_h_
+#define _fwil_h_
+
+s32 brcmf_fil_cmd_data_set(struct brcmf_if *ifp, u32 cmd, void *data, u32 len);
+s32 brcmf_fil_cmd_data_get(struct brcmf_if *ifp, u32 cmd, void *data, u32 len);
+s32 brcmf_fil_cmd_int_set(struct brcmf_if *ifp, u32 cmd, u32 data);
+s32 brcmf_fil_cmd_int_get(struct brcmf_if *ifp, u32 cmd, u32 *data);
+
+s32 brcmf_fil_iovar_data_set(struct brcmf_if *ifp, char *name, void *data,
+                             u32 len);
+s32 brcmf_fil_iovar_data_get(struct brcmf_if *ifp, char *name, void *data,
+                             u32 len);
+s32 brcmf_fil_iovar_int_set(struct brcmf_if *ifp, char *name, u32 data);
+s32 brcmf_fil_iovar_int_get(struct brcmf_if *ifp, char *name, u32 *data);
+
+s32 brcmf_fil_bsscfg_data_set(struct brcmf_if *ifp, char *name, void *data,
+                              u32 len);
+s32 brcmf_fil_bsscfg_data_get(struct brcmf_if *ifp, char *name, void *data,
+                              u32 len);
+s32 brcmf_fil_bsscfg_int_set(struct brcmf_if *ifp, char *name, u32 data);
+s32 brcmf_fil_bsscfg_int_get(struct brcmf_if *ifp, char *name, u32 *data);
+
+#endif /* _fwil_h_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
index a2b4b1e71017..484a6e4f23a2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/usb.c
@@ -42,7 +42,6 @@
 
 #define IOCTL_RESP_TIMEOUT  2000
 
-#define BRCMF_USB_SYNC_TIMEOUT          300     /* ms */
 #define BRCMF_USB_DLIMAGE_SPINWAIT      100     /* in unit of ms */
 #define BRCMF_USB_DLIMAGE_LIMIT         500     /* spinwait limit (ms) */
 
@@ -116,10 +115,6 @@ struct brcmf_usbdev_info {
         u8 *image;      /* buffer for combine fw and nvram */
         int image_len;
 
-        wait_queue_head_t wait;
-        bool waitdone;
-        int sync_urb_status;
-
         struct usb_device *usbdev;
         struct device *dev;
 
@@ -131,7 +126,6 @@ struct brcmf_usbdev_info {
         int ctl_urb_status;
         int ctl_completed;
         wait_queue_head_t ioctl_resp_wait;
-        wait_queue_head_t ctrl_wait;
         ulong ctl_op;
 
         struct urb *bulk_urb; /* used for FW download */
@@ -754,34 +748,14 @@ static void brcmf_usb_down(struct device *dev)
         brcmf_usb_free_q(&devinfo->rx_postq, true);
 }
 
-static int
-brcmf_usb_sync_wait(struct brcmf_usbdev_info *devinfo, u16 time)
-{
-        int ret;
-        int err = 0;
-        int ms = time;
-
-        ret = wait_event_interruptible_timeout(devinfo->wait,
-                devinfo->waitdone == true, (ms * HZ / 1000));
-
-        if ((devinfo->waitdone == false) || (devinfo->sync_urb_status)) {
-                brcmf_dbg(ERROR, "timeout(%d) or urb err=%d\n",
-                          ret, devinfo->sync_urb_status);
-                err = -EINVAL;
-        }
-        devinfo->waitdone = false;
-        return err;
-}
-
 static void
 brcmf_usb_sync_complete(struct urb *urb)
 {
         struct brcmf_usbdev_info *devinfo =
                         (struct brcmf_usbdev_info *)urb->context;
 
-        devinfo->waitdone = true;
-        wake_up_interruptible(&devinfo->wait);
-        devinfo->sync_urb_status = urb->status;
+        devinfo->ctl_completed = true;
+        brcmf_usb_ioctl_resp_wake(devinfo);
 }
 
 static bool brcmf_usb_dl_cmd(struct brcmf_usbdev_info *devinfo, u8 cmd,
@@ -813,6 +787,7 @@ static bool brcmf_usb_dl_cmd(struct brcmf_usbdev_info *devinfo, u8 cmd,
                 (void *) tmpbuf, size,
                 (usb_complete_t)brcmf_usb_sync_complete, devinfo);
 
+        devinfo->ctl_completed = false;
         ret = usb_submit_urb(devinfo->ctl_urb, GFP_ATOMIC);
         if (ret < 0) {
                 brcmf_dbg(ERROR, "usb_submit_urb failed %d\n", ret);
@@ -820,11 +795,11 @@ static bool brcmf_usb_dl_cmd(struct brcmf_usbdev_info *devinfo, u8 cmd,
                 return false;
         }
 
-        ret = brcmf_usb_sync_wait(devinfo, BRCMF_USB_SYNC_TIMEOUT);
+        ret = brcmf_usb_ioctl_resp_wait(devinfo);
         memcpy(buffer, tmpbuf, buflen);
         kfree(tmpbuf);
 
-        return (ret == 0);
+        return ret;
 }
 
 static bool
@@ -918,13 +893,14 @@ brcmf_usb_dl_send_bulk(struct brcmf_usbdev_info *devinfo, void *buffer, int len)
 
         devinfo->bulk_urb->transfer_flags |= URB_ZERO_PACKET;
 
+        devinfo->ctl_completed = false;
         ret = usb_submit_urb(devinfo->bulk_urb, GFP_ATOMIC);
         if (ret) {
                 brcmf_dbg(ERROR, "usb_submit_urb failed %d\n", ret);
                 return ret;
         }
-        ret = brcmf_usb_sync_wait(devinfo, BRCMF_USB_SYNC_TIMEOUT);
-        return ret;
+        ret = brcmf_usb_ioctl_resp_wait(devinfo);
+        return (ret == 0);
 }
 
 static int
@@ -1284,7 +1260,6 @@ struct brcmf_usbdev *brcmf_usb_attach(struct brcmf_usbdev_info *devinfo,
                 goto error;
         }
 
-        init_waitqueue_head(&devinfo->wait);
         if (!brcmf_usb_dlneeded(devinfo))
                 return &devinfo->bus_pub;
 
@@ -1339,7 +1314,7 @@ static int brcmf_usb_probe_cb(struct brcmf_usbdev_info *devinfo,
         }
 
         ret = brcmf_bus_start(dev);
-        if (ret == -ENOLINK) {
+        if (ret) {
                 brcmf_dbg(ERROR, "dongle is not responding\n");
                 brcmf_detach(dev);
                 goto fail;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index 0e952092ee8f..cb30feaa565b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -35,6 +35,7 @@
 #include <brcmu_wifi.h>
 #include "dhd.h"
 #include "wl_cfg80211.h"
+#include "fwil.h"
 
 #define BRCMF_SCAN_IE_LEN_MAX           2048
 #define BRCMF_PNO_VERSION               2
@@ -48,6 +49,8 @@
 #define BRCMF_PNO_SCAN_COMPLETE         1
 #define BRCMF_PNO_SCAN_INCOMPLETE       0
 
+#define BRCMF_IFACE_MAX_CNT             2
+
 #define TLV_LEN_OFF                     1       /* length offset */
 #define TLV_HDR_LEN                     2       /* header length */
 #define TLV_BODY_OFF                    2       /* body offset */
@@ -91,16 +94,13 @@
 #define BRCMF_ASSOC_PARAMS_FIXED_SIZE \
         (sizeof(struct brcmf_assoc_params_le) - sizeof(u16))
 
-static const u8 ether_bcast[ETH_ALEN] = {255, 255, 255, 255, 255, 255};
-
 static u32 brcmf_dbg_level = WL_DBG_ERR;
 
-static bool check_sys_up(struct wiphy *wiphy)
+static bool check_vif_up(struct brcmf_cfg80211_vif *vif)
 {
-        struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        if (!test_bit(WL_STATUS_READY, &cfg->status)) {
-                WL_INFO("device is not ready : status (%d)\n",
-                        (int)cfg->status);
+        if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state)) {
+                WL_INFO("device is not ready : status (%lu)\n",
+                        vif->sme_state);
                 return false;
         }
         return true;
@@ -391,55 +391,29 @@ static u8 brcmf_mw_to_qdbm(u16 mw)
         return qdbm;
 }
 
-/* function for reading/writing a single u32 from/to the dongle */
-static int
-brcmf_exec_dcmd_u32(struct net_device *ndev, u32 cmd, u32 *par)
+static u16 channel_to_chanspec(struct ieee80211_channel *ch)
 {
-        int err;
-        __le32 par_le = cpu_to_le32(*par);
-
-        err = brcmf_exec_dcmd(ndev, cmd, &par_le, sizeof(__le32));
-        *par = le32_to_cpu(par_le);
-
-        return err;
-}
-
-static s32
-brcmf_dev_iovar_setbuf_bsscfg(struct net_device *ndev, s8 *name,
-                              void *param, s32 paramlen,
-                              void *buf, s32 buflen, s32 bssidx)
-{
-        s32 err = -ENOMEM;
-        u32 len;
-
-        len = brcmf_c_mkiovar_bsscfg(name, param, paramlen,
-                                     buf, buflen, bssidx);
-        BUG_ON(!len);
-        if (len > 0)
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, buf, len);
-        if (err)
-                WL_ERR("error (%d)\n", err);
+        u16 chanspec;
 
-        return err;
-}
+        chanspec = ieee80211_frequency_to_channel(ch->center_freq);
+        chanspec &= WL_CHANSPEC_CHAN_MASK;
 
-static s32
-brcmf_dev_iovar_getbuf_bsscfg(struct net_device *ndev, s8 *name,
-                              void *param, s32 paramlen,
-                              void *buf, s32 buflen, s32 bssidx)
-{
-        s32 err = -ENOMEM;
-        u32 len;
-
-        len = brcmf_c_mkiovar_bsscfg(name, param, paramlen,
-                                     buf, buflen, bssidx);
-        BUG_ON(!len);
-        if (len > 0)
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, buf, len);
-        if (err)
-                WL_ERR("error (%d)\n", err);
+        if (ch->band == IEEE80211_BAND_2GHZ)
+                chanspec |= WL_CHANSPEC_BAND_2G;
+        else
+                chanspec |= WL_CHANSPEC_BAND_5G;
 
-        return err;
+        if (ch->flags & IEEE80211_CHAN_NO_HT40) {
+                chanspec |= WL_CHANSPEC_BW_20;
+                chanspec |= WL_CHANSPEC_CTL_SB_NONE;
+        } else {
+                chanspec |= WL_CHANSPEC_BW_40;
+                if (ch->flags & IEEE80211_CHAN_NO_HT40PLUS)
+                        chanspec |= WL_CHANSPEC_CTL_SB_LOWER;
+                else
+                        chanspec |= WL_CHANSPEC_CTL_SB_UPPER;
+        }
+        return chanspec;
 }
 
 static void convert_key_from_CPU(struct brcmf_wsec_key *key,
@@ -457,18 +431,17 @@ static void convert_key_from_CPU(struct brcmf_wsec_key *key,
 }
 
 static int
-send_key_to_dongle(struct brcmf_cfg80211_info *cfg, s32 bssidx,
-                   struct net_device *ndev, struct brcmf_wsec_key *key)
+send_key_to_dongle(struct net_device *ndev, struct brcmf_wsec_key *key)
 {
         int err;
         struct brcmf_wsec_key_le key_le;
 
         convert_key_from_CPU(key, &key_le);
 
-        err  = brcmf_dev_iovar_setbuf_bsscfg(ndev, "wsec_key", &key_le,
-                                             sizeof(key_le),
-                                             cfg->extra_buf,
-                                             WL_EXTRA_BUF_MAX, bssidx);
+        brcmf_netdev_wait_pend8021x(ndev);
+
+        err = brcmf_fil_bsscfg_data_set(netdev_priv(ndev), "wsec_key", &key_le,
+                                        sizeof(key_le));
 
         if (err)
                 WL_ERR("wsec_key error (%d)\n", err);
@@ -480,6 +453,7 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
                          enum nl80211_iftype type, u32 *flags,
                          struct vif_params *params)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
         s32 infra = 0;
         s32 ap = 0;
@@ -511,7 +485,7 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
         }
 
         if (ap) {
-                set_bit(WL_STATUS_AP_CREATING, &cfg->status);
+                set_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state);
                 if (!cfg->ap_info)
                         cfg->ap_info = kzalloc(sizeof(*cfg->ap_info),
                                                GFP_KERNEL);
@@ -521,7 +495,8 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
                 }
                 WL_INFO("IF Type = AP\n");
         } else {
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_INFRA, &infra);
+                err = brcmf_fil_cmd_int_set(netdev_priv(ndev),
+                                            BRCMF_C_SET_INFRA, infra);
                 if (err) {
                         WL_ERR("WLC_SET_INFRA error (%d)\n", err);
                         err = -EAGAIN;
@@ -539,99 +514,13 @@ done:
         return err;
 }
 
-static s32 brcmf_dev_intvar_set(struct net_device *ndev, s8 *name, s32 val)
-{
-        s8 buf[BRCMF_DCMD_SMLEN];
-        u32 len;
-        s32 err = 0;
-        __le32 val_le;
-
-        val_le = cpu_to_le32(val);
-        len = brcmf_c_mkiovar(name, (char *)(&val_le), sizeof(val_le), buf,
-                            sizeof(buf));
-        BUG_ON(!len);
-
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, buf, len);
-        if (err)
-                WL_ERR("error (%d)\n", err);
-
-        return err;
-}
-
-static s32
-brcmf_dev_intvar_get(struct net_device *ndev, s8 *name, s32 *retval)
-{
-        union {
-                s8 buf[BRCMF_DCMD_SMLEN];
-                __le32 val;
-        } var;
-        u32 len;
-        u32 data_null;
-        s32 err = 0;
-
-        len =
-            brcmf_c_mkiovar(name, (char *)(&data_null), 0, (char *)(&var),
-                        sizeof(var.buf));
-        BUG_ON(!len);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, &var, len);
-        if (err)
-                WL_ERR("error (%d)\n", err);
-
-        *retval = le32_to_cpu(var.val);
-
-        return err;
-}
-
-static s32
-brcmf_dev_intvar_set_bsscfg(struct net_device *ndev, s8 *name, u32 val,
-                            s32 bssidx)
-{
-        s8 buf[BRCMF_DCMD_SMLEN];
-        __le32 val_le;
-
-        val_le = cpu_to_le32(val);
-
-        return brcmf_dev_iovar_setbuf_bsscfg(ndev, name, &val_le,
-                                             sizeof(val_le), buf, sizeof(buf),
-                                             bssidx);
-}
-
-static s32
-brcmf_dev_intvar_get_bsscfg(struct net_device *ndev, s8 *name, s32 *val,
-                            s32 bssidx)
-{
-        s8 buf[BRCMF_DCMD_SMLEN];
-        s32 err;
-        __le32 val_le;
-
-        memset(buf, 0, sizeof(buf));
-        err = brcmf_dev_iovar_getbuf_bsscfg(ndev, name, val, sizeof(*val), buf,
-                                            sizeof(buf), bssidx);
-        if (err == 0) {
-                memcpy(&val_le, buf, sizeof(val_le));
-                *val = le32_to_cpu(val_le);
-        }
-        return err;
-}
-
-
-/*
- * For now brcmf_find_bssidx will return 0. Once p2p gets implemented this
- * should return the ndev matching bssidx.
- */
-static s32
-brcmf_find_bssidx(struct brcmf_cfg80211_info *cfg, struct net_device *ndev)
-{
-        return 0;
-}
-
 static void brcmf_set_mpc(struct net_device *ndev, int mpc)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
 
-        if (test_bit(WL_STATUS_READY, &cfg->status)) {
-                err = brcmf_dev_intvar_set(ndev, "mpc", mpc);
+        if (check_vif_up(ifp->vif)) {
+                err = brcmf_fil_iovar_int_set(ifp, "mpc", mpc);
                 if (err) {
                         WL_ERR("fail to set mpc\n");
                         return;
@@ -643,7 +532,7 @@ static void brcmf_set_mpc(struct net_device *ndev, int mpc)
 static void brcmf_iscan_prep(struct brcmf_scan_params_le *params_le,
                              struct brcmf_ssid *ssid)
 {
-        memcpy(params_le->bssid, ether_bcast, ETH_ALEN);
+        memset(params_le->bssid, 0xFF, ETH_ALEN);
         params_le->bss_type = DOT11_BSSTYPE_ANY;
         params_le->scan_type = 0;
         params_le->channel_num = 0;
@@ -658,30 +547,6 @@ static void brcmf_iscan_prep(struct brcmf_scan_params_le *params_le,
 }
 
 static s32
-brcmf_dev_iovar_setbuf(struct net_device *ndev, s8 * iovar, void *param,
-                    s32 paramlen, void *bufptr, s32 buflen)
-{
-        s32 iolen;
-
-        iolen = brcmf_c_mkiovar(iovar, param, paramlen, bufptr, buflen);
-        BUG_ON(!iolen);
-
-        return brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, bufptr, iolen);
-}
-
-static s32
-brcmf_dev_iovar_getbuf(struct net_device *ndev, s8 * iovar, void *param,
-                    s32 paramlen, void *bufptr, s32 buflen)
-{
-        s32 iolen;
-
-        iolen = brcmf_c_mkiovar(iovar, param, paramlen, bufptr, buflen);
-        BUG_ON(!iolen);
-
-        return brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, bufptr, buflen);
-}
-
-static s32
 brcmf_run_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan,
                 struct brcmf_ssid *ssid, u16 action)
 {
@@ -703,8 +568,8 @@ brcmf_run_iscan(struct brcmf_cfg80211_iscan_ctrl *iscan,
         params->action = cpu_to_le16(action);
         params->scan_duration = cpu_to_le16(0);
 
-        err = brcmf_dev_iovar_setbuf(iscan->ndev, "iscan", params, params_size,
-                                     iscan->dcmd_buf, BRCMF_DCMD_SMLEN);
+        err = brcmf_fil_iovar_data_set(netdev_priv(iscan->ndev), "iscan",
+                                       params, params_size);
         if (err) {
                 if (err == -EBUSY)
                         WL_INFO("system busy : iscan canceled\n");
@@ -721,7 +586,7 @@ static s32 brcmf_do_iscan(struct brcmf_cfg80211_info *cfg)
         struct brcmf_cfg80211_iscan_ctrl *iscan = cfg_to_iscan(cfg);
         struct net_device *ndev = cfg_to_ndev(cfg);
         struct brcmf_ssid ssid;
-        __le32 passive_scan;
+        u32 passive_scan;
         s32 err = 0;
 
         /* Broadcast scan by default */
@@ -729,9 +594,9 @@ static s32 brcmf_do_iscan(struct brcmf_cfg80211_info *cfg)
 
         iscan->state = WL_ISCAN_STATE_SCANING;
 
-        passive_scan = cfg->active_scan ? 0 : cpu_to_le32(1);
-        err = brcmf_exec_dcmd(cfg_to_ndev(cfg), BRCMF_C_SET_PASSIVE_SCAN,
-                        &passive_scan, sizeof(passive_scan));
+        passive_scan = cfg->active_scan ? 0 : 1;
+        err = brcmf_fil_cmd_int_set(netdev_priv(ndev),
+                                    BRCMF_C_SET_PASSIVE_SCAN, passive_scan);
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 return err;
@@ -754,27 +619,27 @@ brcmf_cfg80211_iscan(struct wiphy *wiphy, struct net_device *ndev,
                      struct cfg80211_scan_request *request,
                      struct cfg80211_ssid *this_ssid)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
         struct cfg80211_ssid *ssids;
         struct brcmf_cfg80211_scan_req *sr = cfg->scan_req_int;
-        __le32 passive_scan;
+        u32 passive_scan;
         bool iscan_req;
         bool spec_scan;
         s32 err = 0;
         u32 SSID_len;
 
-        if (test_bit(WL_STATUS_SCANNING, &cfg->status)) {
-                WL_ERR("Scanning already : status (%lu)\n", cfg->status);
+        if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
+                WL_ERR("Scanning already: status (%lu)\n", cfg->scan_status);
                 return -EAGAIN;
         }
-        if (test_bit(WL_STATUS_SCAN_ABORTING, &cfg->status)) {
-                WL_ERR("Scanning being aborted : status (%lu)\n",
-                       cfg->status);
+        if (test_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status)) {
+                WL_ERR("Scanning being aborted: status (%lu)\n",
+                       cfg->scan_status);
                 return -EAGAIN;
         }
-        if (test_bit(WL_STATUS_CONNECTING, &cfg->status)) {
-                WL_ERR("Connecting : status (%lu)\n",
-                       cfg->status);
+        if (test_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state)) {
+                WL_ERR("Connecting: status (%lu)\n", ifp->vif->sme_state);
                 return -EAGAIN;
         }
 
@@ -792,7 +657,7 @@ brcmf_cfg80211_iscan(struct wiphy *wiphy, struct net_device *ndev,
         }
 
         cfg->scan_request = request;
-        set_bit(WL_STATUS_SCANNING, &cfg->status);
+        set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
         if (iscan_req) {
                 err = brcmf_do_iscan(cfg);
                 if (!err)
@@ -813,16 +678,16 @@ brcmf_cfg80211_iscan(struct wiphy *wiphy, struct net_device *ndev,
                         WL_SCAN("Broadcast scan\n");
                 }
 
-                passive_scan = cfg->active_scan ? 0 : cpu_to_le32(1);
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_PASSIVE_SCAN,
-                                &passive_scan, sizeof(passive_scan));
+                passive_scan = cfg->active_scan ? 0 : 1;
+                err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PASSIVE_SCAN,
+                                            passive_scan);
                 if (err) {
                         WL_ERR("WLC_SET_PASSIVE_SCAN error (%d)\n", err);
                         goto scan_out;
                 }
                 brcmf_set_mpc(ndev, 0);
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN, &sr->ssid_le,
-                                      sizeof(sr->ssid_le));
+                err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
+                                             &sr->ssid_le, sizeof(sr->ssid_le));
                 if (err) {
                         if (err == -EBUSY)
                                 WL_INFO("system busy : scan for \"%s\" "
@@ -838,7 +703,7 @@ brcmf_cfg80211_iscan(struct wiphy *wiphy, struct net_device *ndev,
         return 0;
 
 scan_out:
-        clear_bit(WL_STATUS_SCANNING, &cfg->status);
+        clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
         cfg->scan_request = NULL;
         return err;
 }
@@ -851,12 +716,10 @@ static void brcmf_escan_prep(struct brcmf_scan_params_le *params_le,
         s32 i;
         s32 offset;
         u16 chanspec;
-        u16 channel;
-        struct ieee80211_channel *req_channel;
         char *ptr;
         struct brcmf_ssid_le ssid_le;
 
-        memcpy(params_le->bssid, ether_bcast, ETH_ALEN);
+        memset(params_le->bssid, 0xFF, ETH_ALEN);
         params_le->bss_type = DOT11_BSSTYPE_ANY;
         params_le->scan_type = 0;
         params_le->channel_num = 0;
@@ -876,30 +739,9 @@ static void brcmf_escan_prep(struct brcmf_scan_params_le *params_le,
         WL_SCAN("### List of channelspecs to scan ### %d\n", n_channels);
         if (n_channels > 0) {
                 for (i = 0; i < n_channels; i++) {
-                        chanspec = 0;
-                        req_channel = request->channels[i];
-                        channel = ieee80211_frequency_to_channel(
-                                        req_channel->center_freq);
-                        if (req_channel->band == IEEE80211_BAND_2GHZ)
-                                chanspec |= WL_CHANSPEC_BAND_2G;
-                        else
-                                chanspec |= WL_CHANSPEC_BAND_5G;
-
-                        if (req_channel->flags & IEEE80211_CHAN_NO_HT40) {
-                                chanspec |= WL_CHANSPEC_BW_20;
-                                chanspec |= WL_CHANSPEC_CTL_SB_NONE;
-                        } else {
-                                chanspec |= WL_CHANSPEC_BW_40;
-                                if (req_channel->flags &
-                                                IEEE80211_CHAN_NO_HT40PLUS)
-                                        chanspec |= WL_CHANSPEC_CTL_SB_LOWER;
-                                else
-                                        chanspec |= WL_CHANSPEC_CTL_SB_UPPER;
-                        }
-
-                        chanspec |= (channel & WL_CHANSPEC_CHAN_MASK);
+                        chanspec = channel_to_chanspec(request->channels[i]);
                         WL_SCAN("Chan : %d, Channel spec: %x\n",
-                                channel, chanspec);
+                                request->channels[i]->hw_value, chanspec);
                         params_le->channel_list[i] = cpu_to_le16(chanspec);
                 }
         } else {
@@ -966,7 +808,7 @@ brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
                 /* Do a scan abort to stop the driver's scan engine */
                 WL_SCAN("ABORT scan in firmware\n");
                 memset(&params_le, 0, sizeof(params_le));
-                memcpy(params_le.bssid, ether_bcast, ETH_ALEN);
+                memset(params_le.bssid, 0xFF, ETH_ALEN);
                 params_le.bss_type = DOT11_BSSTYPE_ANY;
                 params_le.scan_type = 0;
                 params_le.channel_num = cpu_to_le32(1);
@@ -977,8 +819,8 @@ brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
                 /* Scan is aborted by setting channel_list[0] to -1 */
                 params_le.channel_list[0] = cpu_to_le16(-1);
                 /* E-Scan (or anyother type) can be aborted by SCAN */
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN, &params_le,
-                        sizeof(params_le));
+                err = brcmf_fil_cmd_data_set(netdev_priv(ndev), BRCMF_C_SCAN,
+                                             &params_le, sizeof(params_le));
                 if (err)
                         WL_ERR("Scan abort  failed\n");
         }
@@ -998,7 +840,7 @@ brcmf_notify_escan_complete(struct brcmf_cfg80211_info *cfg,
                 cfg80211_scan_done(scan_request, aborted);
                 brcmf_set_mpc(ndev, 1);
         }
-        if (!test_and_clear_bit(WL_STATUS_SCANNING, &cfg->status)) {
+        if (!test_and_clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
                 WL_ERR("Scan complete while device not scanning\n");
                 return -EPERM;
         }
@@ -1036,8 +878,8 @@ brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct net_device *ndev,
         params->action = cpu_to_le16(action);
         params->sync_id = cpu_to_le16(0x1234);
 
-        err = brcmf_dev_iovar_setbuf(ndev, "escan", params, params_size,
-                        cfg->escan_ioctl_buf, BRCMF_DCMD_MEDLEN);
+        err = brcmf_fil_iovar_data_set(netdev_priv(ndev), "escan",
+                                       params, params_size);
         if (err) {
                 if (err == -EBUSY)
                         WL_INFO("system busy : escan canceled\n");
@@ -1055,16 +897,16 @@ brcmf_do_escan(struct brcmf_cfg80211_info *cfg, struct wiphy *wiphy,
                struct net_device *ndev, struct cfg80211_scan_request *request)
 {
         s32 err;
-        __le32 passive_scan;
+        u32 passive_scan;
         struct brcmf_scan_results *results;
 
         WL_SCAN("Enter\n");
         cfg->escan_info.ndev = ndev;
         cfg->escan_info.wiphy = wiphy;
         cfg->escan_info.escan_state = WL_ESCAN_STATE_SCANNING;
-        passive_scan = cfg->active_scan ? 0 : cpu_to_le32(1);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_PASSIVE_SCAN,
-                        &passive_scan, sizeof(passive_scan));
+        passive_scan = cfg->active_scan ? 0 : 1;
+        err = brcmf_fil_cmd_int_set(netdev_priv(ndev), BRCMF_C_SET_PASSIVE_SCAN,
+                                    passive_scan);
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 return err;
@@ -1086,10 +928,11 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
                      struct cfg80211_scan_request *request,
                      struct cfg80211_ssid *this_ssid)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
         struct cfg80211_ssid *ssids;
         struct brcmf_cfg80211_scan_req *sr = cfg->scan_req_int;
-        __le32 passive_scan;
+        u32 passive_scan;
         bool escan_req;
         bool spec_scan;
         s32 err;
@@ -1097,18 +940,17 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
 
         WL_SCAN("START ESCAN\n");
 
-        if (test_bit(WL_STATUS_SCANNING, &cfg->status)) {
-                WL_ERR("Scanning already : status (%lu)\n", cfg->status);
+        if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
+                WL_ERR("Scanning already: status (%lu)\n", cfg->scan_status);
                 return -EAGAIN;
         }
-        if (test_bit(WL_STATUS_SCAN_ABORTING, &cfg->status)) {
-                WL_ERR("Scanning being aborted : status (%lu)\n",
-                       cfg->status);
+        if (test_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status)) {
+                WL_ERR("Scanning being aborted: status (%lu)\n",
+                       cfg->scan_status);
                 return -EAGAIN;
         }
-        if (test_bit(WL_STATUS_CONNECTING, &cfg->status)) {
-                WL_ERR("Connecting : status (%lu)\n",
-                       cfg->status);
+        if (test_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state)) {
+                WL_ERR("Connecting: status (%lu)\n", ifp->vif->sme_state);
                 return -EAGAIN;
         }
 
@@ -1128,7 +970,7 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
         }
 
         cfg->scan_request = request;
-        set_bit(WL_STATUS_SCANNING, &cfg->status);
+        set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
         if (escan_req) {
                 err = brcmf_do_escan(cfg, wiphy, ndev, request);
                 if (!err)
@@ -1149,16 +991,16 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
                 } else
                         WL_SCAN("Broadcast scan\n");
 
-                passive_scan = cfg->active_scan ? 0 : cpu_to_le32(1);
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_PASSIVE_SCAN,
-                                &passive_scan, sizeof(passive_scan));
+                passive_scan = cfg->active_scan ? 0 : 1;
+                err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PASSIVE_SCAN,
+                                            passive_scan);
                 if (err) {
                         WL_ERR("WLC_SET_PASSIVE_SCAN error (%d)\n", err);
                         goto scan_out;
                 }
                 brcmf_set_mpc(ndev, 0);
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN, &sr->ssid_le,
-                                      sizeof(sr->ssid_le));
+                err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
+                                             &sr->ssid_le, sizeof(sr->ssid_le));
                 if (err) {
                         if (err == -EBUSY)
                                 WL_INFO("BUSY: scan for \"%s\" canceled\n",
@@ -1174,7 +1016,7 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct net_device *ndev,
         return 0;
 
 scan_out:
-        clear_bit(WL_STATUS_SCANNING, &cfg->status);
+        clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
         if (timer_pending(&cfg->escan_timeout))
                 del_timer_sync(&cfg->escan_timeout);
         cfg->scan_request = NULL;
@@ -1182,8 +1024,7 @@ scan_out:
 }
 
 static s32
-brcmf_cfg80211_scan(struct wiphy *wiphy,
-                 struct cfg80211_scan_request *request)
+brcmf_cfg80211_scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 {
         struct net_device *ndev = request->wdev->netdev;
         struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
@@ -1191,7 +1032,8 @@ brcmf_cfg80211_scan(struct wiphy *wiphy,
 
         WL_TRACE("Enter\n");
 
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(container_of(request->wdev,
+                                       struct brcmf_cfg80211_vif, wdev)))
                 return -EIO;
 
         if (cfg->iscan_on)
@@ -1210,7 +1052,8 @@ static s32 brcmf_set_rts(struct net_device *ndev, u32 rts_threshold)
 {
         s32 err = 0;
 
-        err = brcmf_dev_intvar_set(ndev, "rtsthresh", rts_threshold);
+        err = brcmf_fil_iovar_int_set(netdev_priv(ndev), "rtsthresh",
+                                      rts_threshold);
         if (err)
                 WL_ERR("Error (%d)\n", err);
 
@@ -1221,7 +1064,8 @@ static s32 brcmf_set_frag(struct net_device *ndev, u32 frag_threshold)
 {
         s32 err = 0;
 
-        err = brcmf_dev_intvar_set(ndev, "fragthresh", frag_threshold);
+        err = brcmf_fil_iovar_int_set(netdev_priv(ndev), "fragthresh",
+                                      frag_threshold);
         if (err)
                 WL_ERR("Error (%d)\n", err);
 
@@ -1233,7 +1077,7 @@ static s32 brcmf_set_retry(struct net_device *ndev, u32 retry, bool l)
         s32 err = 0;
         u32 cmd = (l ? BRCM_SET_LRL : BRCM_SET_SRL);
 
-        err = brcmf_exec_dcmd_u32(ndev, cmd, &retry);
+        err = brcmf_fil_cmd_int_set(netdev_priv(ndev), cmd, retry);
         if (err) {
                 WL_ERR("cmd (%d) , error (%d)\n", cmd, err);
                 return err;
@@ -1245,10 +1089,11 @@ static s32 brcmf_cfg80211_set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
         struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         if (changed & WIPHY_PARAM_RTS_THRESHOLD &&
@@ -1327,7 +1172,8 @@ static void brcmf_link_down(struct brcmf_cfg80211_info *cfg)
         if (cfg->link_up) {
                 ndev = cfg_to_ndev(cfg);
                 WL_INFO("Call WLC_DISASSOC to stop excess roaming\n ");
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_DISASSOC, NULL, 0);
+                err = brcmf_fil_cmd_data_set(netdev_priv(ndev),
+                                             BRCMF_C_DISASSOC, NULL, 0);
                 if (err)
                         WL_ERR("WLC_DISASSOC failed (%d)\n", err);
                 cfg->link_up = false;
@@ -1340,7 +1186,8 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
                       struct cfg80211_ibss_params *params)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_join_params join_params;
         size_t join_params_size = 0;
         s32 err = 0;
@@ -1348,7 +1195,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
         s32 bcnprd;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         if (params->ssid)
@@ -1358,7 +1205,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
                 return -EOPNOTSUPP;
         }
 
-        set_bit(WL_STATUS_CONNECTING, &cfg->status);
+        set_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state);
 
         if (params->bssid)
                 WL_CONN("BSSID: %pM\n", params->bssid);
@@ -1399,7 +1246,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
         if (params->privacy)
                 wsec |= WEP_ENABLED;
 
-        err = brcmf_dev_intvar_set(ndev, "wsec", wsec);
+        err = brcmf_fil_iovar_int_set(ifp, "wsec", wsec);
         if (err) {
                 WL_ERR("wsec failed (%d)\n", err);
                 goto done;
@@ -1411,7 +1258,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
         else
                 bcnprd = 100;
 
-        err = brcmf_exec_dcmd_u32(ndev, BRCM_SET_BCNPRD, &bcnprd);
+        err = brcmf_fil_cmd_int_set(ifp, BRCM_SET_BCNPRD, bcnprd);
         if (err) {
                 WL_ERR("WLC_SET_BCNPRD failed (%d)\n", err);
                 goto done;
@@ -1434,7 +1281,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
                                    BRCMF_ASSOC_PARAMS_FIXED_SIZE;
                 memcpy(profile->bssid, params->bssid, ETH_ALEN);
         } else {
-                memcpy(join_params.params_le.bssid, ether_bcast, ETH_ALEN);
+                memset(join_params.params_le.bssid, 0xFF, ETH_ALEN);
                 memset(profile->bssid, 0, ETH_ALEN);
         }
 
@@ -1453,8 +1300,8 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 
                 /* set channel for starter */
                 target_channel = cfg->channel;
-                err = brcmf_exec_dcmd_u32(ndev, BRCM_SET_CHANNEL,
-                                          &target_channel);
+                err = brcmf_fil_cmd_int_set(ifp, BRCM_SET_CHANNEL,
+                                            target_channel);
                 if (err) {
                         WL_ERR("WLC_SET_CHANNEL failed (%d)\n", err);
                         goto done;
@@ -1465,8 +1312,8 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
         cfg->ibss_starter = false;
 
 
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SSID,
-                           &join_params, join_params_size);
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SSID,
+                                     &join_params, join_params_size);
         if (err) {
                 WL_ERR("WLC_SET_SSID failed (%d)\n", err);
                 goto done;
@@ -1474,7 +1321,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 
 done:
         if (err)
-                clear_bit(WL_STATUS_CONNECTING, &cfg->status);
+                clear_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state);
         WL_TRACE("Exit\n");
         return err;
 }
@@ -1483,10 +1330,11 @@ static s32
 brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         brcmf_link_down(cfg);
@@ -1499,8 +1347,7 @@ brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 static s32 brcmf_set_wpa_version(struct net_device *ndev,
                                  struct cfg80211_connect_params *sme)
 {
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
         struct brcmf_cfg80211_security *sec;
         s32 val = 0;
         s32 err = 0;
@@ -1512,7 +1359,7 @@ static s32 brcmf_set_wpa_version(struct net_device *ndev,
         else
                 val = WPA_AUTH_DISABLED;
         WL_CONN("setting wpa_auth to 0x%0x\n", val);
-        err = brcmf_dev_intvar_set(ndev, "wpa_auth", val);
+        err = brcmf_fil_iovar_int_set(netdev_priv(ndev), "wpa_auth", val);
         if (err) {
                 WL_ERR("set wpa_auth failed (%d)\n", err);
                 return err;
@@ -1525,8 +1372,7 @@ static s32 brcmf_set_wpa_version(struct net_device *ndev,
 static s32 brcmf_set_auth_type(struct net_device *ndev,
                                struct cfg80211_connect_params *sme)
 {
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
         struct brcmf_cfg80211_security *sec;
         s32 val = 0;
         s32 err = 0;
@@ -1552,7 +1398,7 @@ static s32 brcmf_set_auth_type(struct net_device *ndev,
                 break;
         }
 
-        err = brcmf_dev_intvar_set(ndev, "auth", val);
+        err = brcmf_fil_iovar_int_set(netdev_priv(ndev), "auth", val);
         if (err) {
                 WL_ERR("set auth failed (%d)\n", err);
                 return err;
@@ -1566,8 +1412,7 @@ static s32
 brcmf_set_set_cipher(struct net_device *ndev,
                      struct cfg80211_connect_params *sme)
 {
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
         struct brcmf_cfg80211_security *sec;
         s32 pval = 0;
         s32 gval = 0;
@@ -1617,7 +1462,7 @@ brcmf_set_set_cipher(struct net_device *ndev,
         }
 
         WL_CONN("pval (%d) gval (%d)\n", pval, gval);
-        err = brcmf_dev_intvar_set(ndev, "wsec", pval | gval);
+        err = brcmf_fil_iovar_int_set(netdev_priv(ndev), "wsec", pval | gval);
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 return err;
@@ -1633,14 +1478,14 @@ brcmf_set_set_cipher(struct net_device *ndev,
 static s32
 brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
 {
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
         struct brcmf_cfg80211_security *sec;
         s32 val = 0;
         s32 err = 0;
 
         if (sme->crypto.n_akm_suites) {
-                err = brcmf_dev_intvar_get(ndev, "wpa_auth", &val);
+                err = brcmf_fil_iovar_int_get(netdev_priv(ndev),
+                                              "wpa_auth", &val);
                 if (err) {
                         WL_ERR("could not get wpa_auth (%d)\n", err);
                         return err;
@@ -1674,7 +1519,8 @@ brcmf_set_key_mgmt(struct net_device *ndev, struct cfg80211_connect_params *sme)
                 }
 
                 WL_CONN("setting wpa_auth to %d\n", val);
-                err = brcmf_dev_intvar_set(ndev, "wpa_auth", val);
+                err = brcmf_fil_iovar_int_set(netdev_priv(ndev),
+                                              "wpa_auth", val);
                 if (err) {
                         WL_ERR("could not set wpa_auth (%d)\n", err);
                         return err;
@@ -1690,13 +1536,11 @@ static s32
 brcmf_set_sharedkey(struct net_device *ndev,
                     struct cfg80211_connect_params *sme)
 {
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
         struct brcmf_cfg80211_security *sec;
         struct brcmf_wsec_key key;
         s32 val;
         s32 err = 0;
-        s32 bssidx;
 
         WL_CONN("key len (%d)\n", sme->key_len);
 
@@ -1739,15 +1583,14 @@ brcmf_set_sharedkey(struct net_device *ndev,
         WL_CONN("key length (%d) key index (%d) algo (%d)\n",
                 key.len, key.index, key.algo);
         WL_CONN("key \"%s\"\n", key.data);
-        bssidx = brcmf_find_bssidx(cfg, ndev);
-        err = send_key_to_dongle(cfg, bssidx, ndev, &key);
+        err = send_key_to_dongle(ndev, &key);
         if (err)
                 return err;
 
         if (sec->auth_type == NL80211_AUTHTYPE_SHARED_KEY) {
                 WL_CONN("set auth_type to shared key\n");
                 val = WL_AUTH_SHARED_KEY;       /* shared key */
-                err = brcmf_dev_intvar_set_bsscfg(ndev, "auth", val, bssidx);
+                err = brcmf_fil_bsscfg_int_set(netdev_priv(ndev), "auth", val);
                 if (err)
                         WL_ERR("set auth failed (%d)\n", err);
         }
@@ -1759,7 +1602,8 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
                     struct cfg80211_connect_params *sme)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct ieee80211_channel *chan = sme->channel;
         struct brcmf_join_params join_params;
         size_t join_params_size;
@@ -1768,7 +1612,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         if (!sme->ssid) {
@@ -1776,7 +1620,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
                 return -EOPNOTSUPP;
         }
 
-        set_bit(WL_STATUS_CONNECTING, &cfg->status);
+        set_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state);
 
         if (chan) {
                 cfg->channel =
@@ -1827,7 +1671,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
         memcpy(&profile->ssid.SSID, sme->ssid, profile->ssid.SSID_len);
         join_params.ssid_le.SSID_len = cpu_to_le32(profile->ssid.SSID_len);
 
-        memcpy(join_params.params_le.bssid, ether_bcast, ETH_ALEN);
+        memset(join_params.params_le.bssid, 0xFF, ETH_ALEN);
 
         if (ssid.SSID_len < IEEE80211_MAX_SSID_LEN)
                 WL_CONN("ssid \"%s\", len (%d)\n",
@@ -1835,14 +1679,14 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 
         brcmf_ch_to_chanspec(cfg->channel,
                              &join_params, &join_params_size);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SSID,
-                           &join_params, join_params_size);
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SSID,
+                                     &join_params, join_params_size);
         if (err)
                 WL_ERR("WLC_SET_SSID failed (%d)\n", err);
 
 done:
         if (err)
-                clear_bit(WL_STATUS_CONNECTING, &cfg->status);
+                clear_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state);
         WL_TRACE("Exit\n");
         return err;
 }
@@ -1852,20 +1696,21 @@ brcmf_cfg80211_disconnect(struct wiphy *wiphy, struct net_device *ndev,
                        u16 reason_code)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_scb_val_le scbval;
         s32 err = 0;
 
         WL_TRACE("Enter. Reason code = %d\n", reason_code);
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
-        clear_bit(WL_STATUS_CONNECTED, &cfg->status);
+        clear_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state);
 
         memcpy(&scbval.ea, &profile->bssid, ETH_ALEN);
         scbval.val = cpu_to_le32(reason_code);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_DISASSOC, &scbval,
-                              sizeof(struct brcmf_scb_val_le));
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_DISASSOC,
+                                     &scbval, sizeof(scbval));
         if (err)
                 WL_ERR("error (%d)\n", err);
 
@@ -1882,13 +1727,14 @@ brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
 
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
         struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         u16 txpwrmw;
         s32 err = 0;
         s32 disable = 0;
         s32 dbm = MBM_TO_DBM(mbm);
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         switch (type) {
@@ -1905,7 +1751,7 @@ brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
         }
         /* Make sure radio is off or on as far as software is concerned */
         disable = WL_RADIO_SW_DISABLE << 16;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_RADIO, &disable);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_RADIO, disable);
         if (err)
                 WL_ERR("WLC_SET_RADIO error (%d)\n", err);
 
@@ -1913,8 +1759,8 @@ brcmf_cfg80211_set_tx_power(struct wiphy *wiphy,
                 txpwrmw = 0xffff;
         else
                 txpwrmw = (u16) dbm;
-        err = brcmf_dev_intvar_set(ndev, "qtxpower",
-                        (s32) (brcmf_mw_to_qdbm(txpwrmw)));
+        err = brcmf_fil_iovar_int_set(ifp, "qtxpower",
+                                      (s32)brcmf_mw_to_qdbm(txpwrmw));
         if (err)
                 WL_ERR("qtxpower error (%d)\n", err);
         cfg->conf->tx_power = dbm;
@@ -1927,16 +1773,16 @@ done:
 static s32 brcmf_cfg80211_get_tx_power(struct wiphy *wiphy, s32 *dbm)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
         s32 txpwrdbm;
         u8 result;
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
-        err = brcmf_dev_intvar_get(ndev, "qtxpower", &txpwrdbm);
+        err = brcmf_fil_iovar_int_get(ifp, "qtxpower", &txpwrdbm);
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 goto done;
@@ -1954,19 +1800,17 @@ static s32
 brcmf_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *ndev,
                                u8 key_idx, bool unicast, bool multicast)
 {
-        struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         u32 index;
         u32 wsec;
         s32 err = 0;
-        s32 bssidx;
 
         WL_TRACE("Enter\n");
         WL_CONN("key index (%d)\n", key_idx);
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
-        bssidx = brcmf_find_bssidx(cfg, ndev);
-        err = brcmf_dev_intvar_get_bsscfg(ndev, "wsec", &wsec, bssidx);
+        err = brcmf_fil_bsscfg_int_get(ifp, "wsec", &wsec);
         if (err) {
                 WL_ERR("WLC_GET_WSEC error (%d)\n", err);
                 goto done;
@@ -1975,8 +1819,8 @@ brcmf_cfg80211_config_default_key(struct wiphy *wiphy, struct net_device *ndev,
         if (wsec & WEP_ENABLED) {
                 /* Just select a new current key */
                 index = key_idx;
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_KEY_PRIMARY,
-                                          &index);
+                err = brcmf_fil_cmd_int_set(ifp,
+                                            BRCMF_C_SET_KEY_PRIMARY, index);
                 if (err)
                         WL_ERR("error (%d)\n", err);
         }
@@ -1989,11 +1833,8 @@ static s32
 brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
               u8 key_idx, const u8 *mac_addr, struct key_params *params)
 {
-        struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
         struct brcmf_wsec_key key;
-        struct brcmf_wsec_key_le key_le;
         s32 err = 0;
-        s32 bssidx;
 
         memset(&key, 0, sizeof(key));
         key.index = (u32) key_idx;
@@ -2002,11 +1843,10 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
         if (!is_multicast_ether_addr(mac_addr))
                 memcpy((char *)&key.ea, (void *)mac_addr, ETH_ALEN);
         key.len = (u32) params->key_len;
-        bssidx = brcmf_find_bssidx(cfg, ndev);
         /* check for key index change */
         if (key.len == 0) {
                 /* key delete */
-                err = send_key_to_dongle(cfg, bssidx, ndev, &key);
+                err = send_key_to_dongle(ndev, &key);
                 if (err)
                         WL_ERR("key delete error (%d)\n", err);
         } else {
@@ -2061,13 +1901,7 @@ brcmf_add_keyext(struct wiphy *wiphy, struct net_device *ndev,
                         WL_ERR("Invalid cipher (0x%x)\n", params->cipher);
                         return -EINVAL;
                 }
-                convert_key_from_CPU(&key, &key_le);
-
-                brcmf_netdev_wait_pend8021x(ndev);
-                err  = brcmf_dev_iovar_setbuf_bsscfg(ndev, "wsec_key", &key_le,
-                                                     sizeof(key_le),
-                                                     cfg->extra_buf,
-                                                     WL_EXTRA_BUF_MAX, bssidx);
+                err = send_key_to_dongle(ndev, &key);
                 if (err)
                         WL_ERR("wsec_key error (%d)\n", err);
         }
@@ -2080,16 +1914,16 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
                     struct key_params *params)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_wsec_key key;
         s32 val;
         s32 wsec;
         s32 err = 0;
         u8 keybuf[8];
-        s32 bssidx;
 
         WL_TRACE("Enter\n");
         WL_CONN("key index (%d)\n", key_idx);
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         if (mac_addr) {
@@ -2147,18 +1981,17 @@ brcmf_cfg80211_add_key(struct wiphy *wiphy, struct net_device *ndev,
                 goto done;
         }
 
-        bssidx = brcmf_find_bssidx(cfg, ndev);
-        err = send_key_to_dongle(cfg, bssidx, ndev, &key);
+        err = send_key_to_dongle(ndev, &key);
         if (err)
                 goto done;
 
-        err = brcmf_dev_intvar_get_bsscfg(ndev, "wsec", &wsec, bssidx);
+        err = brcmf_fil_bsscfg_int_get(ifp, "wsec", &wsec);
         if (err) {
                 WL_ERR("get wsec error (%d)\n", err);
                 goto done;
         }
         wsec |= val;
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "wsec", wsec, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "wsec", wsec);
         if (err) {
                 WL_ERR("set wsec error (%d)\n", err);
                 goto done;
@@ -2173,13 +2006,12 @@ static s32
 brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
                     u8 key_idx, bool pairwise, const u8 *mac_addr)
 {
-        struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_wsec_key key;
         s32 err = 0;
-        s32 bssidx;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         memset(&key, 0, sizeof(key));
@@ -2191,8 +2023,7 @@ brcmf_cfg80211_del_key(struct wiphy *wiphy, struct net_device *ndev,
         WL_CONN("key index (%d)\n", key_idx);
 
         /* Set the new key/index */
-        bssidx = brcmf_find_bssidx(cfg, ndev);
-        err = send_key_to_dongle(cfg, bssidx, ndev, &key);
+        err = send_key_to_dongle(ndev, &key);
         if (err) {
                 if (err == -EINVAL) {
                         if (key.index >= DOT11_MAX_DEFAULT_KEYS)
@@ -2213,22 +2044,20 @@ brcmf_cfg80211_get_key(struct wiphy *wiphy, struct net_device *ndev,
                     void (*callback) (void *cookie, struct key_params * params))
 {
         struct key_params params;
-        struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_cfg80211_security *sec;
         s32 wsec;
         s32 err = 0;
-        s32 bssidx;
 
         WL_TRACE("Enter\n");
         WL_CONN("key index (%d)\n", key_idx);
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         memset(&params, 0, sizeof(params));
 
-        bssidx = brcmf_find_bssidx(cfg, ndev);
-        err = brcmf_dev_intvar_get_bsscfg(ndev, "wsec", &wsec, bssidx);
+        err = brcmf_fil_bsscfg_int_get(ifp, "wsec", &wsec);
         if (err) {
                 WL_ERR("WLC_GET_WSEC error (%d)\n", err);
                 /* Ignore this error, may happen during DISASSOC */
@@ -2280,33 +2109,33 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
                            u8 *mac, struct station_info *sinfo)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_scb_val_le scb_val;
         int rssi;
         s32 rate;
         s32 err = 0;
         u8 *bssid = profile->bssid;
-        struct brcmf_sta_info_le *sta_info_le;
+        struct brcmf_sta_info_le sta_info_le;
 
         WL_TRACE("Enter, MAC %pM\n", mac);
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         if (cfg->conf->mode == WL_MODE_AP) {
-                err = brcmf_dev_iovar_getbuf(ndev, "sta_info", mac, ETH_ALEN,
-                                             cfg->dcmd_buf,
-                                             WL_DCMD_LEN_MAX);
+                memcpy(&sta_info_le, mac, ETH_ALEN);
+                err = brcmf_fil_iovar_data_get(ifp, "sta_info",
+                                               &sta_info_le,
+                                               sizeof(sta_info_le));
                 if (err < 0) {
                         WL_ERR("GET STA INFO failed, %d\n", err);
                         goto done;
                 }
-                sta_info_le = (struct brcmf_sta_info_le *)cfg->dcmd_buf;
-
                 sinfo->filled = STATION_INFO_INACTIVE_TIME;
-                sinfo->inactive_time = le32_to_cpu(sta_info_le->idle) * 1000;
-                if (le32_to_cpu(sta_info_le->flags) & BRCMF_STA_ASSOC) {
+                sinfo->inactive_time = le32_to_cpu(sta_info_le.idle) * 1000;
+                if (le32_to_cpu(sta_info_le.flags) & BRCMF_STA_ASSOC) {
                         sinfo->filled |= STATION_INFO_CONNECTED_TIME;
-                        sinfo->connected_time = le32_to_cpu(sta_info_le->in);
+                        sinfo->connected_time = le32_to_cpu(sta_info_le.in);
                 }
                 WL_TRACE("STA idle time : %d ms, connected time :%d sec\n",
                          sinfo->inactive_time, sinfo->connected_time);
@@ -2318,7 +2147,7 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
                         goto done;
                 }
                 /* Report the current tx rate */
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_GET_RATE, &rate);
+        err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_RATE, &rate);
                 if (err) {
                         WL_ERR("Could not get rate (%d)\n", err);
                         goto done;
@@ -2328,10 +2157,11 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
                         WL_CONN("Rate %d Mbps\n", rate / 2);
                 }
 
-                if (test_bit(WL_STATUS_CONNECTED, &cfg->status)) {
+                if (test_bit(BRCMF_VIF_STATUS_CONNECTED,
+                             &ifp->vif->sme_state)) {
                         memset(&scb_val, 0, sizeof(scb_val));
-                        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_RSSI, &scb_val,
-                                              sizeof(scb_val));
+                        err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_RSSI,
+                                                     &scb_val, sizeof(scb_val));
                         if (err) {
                                 WL_ERR("Could not get rssi (%d)\n", err);
                                 goto done;
@@ -2356,6 +2186,7 @@ brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *ndev,
         s32 pm;
         s32 err = 0;
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
 
         WL_TRACE("Enter\n");
 
@@ -2367,7 +2198,7 @@ brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *ndev,
          * FW later while initializing the dongle
          */
         cfg->pwr_save = enabled;
-        if (!test_bit(WL_STATUS_READY, &cfg->status)) {
+        if (!check_vif_up(ifp->vif)) {
 
                 WL_INFO("Device is not ready, storing the value in cfg_info struct\n");
                 goto done;
@@ -2376,7 +2207,7 @@ brcmf_cfg80211_set_power_mgmt(struct wiphy *wiphy, struct net_device *ndev,
         pm = enabled ? PM_FAST : PM_OFF;
         WL_INFO("power save %s\n", (pm ? "enabled" : "disabled"));
 
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_PM, &pm);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM, pm);
         if (err) {
                 if (err == -ENODEV)
                         WL_ERR("net_device is not ready yet\n");
@@ -2393,6 +2224,7 @@ brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *ndev,
                              const u8 *addr,
                              const struct cfg80211_bitrate_mask *mask)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcm_rateset_le rateset_le;
         s32 rate;
         s32 val;
@@ -2402,13 +2234,13 @@ brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *ndev,
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         /* addr param is always NULL. ignore it */
         /* Get current rateset */
-        err = brcmf_exec_dcmd(ndev, BRCM_GET_CURR_RATESET, &rateset_le,
-                              sizeof(rateset_le));
+        err = brcmf_fil_cmd_data_get(ifp, BRCM_GET_CURR_RATESET,
+                                     &rateset_le, sizeof(rateset_le));
         if (err) {
                 WL_ERR("could not get current rateset (%d)\n", err);
                 goto done;
@@ -2435,8 +2267,8 @@ brcmf_cfg80211_set_bitrate_mask(struct wiphy *wiphy, struct net_device *ndev,
          *      Set rate override,
          *      Since the is a/b/g-blind, both a/bg_rate are enforced.
          */
-        err_bg = brcmf_dev_intvar_set(ndev, "bg_rate", rate);
-        err_a = brcmf_dev_intvar_set(ndev, "a_rate", rate);
+        err_bg = brcmf_fil_iovar_int_set(ifp, "bg_rate", rate);
+        err_a = brcmf_fil_iovar_int_set(ifp, "a_rate", rate);
         if (err_bg && err_a) {
                 WL_ERR("could not set fixed rate (%d) (%d)\n", err_bg, err_a);
                 err = err_bg | err_a;
@@ -2565,7 +2397,8 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 
         *(__le32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
 
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_BSS_INFO, buf, WL_BSS_INFO_MAX);
+        err = brcmf_fil_cmd_data_get(netdev_priv(ndev), BRCMF_C_GET_BSS_INFO,
+                                     buf, WL_BSS_INFO_MAX);
         if (err) {
                 WL_ERR("WLC_GET_BSS_INFO failed: %d\n", err);
                 goto CleanUp;
@@ -2674,7 +2507,7 @@ brcmf_tlv_has_ie(u8 *ie, u8 **tlvs, u32 *tlvs_len,
         return false;
 }
 
-struct brcmf_vs_tlv *
+static struct brcmf_vs_tlv *
 brcmf_find_wpaie(u8 *parse, u32 len)
 {
         struct brcmf_tlv *ie;
@@ -2689,7 +2522,9 @@ brcmf_find_wpaie(u8 *parse, u32 len)
 
 static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg)
 {
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_cfg80211_profile *profile = ndev_to_prof(ndev);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_bss_info_le *bi;
         struct brcmf_ssid *ssid;
         struct brcmf_tlv *tim;
@@ -2706,8 +2541,8 @@ static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg)
         ssid = &profile->ssid;
 
         *(__le32 *)cfg->extra_buf = cpu_to_le32(WL_EXTRA_BUF_MAX);
-        err = brcmf_exec_dcmd(cfg_to_ndev(cfg), BRCMF_C_GET_BSS_INFO,
-                        cfg->extra_buf, WL_EXTRA_BUF_MAX);
+        err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSS_INFO,
+                                     cfg->extra_buf, WL_EXTRA_BUF_MAX);
         if (err) {
                 WL_ERR("Could not get bss info %d\n", err);
                 goto update_bss_info_out;
@@ -2732,8 +2567,7 @@ static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg)
                 * so we speficially query dtim information to dongle.
                 */
                 u32 var;
-                err = brcmf_dev_intvar_get(cfg_to_ndev(cfg),
-                                           "dtim_assoc", &var);
+                err = brcmf_fil_iovar_int_get(ifp, "dtim_assoc", &var);
                 if (err) {
                         WL_ERR("wl dtim_assoc failed (%d)\n", err);
                         goto update_bss_info_out;
@@ -2741,9 +2575,6 @@ static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg)
                 dtim_period = (u8)var;
         }
 
-        profile->beacon_interval = beacon_interval;
-        profile->dtim_period = dtim_period;
-
 update_bss_info_out:
         WL_TRACE("Exit");
         return err;
@@ -2755,7 +2586,7 @@ static void brcmf_abort_scanning(struct brcmf_cfg80211_info *cfg)
         struct escan_info *escan = &cfg->escan_info;
         struct brcmf_ssid ssid;
 
-        set_bit(WL_STATUS_SCAN_ABORTING, &cfg->status);
+        set_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status);
         if (cfg->iscan_on) {
                 iscan->state = WL_ISCAN_STATE_IDLE;
 
@@ -2781,8 +2612,8 @@ static void brcmf_abort_scanning(struct brcmf_cfg80211_info *cfg)
                 escan->escan_state = WL_ESCAN_STATE_IDLE;
                 brcmf_notify_escan_complete(cfg, escan->ndev, true, true);
         }
-        clear_bit(WL_STATUS_SCANNING, &cfg->status);
-        clear_bit(WL_STATUS_SCAN_ABORTING, &cfg->status);
+        clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
+        clear_bit(BRCMF_SCAN_STATUS_ABORT, &cfg->scan_status);
 }
 
 static void brcmf_notify_iscan_complete(struct brcmf_cfg80211_iscan_ctrl *iscan,
@@ -2791,7 +2622,7 @@ static void brcmf_notify_iscan_complete(struct brcmf_cfg80211_iscan_ctrl *iscan,
         struct brcmf_cfg80211_info *cfg = iscan_to_cfg(iscan);
         struct net_device *ndev = cfg_to_ndev(cfg);
 
-        if (!test_and_clear_bit(WL_STATUS_SCANNING, &cfg->status)) {
+        if (!test_and_clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
                 WL_ERR("Scan complete while device not scanning\n");
                 return;
         }
@@ -2820,7 +2651,6 @@ static s32
 brcmf_get_iscan_results(struct brcmf_cfg80211_iscan_ctrl *iscan, u32 *status,
                      struct brcmf_scan_results **bss_list)
 {
-        struct brcmf_iscan_results list;
         struct brcmf_scan_results *results;
         struct brcmf_scan_results_le *results_le;
         struct brcmf_iscan_results *list_buf;
@@ -2830,15 +2660,13 @@ brcmf_get_iscan_results(struct brcmf_cfg80211_iscan_ctrl *iscan, u32 *status,
         list_buf = (struct brcmf_iscan_results *)iscan->scan_buf;
         results = &list_buf->results;
         results_le = &list_buf->results_le;
-        results->buflen = BRCMF_ISCAN_RESULTS_FIXED_SIZE;
-        results->version = 0;
-        results->count = 0;
+        results_le->buflen = cpu_to_le32(sizeof(iscan->scan_buf));
+        results_le->version = 0;
+        results_le->count = 0;
 
-        memset(&list, 0, sizeof(list));
-        list.results_le.buflen = cpu_to_le32(WL_ISCAN_BUF_MAX);
-        err = brcmf_dev_iovar_getbuf(iscan->ndev, "iscanresults", &list,
-                                     BRCMF_ISCAN_RESULTS_FIXED_SIZE,
-                                     iscan->scan_buf, WL_ISCAN_BUF_MAX);
+        err = brcmf_fil_iovar_data_get(netdev_priv(iscan->ndev), "iscanresults",
+                                       iscan->scan_buf,
+                                       sizeof(iscan->scan_buf));
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 return err;
@@ -3052,10 +2880,10 @@ brcmf_cfg80211_escan_handler(struct brcmf_cfg80211_info *cfg,
         status = be32_to_cpu(e->status);
 
         if (!ndev || !cfg->escan_on ||
-                        !test_bit(WL_STATUS_SCANNING, &cfg->status)) {
+                        !test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
                 WL_ERR("scan not ready ndev %p wl->escan_on %d drv_status %x\n",
                         ndev, cfg->escan_on,
-                        !test_bit(WL_STATUS_SCANNING, &cfg->status));
+                        !test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status));
                 return -EPERM;
         }
 
@@ -3159,16 +2987,17 @@ static __always_inline void brcmf_delay(u32 ms)
 static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
 
         /*
-         * Check for WL_STATUS_READY before any function call which
+         * Check for BRCMF_VIF_STATUS_READY before any function call which
          * could result is bus access. Don't block the resume for
          * any driver error conditions
          */
         WL_TRACE("Enter\n");
 
-        if (test_bit(WL_STATUS_READY, &cfg->status))
-                brcmf_invoke_iscan(wiphy_to_cfg(wiphy));
+        if (check_vif_up(ifp->vif))
+                brcmf_invoke_iscan(cfg);
 
         WL_TRACE("Exit\n");
         return 0;
@@ -3179,85 +3008,53 @@ static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
         struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_cfg80211_vif *vif;
 
         WL_TRACE("Enter\n");
 
         /*
-         * Check for WL_STATUS_READY before any function call which
-         * could result is bus access. Don't block the suspend for
-         * any driver error conditions
-         */
-
-        /*
-         * While going to suspend if associated with AP disassociate
-         * from AP to save power while system is in suspended state
+         * if the primary net_device is not READY there is nothing
+         * we can do but pray resume goes smoothly.
          */
-        if ((test_bit(WL_STATUS_CONNECTED, &cfg->status) ||
-             test_bit(WL_STATUS_CONNECTING, &cfg->status)) &&
-             test_bit(WL_STATUS_READY, &cfg->status)) {
-                WL_INFO("Disassociating from AP"
-                        " while entering suspend state\n");
-                brcmf_link_down(cfg);
+        vif = ((struct brcmf_if *)netdev_priv(ndev))->vif;
+        if (!check_vif_up(vif))
+                goto exit;
 
+        list_for_each_entry(vif, &cfg->vif_list, list) {
+                if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state))
+                        continue;
                 /*
-                 * Make sure WPA_Supplicant receives all the event
-                 * generated due to DISASSOC call to the fw to keep
-                 * the state fw and WPA_Supplicant state consistent
+                 * While going to suspend if associated with AP disassociate
+                 * from AP to save power while system is in suspended state
                  */
-                brcmf_delay(500);
+                if (test_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state) ||
+                    test_bit(BRCMF_VIF_STATUS_CONNECTING, &vif->sme_state)) {
+                        WL_INFO("Disassociating from AP before suspend\n");
+                        brcmf_link_down(cfg);
+
+                        /* Make sure WPA_Supplicant receives all the event
+                         * generated due to DISASSOC call to the fw to keep
+                         * the state fw and WPA_Supplicant state consistent
+                         */
+                        brcmf_delay(500);
+                }
         }
 
-        if (test_bit(WL_STATUS_READY, &cfg->status))
+        /* end any scanning */
+        if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
                 brcmf_abort_scanning(cfg);
-        else
-                clear_bit(WL_STATUS_SCANNING, &cfg->status);
 
         /* Turn off watchdog timer */
-        if (test_bit(WL_STATUS_READY, &cfg->status))
-                brcmf_set_mpc(ndev, 1);
+        brcmf_set_mpc(ndev, 1);
 
+exit:
         WL_TRACE("Exit\n");
-
+        /* clear any scanning activity */
+        cfg->scan_status = 0;
         return 0;
 }
 
 static __used s32
-brcmf_dev_bufvar_set(struct net_device *ndev, s8 *name, s8 *buf, s32 len)
-{
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        u32 buflen;
-
-        buflen = brcmf_c_mkiovar(name, buf, len, cfg->dcmd_buf,
-                               WL_DCMD_LEN_MAX);
-        BUG_ON(!buflen);
-
-        return brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, cfg->dcmd_buf,
-                               buflen);
-}
-
-static s32
-brcmf_dev_bufvar_get(struct net_device *ndev, s8 *name, s8 *buf,
-                  s32 buf_len)
-{
-        struct brcmf_cfg80211_info *cfg = ndev_to_cfg(ndev);
-        u32 len;
-        s32 err = 0;
-
-        len = brcmf_c_mkiovar(name, NULL, 0, cfg->dcmd_buf,
-                            WL_DCMD_LEN_MAX);
-        BUG_ON(!len);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, cfg->dcmd_buf,
-                              WL_DCMD_LEN_MAX);
-        if (err) {
-                WL_ERR("error (%d)\n", err);
-                return err;
-        }
-        memcpy(buf, cfg->dcmd_buf, buf_len);
-
-        return err;
-}
-
-static __used s32
 brcmf_update_pmklist(struct net_device *ndev,
                      struct brcmf_cfg80211_pmk_list *pmk_list, s32 err)
 {
@@ -3275,8 +3072,8 @@ brcmf_update_pmklist(struct net_device *ndev,
         }
 
         if (!err)
-                brcmf_dev_bufvar_set(ndev, "pmkid_info", (char *)pmk_list,
-                                        sizeof(*pmk_list));
+                brcmf_fil_iovar_data_set(netdev_priv(ndev), "pmkid_info",
+                                         (char *)pmk_list, sizeof(*pmk_list));
 
         return err;
 }
@@ -3286,13 +3083,14 @@ brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *ndev,
                          struct cfg80211_pmksa *pmksa)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct pmkid_list *pmkids = &cfg->pmk_list->pmkids;
         s32 err = 0;
         int i;
         int pmkid_len;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         pmkid_len = le32_to_cpu(pmkids->npmkid);
@@ -3325,12 +3123,13 @@ brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *ndev,
                       struct cfg80211_pmksa *pmksa)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct pmkid_list pmkid;
         s32 err = 0;
         int i, pmkid_len;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETH_ALEN);
@@ -3375,10 +3174,11 @@ static s32
 brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *ndev)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
 
         WL_TRACE("Enter\n");
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         memset(cfg->pmk_list, 0, sizeof(*cfg->pmk_list));
@@ -3478,15 +3278,15 @@ brcmf_notify_sched_scan_results(struct brcmf_cfg80211_info *cfg,
                 if (request->n_ssids)
                         request->ssids = &ssid[0];
 
-                if (test_bit(WL_STATUS_SCANNING, &cfg->status)) {
+                if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
                         /* Abort any on-going scan */
                         brcmf_abort_scanning(cfg);
                 }
 
-                set_bit(WL_STATUS_SCANNING, &cfg->status);
+                set_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
                 err = brcmf_do_escan(cfg, wiphy, ndev, request);
                 if (err) {
-                        clear_bit(WL_STATUS_SCANNING, &cfg->status);
+                        clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status);
                         goto out_err;
                 }
                 cfg->sched_escan = true;
@@ -3512,15 +3312,14 @@ out_err:
 #ifndef CONFIG_BRCMISCAN
 static int brcmf_dev_pno_clean(struct net_device *ndev)
 {
-        char iovbuf[128];
         int ret;
 
         /* Disable pfn */
-        ret = brcmf_dev_intvar_set(ndev, "pfn", 0);
+        ret = brcmf_fil_iovar_int_set(netdev_priv(ndev), "pfn", 0);
         if (ret == 0) {
                 /* clear pfn */
-                ret = brcmf_dev_iovar_setbuf(ndev, "pfnclear", NULL, 0,
-                                             iovbuf, sizeof(iovbuf));
+                ret = brcmf_fil_iovar_data_set(netdev_priv(ndev), "pfnclear",
+                                               NULL, 0);
         }
         if (ret < 0)
                 WL_ERR("failed code %d\n", ret);
@@ -3531,7 +3330,6 @@ static int brcmf_dev_pno_clean(struct net_device *ndev)
 static int brcmf_dev_pno_config(struct net_device *ndev)
 {
         struct brcmf_pno_param_le pfn_param;
-        char iovbuf[128];
 
         memset(&pfn_param, 0, sizeof(pfn_param));
         pfn_param.version = cpu_to_le32(BRCMF_PNO_VERSION);
@@ -3544,9 +3342,8 @@ static int brcmf_dev_pno_config(struct net_device *ndev)
         /* set up pno scan fr */
         pfn_param.scan_freq = cpu_to_le32(BRCMF_PNO_TIME);
 
-        return brcmf_dev_iovar_setbuf(ndev, "pfn_set",
-                                      &pfn_param, sizeof(pfn_param),
-                                      iovbuf, sizeof(iovbuf));
+        return brcmf_fil_iovar_data_set(netdev_priv(ndev), "pfn_set",
+                                        &pfn_param, sizeof(pfn_param));
 }
 
 static int
@@ -3554,7 +3351,7 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
                                 struct net_device *ndev,
                                 struct cfg80211_sched_scan_request *request)
 {
-        char iovbuf[128];
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_cfg80211_info *cfg = wiphy_priv(wiphy);
         struct brcmf_pno_net_param_le pfn;
         int i;
@@ -3562,14 +3359,14 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 
         WL_SCAN("Enter n_match_sets:%d   n_ssids:%d\n",
                 request->n_match_sets, request->n_ssids);
-        if (test_bit(WL_STATUS_SCANNING, &cfg->status)) {
-                WL_ERR("Scanning already : status (%lu)\n", cfg->status);
+        if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
+                WL_ERR("Scanning already: status (%lu)\n", cfg->scan_status);
                 return -EAGAIN;
         }
 
         if (!request || !request->n_ssids || !request->n_match_sets) {
                 WL_ERR("Invalid sched scan req!! n_ssids:%d\n",
-                       request->n_ssids);
+                       request ? request->n_ssids : 0);
                 return -EINVAL;
         }
 
@@ -3620,15 +3417,14 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
                         pfn.flags = cpu_to_le32(1 << BRCMF_PNO_HIDDEN_BIT);
                         pfn.ssid.SSID_len = cpu_to_le32(ssid_len);
                         memcpy(pfn.ssid.SSID, ssid->ssid, ssid_len);
-                        ret = brcmf_dev_iovar_setbuf(ndev, "pfn_add",
-                                                     &pfn, sizeof(pfn),
-                                                     iovbuf, sizeof(iovbuf));
+                        ret = brcmf_fil_iovar_data_set(ifp, "pfn_add", &pfn,
+                                                       sizeof(pfn));
                         WL_SCAN(">>> PNO filter %s for ssid (%s)\n",
                                 ret == 0 ? "set" : "failed",
                                 ssid->ssid);
                 }
                 /* Enable the PNO */
-                if (brcmf_dev_intvar_set(ndev, "pfn", 1) < 0) {
+                if (brcmf_fil_iovar_int_set(ifp, "pfn", 1) < 0) {
                         WL_ERR("PNO enable failed!! ret=%d\n", ret);
                         return -EINVAL;
                 }
@@ -3656,12 +3452,20 @@ static int brcmf_cfg80211_sched_scan_stop(struct wiphy *wiphy,
 static int brcmf_cfg80211_testmode(struct wiphy *wiphy, void *data, int len)
 {
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        struct net_device *ndev = cfg->wdev->netdev;
+        struct net_device *ndev = cfg_to_ndev(cfg);
         struct brcmf_dcmd *dcmd = data;
         struct sk_buff *reply;
         int ret;
 
-        ret = brcmf_netlink_dcmd(ndev, dcmd);
+        WL_TRACE("cmd %x set %d buf %p len %d\n", dcmd->cmd, dcmd->set,
+                 dcmd->buf, dcmd->len);
+
+        if (dcmd->set)
+                ret = brcmf_fil_cmd_data_set(netdev_priv(ndev), dcmd->cmd,
+                                             dcmd->buf, dcmd->len);
+        else
+                ret = brcmf_fil_cmd_data_get(netdev_priv(ndev), dcmd->cmd,
+                                             dcmd->buf, dcmd->len);
         if (ret == 0) {
                 reply = cfg80211_testmode_alloc_reply_skb(wiphy, sizeof(*dcmd));
                 nla_put(reply, NL80211_ATTR_TESTDATA, sizeof(*dcmd), dcmd);
@@ -3673,23 +3477,23 @@ static int brcmf_cfg80211_testmode(struct wiphy *wiphy, void *data, int len)
 
 static s32 brcmf_configure_opensecurity(struct net_device *ndev, s32 bssidx)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err;
 
         /* set auth */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "auth", 0, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "auth", 0);
         if (err < 0) {
                 WL_ERR("auth error %d\n", err);
                 return err;
         }
         /* set wsec */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "wsec", 0, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "wsec", 0);
         if (err < 0) {
                 WL_ERR("wsec error %d\n", err);
                 return err;
         }
         /* set upper-layer auth */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "wpa_auth",
-                                          WPA_AUTH_NONE, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "wpa_auth", WPA_AUTH_NONE);
         if (err < 0) {
                 WL_ERR("wpa_auth error %d\n", err);
                 return err;
@@ -3710,6 +3514,7 @@ static s32
 brcmf_configure_wpaie(struct net_device *ndev, struct brcmf_vs_tlv *wpa_ie,
                      bool is_rsn_ie, s32 bssidx)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         u32 auth = 0; /* d11 open authentication */
         u16 count;
         s32 err = 0;
@@ -3850,8 +3655,8 @@ brcmf_configure_wpaie(struct net_device *ndev, struct brcmf_vs_tlv *wpa_ie,
                                 wme_bss_disable = 0;
                 }
                 /* set wme_bss_disable to sync RSN Capabilities */
-                err = brcmf_dev_intvar_set_bsscfg(ndev, "wme_bss_disable",
-                                                  wme_bss_disable, bssidx);
+                err = brcmf_fil_bsscfg_int_set(ifp, "wme_bss_disable",
+                                               wme_bss_disable);
                 if (err < 0) {
                         WL_ERR("wme_bss_disable error %d\n", err);
                         goto exit;
@@ -3861,19 +3666,19 @@ brcmf_configure_wpaie(struct net_device *ndev, struct brcmf_vs_tlv *wpa_ie,
         wsec = (pval | gval | SES_OW_ENABLED);
 
         /* set auth */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "auth", auth, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "auth", auth);
         if (err < 0) {
                 WL_ERR("auth error %d\n", err);
                 goto exit;
         }
         /* set wsec */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "wsec", wsec, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "wsec", wsec);
         if (err < 0) {
                 WL_ERR("wsec error %d\n", err);
                 goto exit;
         }
         /* set upper-layer auth */
-        err = brcmf_dev_intvar_set_bsscfg(ndev, "wpa_auth", wpa_auth, bssidx);
+        err = brcmf_fil_bsscfg_int_set(ifp, "wpa_auth", wpa_auth);
         if (err < 0) {
                 WL_ERR("wpa_auth error %d\n", err);
                 goto exit;
@@ -3963,17 +3768,19 @@ brcmf_vndr_ie(u8 *iebuf, s32 pktflag, u8 *ie_ptr, u32 ie_len, s8 *add_del_cmd)
         return ie_len + VNDR_IE_HDR_SIZE;
 }
 
-s32
+static s32
 brcmf_set_management_ie(struct brcmf_cfg80211_info *cfg,
-                        struct net_device *ndev, s32 bssidx, s32 pktflag,
+                        struct net_device *ndev, s32 pktflag,
                         u8 *vndr_ie_buf, u32 vndr_ie_len)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct vif_saved_ie *saved_ie = &ifp->vif->saved_ie;
         s32 err = 0;
         u8  *iovar_ie_buf;
         u8  *curr_ie_buf;
         u8  *mgmt_ie_buf = NULL;
-        u32 mgmt_ie_buf_len = 0;
-        u32 *mgmt_ie_len = 0;
+        int mgmt_ie_buf_len;
+        u32 *mgmt_ie_len;
         u32 del_add_ie_buf_len = 0;
         u32 total_ie_buf_len = 0;
         u32 parsed_ie_buf_len = 0;
@@ -3982,33 +3789,31 @@ brcmf_set_management_ie(struct brcmf_cfg80211_info *cfg,
         struct parsed_vndr_ie_info *vndrie_info;
         s32 i;
         u8 *ptr;
-        u32 remained_buf_len;
+        int remained_buf_len;
 
-        WL_TRACE("bssidx %d, pktflag : 0x%02X\n", bssidx, pktflag);
+        WL_TRACE("bssidx %d, pktflag : 0x%02X\n",
+                 brcmf_ndev_bssidx(ndev), pktflag);
         iovar_ie_buf = kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
         if (!iovar_ie_buf)
                 return -ENOMEM;
         curr_ie_buf = iovar_ie_buf;
-        if (test_bit(WL_STATUS_AP_CREATING, &cfg->status) ||
-            test_bit(WL_STATUS_AP_CREATED, &cfg->status)) {
+        if (ifp->vif->mode == WL_MODE_AP) {
                 switch (pktflag) {
                 case VNDR_IE_PRBRSP_FLAG:
-                        mgmt_ie_buf = cfg->ap_info->probe_res_ie;
-                        mgmt_ie_len = &cfg->ap_info->probe_res_ie_len;
-                        mgmt_ie_buf_len =
-                                sizeof(cfg->ap_info->probe_res_ie);
+                        mgmt_ie_buf = saved_ie->probe_res_ie;
+                        mgmt_ie_len = &saved_ie->probe_res_ie_len;
+                        mgmt_ie_buf_len = sizeof(saved_ie->probe_res_ie);
                         break;
                 case VNDR_IE_BEACON_FLAG:
-                        mgmt_ie_buf = cfg->ap_info->beacon_ie;
-                        mgmt_ie_len = &cfg->ap_info->beacon_ie_len;
-                        mgmt_ie_buf_len = sizeof(cfg->ap_info->beacon_ie);
+                        mgmt_ie_buf = saved_ie->beacon_ie;
+                        mgmt_ie_len = &saved_ie->beacon_ie_len;
+                        mgmt_ie_buf_len = sizeof(saved_ie->beacon_ie);
                         break;
                 default:
                         err = -EPERM;
                         WL_ERR("not suitable type\n");
                         goto exit;
                 }
-                bssidx = 0;
         } else {
                 err = -EPERM;
                 WL_ERR("not suitable type\n");
@@ -4104,11 +3909,8 @@ brcmf_set_management_ie(struct brcmf_cfg80211_info *cfg,
                 }
         }
         if (total_ie_buf_len) {
-                err  = brcmf_dev_iovar_setbuf_bsscfg(ndev, "vndr_ie",
-                                                     iovar_ie_buf,
-                                                     total_ie_buf_len,
-                                                     cfg->extra_buf,
-                                                     WL_EXTRA_BUF_MAX, bssidx);
+                err  = brcmf_fil_bsscfg_data_set(ifp, "vndr_ie", iovar_ie_buf,
+                                                 total_ie_buf_len);
                 if (err)
                         WL_ERR("vndr ie set error : %d\n", err);
         }
@@ -4123,9 +3925,9 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
                         struct cfg80211_ap_settings *settings)
 {
         s32 ie_offset;
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_tlv *ssid_ie;
         struct brcmf_ssid_le ssid_le;
-        s32 ioctl_value;
         s32 err = -EPERM;
         struct brcmf_tlv *rsn_ie;
         struct brcmf_vs_tlv *wpa_ie;
@@ -4140,7 +3942,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
                  settings->ssid, settings->ssid_len, settings->auth_type,
                  settings->inactivity_timeout);
 
-        if (!test_bit(WL_STATUS_AP_CREATING, &cfg->status)) {
+        if (!test_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state)) {
                 WL_ERR("Not in AP creation mode\n");
                 return -EPERM;
         }
@@ -4164,20 +3966,17 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
         }
 
         brcmf_set_mpc(ndev, 0);
-        ioctl_value = 1;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_DOWN, &ioctl_value);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_DOWN, 1);
         if (err < 0) {
                 WL_ERR("BRCMF_C_DOWN error %d\n", err);
                 goto exit;
         }
-        ioctl_value = 1;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_INFRA, &ioctl_value);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_INFRA, 1);
         if (err < 0) {
                 WL_ERR("SET INFRA error %d\n", err);
                 goto exit;
         }
-        ioctl_value = 1;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_AP, &ioctl_value);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_AP, 1);
         if (err < 0) {
                 WL_ERR("setting AP mode failed %d\n", err);
                 goto exit;
@@ -4226,7 +4025,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
                 cfg->ap_info->security_mode = false;
         }
         /* Set Beacon IEs to FW */
-        err = brcmf_set_management_ie(cfg, ndev, bssidx,
+        err = brcmf_set_management_ie(cfg, ndev,
                                       VNDR_IE_BEACON_FLAG,
                                       (u8 *)settings->beacon.tail,
                                       settings->beacon.tail_len);
@@ -4236,7 +4035,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
                 WL_TRACE("Applied Vndr IEs for Beacon\n");
 
         /* Set Probe Response IEs to FW */
-        err = brcmf_set_management_ie(cfg, ndev, bssidx,
+        err = brcmf_set_management_ie(cfg, ndev,
                                       VNDR_IE_PRBRSP_FLAG,
                                       (u8 *)settings->beacon.proberesp_ies,
                                       settings->beacon.proberesp_ies_len);
@@ -4246,25 +4045,22 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
                 WL_TRACE("Applied Vndr IEs for Probe Resp\n");
 
         if (settings->beacon_interval) {
-                ioctl_value = settings->beacon_interval;
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_BCNPRD,
-                                          &ioctl_value);
+                err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_BCNPRD,
+                                            settings->beacon_interval);
                 if (err < 0) {
                         WL_ERR("Beacon Interval Set Error, %d\n", err);
                         goto exit;
                 }
         }
         if (settings->dtim_period) {
-                ioctl_value = settings->dtim_period;
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_DTIMPRD,
-                                          &ioctl_value);
+                err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_DTIMPRD,
+                                            settings->dtim_period);
                 if (err < 0) {
                         WL_ERR("DTIM Interval Set Error, %d\n", err);
                         goto exit;
                 }
         }
-        ioctl_value = 1;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_UP, &ioctl_value);
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 1);
         if (err < 0) {
                 WL_ERR("BRCMF_C_UP error (%d)\n", err);
                 goto exit;
@@ -4274,14 +4070,14 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
         /* join parameters starts with ssid */
         memcpy(&join_params.ssid_le, &ssid_le, sizeof(ssid_le));
         /* create softap */
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SSID, &join_params,
-                              sizeof(join_params));
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_SSID,
+                                     &join_params, sizeof(join_params));
         if (err < 0) {
                 WL_ERR("SET SSID error (%d)\n", err);
                 goto exit;
         }
-        clear_bit(WL_STATUS_AP_CREATING, &cfg->status);
-        set_bit(WL_STATUS_AP_CREATED, &cfg->status);
+        clear_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state);
+        set_bit(BRCMF_VIF_STATUS_AP_CREATED, &ifp->vif->sme_state);
 
 exit:
         if (err)
@@ -4291,8 +4087,8 @@ exit:
 
 static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-        s32 ioctl_value;
         s32 err = -EPERM;
 
         WL_TRACE("Enter\n");
@@ -4301,21 +4097,20 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
                 /* Due to most likely deauths outstanding we sleep */
                 /* first to make sure they get processed by fw. */
                 msleep(400);
-                ioctl_value = 0;
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_AP, &ioctl_value);
+                err = brcmf_fil_cmd_int_set(netdev_priv(ndev),
+                                            BRCMF_C_SET_AP, 0);
                 if (err < 0) {
                         WL_ERR("setting AP mode failed %d\n", err);
                         goto exit;
                 }
-                ioctl_value = 0;
-                err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_UP, &ioctl_value);
+                err = brcmf_fil_cmd_int_set(netdev_priv(ndev), BRCMF_C_UP, 0);
                 if (err < 0) {
                         WL_ERR("BRCMF_C_UP error %d\n", err);
                         goto exit;
                 }
                 brcmf_set_mpc(ndev, 1);
-                clear_bit(WL_STATUS_AP_CREATING, &cfg->status);
-                clear_bit(WL_STATUS_AP_CREATED, &cfg->status);
+                clear_bit(BRCMF_VIF_STATUS_AP_CREATING, &ifp->vif->sme_state);
+                clear_bit(BRCMF_VIF_STATUS_AP_CREATED, &ifp->vif->sme_state);
         }
 exit:
         return err;
@@ -4326,6 +4121,7 @@ brcmf_cfg80211_del_station(struct wiphy *wiphy, struct net_device *ndev,
                            u8 *mac)
 {
         struct brcmf_scb_val_le scbval;
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err;
 
         if (!mac)
@@ -4333,13 +4129,13 @@ brcmf_cfg80211_del_station(struct wiphy *wiphy, struct net_device *ndev,
 
         WL_TRACE("Enter %pM\n", mac);
 
-        if (!check_sys_up(wiphy))
+        if (!check_vif_up(ifp->vif))
                 return -EIO;
 
         memcpy(&scbval.ea, mac, ETH_ALEN);
         scbval.val = cpu_to_le32(WLAN_REASON_DEAUTH_LEAVING);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SCB_DEAUTHENTICATE_FOR_REASON,
-                              &scbval, sizeof(scbval));
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCB_DEAUTHENTICATE_FOR_REASON,
+                                     &scbval, sizeof(scbval));
         if (err)
                 WL_ERR("SCB_DEAUTHENTICATE_FOR_REASON failed %d\n", err);
 
@@ -4410,72 +4206,97 @@ static void brcmf_wiphy_pno_params(struct wiphy *wiphy)
 #endif
 }
 
-static struct wireless_dev *brcmf_alloc_wdev(struct device *ndev)
+static struct wiphy *brcmf_setup_wiphy(struct device *phydev)
 {
-        struct wireless_dev *wdev;
+        struct wiphy *wiphy;
         s32 err = 0;
 
-        wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
-        if (!wdev)
-                return ERR_PTR(-ENOMEM);
-
-        wdev->wiphy = wiphy_new(&wl_cfg80211_ops,
-                                sizeof(struct brcmf_cfg80211_info));
-        if (!wdev->wiphy) {
+        wiphy = wiphy_new(&wl_cfg80211_ops, sizeof(struct brcmf_cfg80211_info));
+        if (!wiphy) {
                 WL_ERR("Could not allocate wiphy device\n");
-                err = -ENOMEM;
-                goto wiphy_new_out;
-        }
-        set_wiphy_dev(wdev->wiphy, ndev);
-        wdev->wiphy->max_scan_ssids = WL_NUM_SCAN_MAX;
-        wdev->wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
-        wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
-                                       BIT(NL80211_IFTYPE_ADHOC) |
-                                       BIT(NL80211_IFTYPE_AP);
-        wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
-        wdev->wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;    /* Set
+                return ERR_PTR(-ENOMEM);
+        }
+        set_wiphy_dev(wiphy, phydev);
+        wiphy->max_scan_ssids = WL_NUM_SCAN_MAX;
+        wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
+        wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+                                 BIT(NL80211_IFTYPE_ADHOC) |
+                                 BIT(NL80211_IFTYPE_AP);
+        wiphy->bands[IEEE80211_BAND_2GHZ] = &__wl_band_2ghz;
+        wiphy->bands[IEEE80211_BAND_5GHZ] = &__wl_band_5ghz_a;  /* Set
                                                 * it as 11a by default.
                                                 * This will be updated with
                                                 * 11n phy tables in
                                                 * "ifconfig up"
                                                 * if phy has 11n capability
                                                 */
-        wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
-        wdev->wiphy->cipher_suites = __wl_cipher_suites;
-        wdev->wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
-        wdev->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;      /* enable power
+        wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
+        wiphy->cipher_suites = __wl_cipher_suites;
+        wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
+        wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;    /* enable power
                                                                  * save mode
                                                                  * by default
                                                                  */
-        brcmf_wiphy_pno_params(wdev->wiphy);
-        err = wiphy_register(wdev->wiphy);
+        brcmf_wiphy_pno_params(wiphy);
+        err = wiphy_register(wiphy);
         if (err < 0) {
                 WL_ERR("Could not register wiphy device (%d)\n", err);
-                goto wiphy_register_out;
+                wiphy_free(wiphy);
+                return ERR_PTR(err);
         }
-        return wdev;
+        return wiphy;
+}
+
+static
+struct brcmf_cfg80211_vif *brcmf_alloc_vif(struct brcmf_cfg80211_info *cfg,
+                                           struct net_device *netdev,
+                                           s32 mode, bool pm_block)
+{
+        struct brcmf_cfg80211_vif *vif;
+
+        if (cfg->vif_cnt == BRCMF_IFACE_MAX_CNT)
+                return ERR_PTR(-ENOSPC);
 
-wiphy_register_out:
-        wiphy_free(wdev->wiphy);
+        vif = kzalloc(sizeof(*vif), GFP_KERNEL);
+        if (!vif)
+                return ERR_PTR(-ENOMEM);
+
+        vif->wdev.wiphy = cfg->wiphy;
+        vif->wdev.netdev = netdev;
+        vif->wdev.iftype = brcmf_mode_to_nl80211_iftype(mode);
+
+        if (netdev) {
+                vif->ifp = netdev_priv(netdev);
+                netdev->ieee80211_ptr = &vif->wdev;
+                SET_NETDEV_DEV(netdev, wiphy_dev(cfg->wiphy));
+        }
 
-wiphy_new_out:
-        kfree(wdev);
+        vif->mode = mode;
+        vif->pm_block = pm_block;
+        vif->roam_off = -1;
 
-        return ERR_PTR(err);
+        brcmf_init_prof(&vif->profile);
+
+        list_add_tail(&vif->list, &cfg->vif_list);
+        cfg->vif_cnt++;
+        return vif;
 }
 
-static void brcmf_free_wdev(struct brcmf_cfg80211_info *cfg)
+static void brcmf_free_vif(struct brcmf_cfg80211_vif *vif)
 {
-        struct wireless_dev *wdev = cfg->wdev;
+        struct brcmf_cfg80211_info *cfg;
+        struct wiphy *wiphy;
 
-        if (!wdev) {
-                WL_ERR("wdev is invalid\n");
-                return;
+        wiphy = vif->wdev.wiphy;
+        cfg = wiphy_priv(wiphy);
+        list_del(&vif->list);
+        cfg->vif_cnt--;
+
+        kfree(vif);
+        if (!cfg->vif_cnt) {
+                wiphy_unregister(wiphy);
+                wiphy_free(wiphy);
         }
-        wiphy_unregister(wdev->wiphy);
-        wiphy_free(wdev->wiphy);
-        kfree(wdev);
-        cfg->wdev = NULL;
 }
 
 static bool brcmf_is_linkup(struct brcmf_cfg80211_info *cfg,
@@ -4541,7 +4362,7 @@ static void brcmf_clear_assoc_ies(struct brcmf_cfg80211_info *cfg)
 
 static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_info *cfg)
 {
-        struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
         struct brcmf_cfg80211_assoc_ielen_le *assoc_info;
         struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg);
         u32 req_len;
@@ -4550,8 +4371,8 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_info *cfg)
 
         brcmf_clear_assoc_ies(cfg);
 
-        err = brcmf_dev_bufvar_get(ndev, "assoc_info", cfg->extra_buf,
-                                WL_ASSOC_INFO_MAX);
+        err = brcmf_fil_iovar_data_get(ifp, "assoc_info",
+                                       cfg->extra_buf, WL_ASSOC_INFO_MAX);
         if (err) {
                 WL_ERR("could not get assoc info (%d)\n", err);
                 return err;
@@ -4561,9 +4382,9 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_info *cfg)
         req_len = le32_to_cpu(assoc_info->req_len);
         resp_len = le32_to_cpu(assoc_info->resp_len);
         if (req_len) {
-                err = brcmf_dev_bufvar_get(ndev, "assoc_req_ies",
-                                           cfg->extra_buf,
-                                           WL_ASSOC_INFO_MAX);
+                err = brcmf_fil_iovar_data_get(ifp, "assoc_req_ies",
+                                               cfg->extra_buf,
+                                               WL_ASSOC_INFO_MAX);
                 if (err) {
                         WL_ERR("could not get assoc req (%d)\n", err);
                         return err;
@@ -4577,9 +4398,9 @@ static s32 brcmf_get_assoc_ies(struct brcmf_cfg80211_info *cfg)
                 conn_info->req_ie = NULL;
         }
         if (resp_len) {
-                err = brcmf_dev_bufvar_get(ndev, "assoc_resp_ies",
-                                           cfg->extra_buf,
-                                           WL_ASSOC_INFO_MAX);
+                err = brcmf_fil_iovar_data_get(ifp, "assoc_resp_ies",
+                                               cfg->extra_buf,
+                                               WL_ASSOC_INFO_MAX);
                 if (err) {
                         WL_ERR("could not get assoc resp (%d)\n", err);
                         return err;
@@ -4603,15 +4424,17 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
                        struct net_device *ndev,
                        const struct brcmf_event_msg *e)
 {
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg);
         struct wiphy *wiphy = cfg_to_wiphy(cfg);
-        struct brcmf_channel_info_le channel_le;
-        struct ieee80211_channel *notify_channel;
+        struct ieee80211_channel *notify_channel = NULL;
         struct ieee80211_supported_band *band;
+        struct brcmf_bss_info_le *bi;
         u32 freq;
         s32 err = 0;
         u32 target_channel;
+        u8 *buf;
 
         WL_TRACE("Enter\n");
 
@@ -4619,11 +4442,23 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
         memcpy(profile->bssid, e->addr, ETH_ALEN);
         brcmf_update_bss_info(cfg);
 
-        brcmf_exec_dcmd(ndev, BRCMF_C_GET_CHANNEL, &channel_le,
-                        sizeof(channel_le));
+        buf = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
+        if (buf == NULL) {
+                err = -ENOMEM;
+                goto done;
+        }
+
+        /* data sent to dongle has to be little endian */
+        *(__le32 *)buf = cpu_to_le32(WL_BSS_INFO_MAX);
+        err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSS_INFO,
+                                     buf, WL_BSS_INFO_MAX);
 
-        target_channel = le32_to_cpu(channel_le.target_channel);
-        WL_CONN("Roamed to channel %d\n", target_channel);
+        if (err)
+                goto done;
+
+        bi = (struct brcmf_bss_info_le *)(buf + 4);
+        target_channel = bi->ctl_ch ? bi->ctl_ch :
+                                      CHSPEC_CHANNEL(le16_to_cpu(bi->chanspec));
 
         if (target_channel <= CH_MAX_2G_CHANNEL)
                 band = wiphy->bands[IEEE80211_BAND_2GHZ];
@@ -4633,12 +4468,14 @@ brcmf_bss_roaming_done(struct brcmf_cfg80211_info *cfg,
         freq = ieee80211_channel_to_frequency(target_channel, band->band);
         notify_channel = ieee80211_get_channel(wiphy, freq);
 
+done:
+        kfree(buf);
         cfg80211_roamed(ndev, notify_channel, (u8 *)profile->bssid,
                         conn_info->req_ie, conn_info->req_ie_len,
                         conn_info->resp_ie, conn_info->resp_ie_len, GFP_KERNEL);
         WL_CONN("Report roaming result\n");
 
-        set_bit(WL_STATUS_CONNECTED, &cfg->status);
+        set_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state);
         WL_TRACE("Exit\n");
         return err;
 }
@@ -4648,13 +4485,15 @@ brcmf_bss_connect_done(struct brcmf_cfg80211_info *cfg,
                        struct net_device *ndev, const struct brcmf_event_msg *e,
                        bool completed)
 {
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         struct brcmf_cfg80211_connect_info *conn_info = cfg_to_conn(cfg);
         s32 err = 0;
 
         WL_TRACE("Enter\n");
 
-        if (test_and_clear_bit(WL_STATUS_CONNECTING, &cfg->status)) {
+        if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTING,
+                               &ifp->vif->sme_state)) {
                 if (completed) {
                         brcmf_get_assoc_ies(cfg);
                         memcpy(profile->bssid, e->addr, ETH_ALEN);
@@ -4670,7 +4509,8 @@ brcmf_bss_connect_done(struct brcmf_cfg80211_info *cfg,
                                                     WLAN_STATUS_AUTH_TIMEOUT,
                                         GFP_KERNEL);
                 if (completed)
-                        set_bit(WL_STATUS_CONNECTED, &cfg->status);
+                        set_bit(BRCMF_VIF_STATUS_CONNECTED,
+                                &ifp->vif->sme_state);
                 WL_CONN("Report connect result - connection %s\n",
                                 completed ? "succeeded" : "failed");
         }
@@ -4722,7 +4562,8 @@ brcmf_notify_connect_status(struct brcmf_cfg80211_info *cfg,
                             struct net_device *ndev,
                             const struct brcmf_event_msg *e, void *data)
 {
-        struct brcmf_cfg80211_profile *profile = cfg->profile;
+        struct brcmf_if *ifp = netdev_priv(ndev);
+        struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
         s32 err = 0;
 
         if (cfg->conf->mode == WL_MODE_AP) {
@@ -4733,30 +4574,34 @@ brcmf_notify_connect_status(struct brcmf_cfg80211_info *cfg,
                         memcpy(profile->bssid, e->addr, ETH_ALEN);
                         wl_inform_ibss(cfg, ndev, e->addr);
                         cfg80211_ibss_joined(ndev, e->addr, GFP_KERNEL);
-                        clear_bit(WL_STATUS_CONNECTING, &cfg->status);
-                        set_bit(WL_STATUS_CONNECTED, &cfg->status);
+                        clear_bit(BRCMF_VIF_STATUS_CONNECTING,
+                                  &ifp->vif->sme_state);
+                        set_bit(BRCMF_VIF_STATUS_CONNECTED,
+                                &ifp->vif->sme_state);
                 } else
                         brcmf_bss_connect_done(cfg, ndev, e, true);
         } else if (brcmf_is_linkdown(cfg, e)) {
                 WL_CONN("Linkdown\n");
                 if (brcmf_is_ibssmode(cfg)) {
-                        clear_bit(WL_STATUS_CONNECTING, &cfg->status);
-                        if (test_and_clear_bit(WL_STATUS_CONNECTED,
-                                &cfg->status))
+                        clear_bit(BRCMF_VIF_STATUS_CONNECTING,
+                                  &ifp->vif->sme_state);
+                        if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTED,
+                                               &ifp->vif->sme_state))
                                 brcmf_link_down(cfg);
                 } else {
                         brcmf_bss_connect_done(cfg, ndev, e, false);
-                        if (test_and_clear_bit(WL_STATUS_CONNECTED,
-                                &cfg->status)) {
+                        if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTED,
+                                               &ifp->vif->sme_state)) {
                                 cfg80211_disconnected(ndev, 0, NULL, 0,
-                                        GFP_KERNEL);
+                                                      GFP_KERNEL);
                                 brcmf_link_down(cfg);
                         }
                 }
-                brcmf_init_prof(cfg->profile);
+                brcmf_init_prof(ndev_to_prof(ndev));
         } else if (brcmf_is_nonetwork(cfg, e)) {
                 if (brcmf_is_ibssmode(cfg))
-                        clear_bit(WL_STATUS_CONNECTING, &cfg->status);
+                        clear_bit(BRCMF_VIF_STATUS_CONNECTING,
+                                  &ifp->vif->sme_state);
                 else
                         brcmf_bss_connect_done(cfg, ndev, e, false);
         }
@@ -4769,12 +4614,13 @@ brcmf_notify_roaming_status(struct brcmf_cfg80211_info *cfg,
                             struct net_device *ndev,
                             const struct brcmf_event_msg *e, void *data)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
         u32 event = be32_to_cpu(e->event_type);
         u32 status = be32_to_cpu(e->status);
 
         if (event == BRCMF_E_ROAM && status == BRCMF_E_STATUS_SUCCESS) {
-                if (test_bit(WL_STATUS_CONNECTED, &cfg->status))
+                if (test_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state))
                         brcmf_bss_roaming_done(cfg, ndev, e);
                 else
                         brcmf_bss_connect_done(cfg, ndev, e, true);
@@ -4807,6 +4653,7 @@ brcmf_notify_scan_status(struct brcmf_cfg80211_info *cfg,
                          struct net_device *ndev,
                          const struct brcmf_event_msg *e, void *data)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         struct brcmf_channel_info_le channel_inform_le;
         struct brcmf_scan_results_le *bss_list_le;
         u32 len = WL_SCAN_BUF_MAX;
@@ -4821,15 +4668,16 @@ brcmf_notify_scan_status(struct brcmf_cfg80211_info *cfg,
                 return brcmf_wakeup_iscan(cfg_to_iscan(cfg));
         }
 
-        if (!test_and_clear_bit(WL_STATUS_SCANNING, &cfg->status)) {
+        if (!test_and_clear_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
                 WL_ERR("Scan complete while device not scanning\n");
                 scan_abort = true;
                 err = -EINVAL;
                 goto scan_done_out;
         }
 
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_CHANNEL, &channel_inform_le,
-                              sizeof(channel_inform_le));
+        err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_CHANNEL,
+                                     &channel_inform_le,
+                                     sizeof(channel_inform_le));
         if (err) {
                 WL_ERR("scan busy (%d)\n", err);
                 scan_abort = true;
@@ -4843,8 +4691,8 @@ brcmf_notify_scan_status(struct brcmf_cfg80211_info *cfg,
 
         memset(cfg->scan_results, 0, len);
         bss_list_le->buflen = cpu_to_le32(len);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SCAN_RESULTS,
-                              cfg->scan_results, len);
+        err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_SCAN_RESULTS,
+                                     cfg->scan_results, len);
         if (err) {
                 WL_ERR("%s Scan_results error (%d)\n", ndev->name, err);
                 err = -EINVAL;
@@ -4906,8 +4754,6 @@ static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_info *cfg)
         cfg->bss_info = NULL;
         kfree(cfg->conf);
         cfg->conf = NULL;
-        kfree(cfg->profile);
-        cfg->profile = NULL;
         kfree(cfg->scan_req_int);
         cfg->scan_req_int = NULL;
         kfree(cfg->escan_ioctl_buf);
@@ -4936,9 +4782,6 @@ static s32 brcmf_init_priv_mem(struct brcmf_cfg80211_info *cfg)
         cfg->conf = kzalloc(sizeof(*cfg->conf), GFP_KERNEL);
         if (!cfg->conf)
                 goto init_priv_mem_out;
-        cfg->profile = kzalloc(sizeof(*cfg->profile), GFP_KERNEL);
-        if (!cfg->profile)
-                goto init_priv_mem_out;
         cfg->bss_info = kzalloc(WL_BSS_INFO_MAX, GFP_KERNEL);
         if (!cfg->bss_info)
                 goto init_priv_mem_out;
@@ -5115,7 +4958,6 @@ static s32 wl_init_priv(struct brcmf_cfg80211_info *cfg)
                 return err;
         brcmf_init_escan(cfg);
         brcmf_init_conf(cfg->conf);
-        brcmf_init_prof(cfg->profile);
         brcmf_link_down(cfg);
 
         return err;
@@ -5131,12 +4973,14 @@ static void wl_deinit_priv(struct brcmf_cfg80211_info *cfg)
         brcmf_deinit_priv_mem(cfg);
 }
 
-struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct net_device *ndev,
-                                                  struct device *busdev,
-                                                  struct brcmf_pub *drvr)
+struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr)
 {
-        struct wireless_dev *wdev;
+        struct net_device *ndev = drvr->iflist[0]->ndev;
+        struct device *busdev = drvr->dev;
         struct brcmf_cfg80211_info *cfg;
+        struct wiphy *wiphy;
+        struct brcmf_cfg80211_vif *vif;
+        struct brcmf_if *ifp;
         s32 err = 0;
 
         if (!ndev) {
@@ -5144,35 +4988,45 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct net_device *ndev,
                 return NULL;
         }
 
-        wdev = brcmf_alloc_wdev(busdev);
-        if (IS_ERR(wdev)) {
+        ifp = netdev_priv(ndev);
+        wiphy = brcmf_setup_wiphy(busdev);
+        if (IS_ERR(wiphy))
                 return NULL;
-        }
 
-        wdev->iftype = brcmf_mode_to_nl80211_iftype(WL_MODE_BSS);
-        cfg = wdev_to_cfg(wdev);
-        cfg->wdev = wdev;
+        cfg = wiphy_priv(wiphy);
+        cfg->wiphy = wiphy;
         cfg->pub = drvr;
-        ndev->ieee80211_ptr = wdev;
-        SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
-        wdev->netdev = ndev;
+        INIT_LIST_HEAD(&cfg->vif_list);
+
+        vif = brcmf_alloc_vif(cfg, ndev, WL_MODE_BSS, false);
+        if (IS_ERR(vif)) {
+                wiphy_free(wiphy);
+                return NULL;
+        }
+
         err = wl_init_priv(cfg);
         if (err) {
                 WL_ERR("Failed to init iwm_priv (%d)\n", err);
                 goto cfg80211_attach_out;
         }
 
+        ifp->vif = vif;
         return cfg;
 
 cfg80211_attach_out:
-        brcmf_free_wdev(cfg);
+        brcmf_free_vif(vif);
         return NULL;
 }
 
 void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg)
 {
+        struct brcmf_cfg80211_vif *vif;
+        struct brcmf_cfg80211_vif *tmp;
+
         wl_deinit_priv(cfg);
-        brcmf_free_wdev(cfg);
+        list_for_each_entry_safe(vif, tmp, &cfg->vif_list, list) {
+                brcmf_free_vif(vif);
+        }
 }
 
 void
@@ -5186,59 +5040,20 @@ brcmf_cfg80211_event(struct net_device *ndev,
                 schedule_work(&cfg->event_work);
 }
 
-static s32 brcmf_dongle_mode(struct net_device *ndev, s32 iftype)
-{
-        s32 infra = 0;
-        s32 err = 0;
-
-        switch (iftype) {
-        case NL80211_IFTYPE_MONITOR:
-        case NL80211_IFTYPE_WDS:
-                WL_ERR("type (%d) : currently we do not support this mode\n",
-                       iftype);
-                err = -EINVAL;
-                return err;
-        case NL80211_IFTYPE_ADHOC:
-                infra = 0;
-                break;
-        case NL80211_IFTYPE_STATION:
-                infra = 1;
-                break;
-        case NL80211_IFTYPE_AP:
-                infra = 1;
-                break;
-        default:
-                err = -EINVAL;
-                WL_ERR("invalid type (%d)\n", iftype);
-                return err;
-        }
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_INFRA, &infra);
-        if (err) {
-                WL_ERR("WLC_SET_INFRA error (%d)\n", err);
-                return err;
-        }
-
-        return 0;
-}
-
 static s32 brcmf_dongle_eventmsg(struct net_device *ndev)
 {
-        /* Room for "event_msgs" + '\0' + bitvec */
-        s8 iovbuf[BRCMF_EVENTING_MASK_LEN + 12];
         s8 eventmask[BRCMF_EVENTING_MASK_LEN];
         s32 err = 0;
 
         WL_TRACE("Enter\n");
 
         /* Setup event_msgs */
-        brcmf_c_mkiovar("event_msgs", eventmask, BRCMF_EVENTING_MASK_LEN,
-                        iovbuf, sizeof(iovbuf));
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_GET_VAR, iovbuf, sizeof(iovbuf));
+        err = brcmf_fil_iovar_data_get(netdev_priv(ndev), "event_msgs",
+                                       eventmask, BRCMF_EVENTING_MASK_LEN);
         if (err) {
                 WL_ERR("Get event_msgs error (%d)\n", err);
                 goto dongle_eventmsg_out;
         }
-        memcpy(eventmask, iovbuf, BRCMF_EVENTING_MASK_LEN);
 
         setbit(eventmask, BRCMF_E_SET_SSID);
         setbit(eventmask, BRCMF_E_ROAM);
@@ -5262,9 +5077,8 @@ static s32 brcmf_dongle_eventmsg(struct net_device *ndev)
         setbit(eventmask, BRCMF_E_ESCAN_RESULT);
         setbit(eventmask, BRCMF_E_PFN_NET_FOUND);
 
-        brcmf_c_mkiovar("event_msgs", eventmask, BRCMF_EVENTING_MASK_LEN,
-                        iovbuf, sizeof(iovbuf));
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, iovbuf, sizeof(iovbuf));
+        err = brcmf_fil_iovar_data_set(netdev_priv(ndev), "event_msgs",
+                                       eventmask, BRCMF_EVENTING_MASK_LEN);
         if (err) {
                 WL_ERR("Set event_msgs error (%d)\n", err);
                 goto dongle_eventmsg_out;
@@ -5278,23 +5092,17 @@ dongle_eventmsg_out:
 static s32
 brcmf_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
 {
-        s8 iovbuf[32];
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
         __le32 roamtrigger[2];
         __le32 roam_delta[2];
-        __le32 bcn_to_le;
-        __le32 roamvar_le;
 
         /*
          * Setup timeout if Beacons are lost and roam is
          * off to report link down
          */
         if (roamvar) {
-                bcn_to_le = cpu_to_le32(bcn_timeout);
-                brcmf_c_mkiovar("bcn_timeout", (char *)&bcn_to_le,
-                        sizeof(bcn_to_le), iovbuf, sizeof(iovbuf));
-                err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR,
-                                   iovbuf, sizeof(iovbuf));
+                err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout", bcn_timeout);
                 if (err) {
                         WL_ERR("bcn_timeout error (%d)\n", err);
                         goto dongle_rom_out;
@@ -5306,10 +5114,7 @@ brcmf_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
          * to take care of roaming
          */
         WL_INFO("Internal Roaming = %s\n", roamvar ? "Off" : "On");
-        roamvar_le = cpu_to_le32(roamvar);
-        brcmf_c_mkiovar("roam_off", (char *)&roamvar_le,
-                                sizeof(roamvar_le), iovbuf, sizeof(iovbuf));
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_VAR, iovbuf, sizeof(iovbuf));
+        err = brcmf_fil_iovar_int_set(ifp, "roam_off", roamvar);
         if (err) {
                 WL_ERR("roam_off error (%d)\n", err);
                 goto dongle_rom_out;
@@ -5317,8 +5122,8 @@ brcmf_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
 
         roamtrigger[0] = cpu_to_le32(WL_ROAM_TRIGGER_LEVEL);
         roamtrigger[1] = cpu_to_le32(BRCM_BAND_ALL);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_ROAM_TRIGGER,
-                        (void *)roamtrigger, sizeof(roamtrigger));
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_ROAM_TRIGGER,
+                                     (void *)roamtrigger, sizeof(roamtrigger));
         if (err) {
                 WL_ERR("WLC_SET_ROAM_TRIGGER error (%d)\n", err);
                 goto dongle_rom_out;
@@ -5326,8 +5131,8 @@ brcmf_dongle_roam(struct net_device *ndev, u32 roamvar, u32 bcn_timeout)
 
         roam_delta[0] = cpu_to_le32(WL_ROAM_DELTA);
         roam_delta[1] = cpu_to_le32(BRCM_BAND_ALL);
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_ROAM_DELTA,
-                                (void *)roam_delta, sizeof(roam_delta));
+        err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SET_ROAM_DELTA,
+                                     (void *)roam_delta, sizeof(roam_delta));
         if (err) {
                 WL_ERR("WLC_SET_ROAM_DELTA error (%d)\n", err);
                 goto dongle_rom_out;
@@ -5341,13 +5146,11 @@ static s32
 brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
                       s32 scan_unassoc_time, s32 scan_passive_time)
 {
+        struct brcmf_if *ifp = netdev_priv(ndev);
         s32 err = 0;
-        __le32 scan_assoc_tm_le = cpu_to_le32(scan_assoc_time);
-        __le32 scan_unassoc_tm_le = cpu_to_le32(scan_unassoc_time);
-        __le32 scan_passive_tm_le = cpu_to_le32(scan_passive_time);
 
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_CHANNEL_TIME,
-                           &scan_assoc_tm_le, sizeof(scan_assoc_tm_le));
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_CHANNEL_TIME,
+                                    scan_assoc_time);
         if (err) {
                 if (err == -EOPNOTSUPP)
                         WL_INFO("Scan assoc time is not supported\n");
@@ -5355,8 +5158,8 @@ brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
                         WL_ERR("Scan assoc time error (%d)\n", err);
                 goto dongle_scantime_out;
         }
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_UNASSOC_TIME,
-                           &scan_unassoc_tm_le, sizeof(scan_unassoc_tm_le));
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_UNASSOC_TIME,
+                                    scan_unassoc_time);
         if (err) {
                 if (err == -EOPNOTSUPP)
                         WL_INFO("Scan unassoc time is not supported\n");
@@ -5365,8 +5168,8 @@ brcmf_dongle_scantime(struct net_device *ndev, s32 scan_assoc_time,
                 goto dongle_scantime_out;
         }
 
-        err = brcmf_exec_dcmd(ndev, BRCMF_C_SET_SCAN_PASSIVE_TIME,
-                           &scan_passive_tm_le, sizeof(scan_passive_tm_le));
+        err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_PASSIVE_TIME,
+                                    scan_passive_time);
         if (err) {
                 if (err == -EOPNOTSUPP)
                         WL_INFO("Scan passive time is not supported\n");
@@ -5381,13 +5184,14 @@ dongle_scantime_out:
 
 static s32 wl_update_wiphybands(struct brcmf_cfg80211_info *cfg)
 {
+        struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
         struct wiphy *wiphy;
         s32 phy_list;
         s8 phy;
         s32 err = 0;
 
-        err = brcmf_exec_dcmd(cfg_to_ndev(cfg), BRCM_GET_PHYLIST,
-                              &phy_list, sizeof(phy_list));
+        err = brcmf_fil_cmd_data_get(ifp, BRCM_GET_PHYLIST,
+                                     &phy_list, sizeof(phy_list));
         if (err) {
                 WL_ERR("error (%d)\n", err);
                 return err;
@@ -5429,7 +5233,8 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
                 goto default_conf_out;
 
         power_mode = cfg->pwr_save ? PM_FAST : PM_OFF;
-        err = brcmf_exec_dcmd_u32(ndev, BRCMF_C_SET_PM, &power_mode);
+        err = brcmf_fil_cmd_int_set(netdev_priv(ndev), BRCMF_C_SET_PM,
+                                    power_mode);
         if (err)
                 goto default_conf_out;
         WL_INFO("power save set to %s\n",
@@ -5439,7 +5244,8 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
                                 WL_BEACON_TIMEOUT);
         if (err)
                 goto default_conf_out;
-        err = brcmf_dongle_mode(ndev, wdev->iftype);
+        err = brcmf_cfg80211_change_iface(wdev->wiphy, ndev, wdev->iftype,
+                                          NULL, NULL);
         if (err && err != -EINPROGRESS)
                 goto default_conf_out;
         err = brcmf_dongle_probecap(cfg);
@@ -5456,47 +5262,12 @@ default_conf_out:
 
 }
 
-static int brcmf_debugfs_add_netdev_params(struct brcmf_cfg80211_info *cfg)
-{
-        char buf[10+IFNAMSIZ];
-        struct dentry *fd;
-        s32 err = 0;
-
-        sprintf(buf, "netdev:%s", cfg_to_ndev(cfg)->name);
-        cfg->debugfsdir = debugfs_create_dir(buf,
-                                        cfg_to_wiphy(cfg)->debugfsdir);
-
-        fd = debugfs_create_u16("beacon_int", S_IRUGO, cfg->debugfsdir,
-                (u16 *)&cfg->profile->beacon_interval);
-        if (!fd) {
-                err = -ENOMEM;
-                goto err_out;
-        }
-
-        fd = debugfs_create_u8("dtim_period", S_IRUGO, cfg->debugfsdir,
-                (u8 *)&cfg->profile->dtim_period);
-        if (!fd) {
-                err = -ENOMEM;
-                goto err_out;
-        }
-
-err_out:
-        return err;
-}
-
-static void brcmf_debugfs_remove_netdev(struct brcmf_cfg80211_info *cfg)
-{
-        debugfs_remove_recursive(cfg->debugfsdir);
-        cfg->debugfsdir = NULL;
-}
-
 static s32 __brcmf_cfg80211_up(struct brcmf_cfg80211_info *cfg)
 {
+        struct brcmf_if *ifp = netdev_priv(cfg_to_ndev(cfg));
         s32 err = 0;
 
-        set_bit(WL_STATUS_READY, &cfg->status);
-
-        brcmf_debugfs_add_netdev_params(cfg);
+        set_bit(BRCMF_VIF_STATUS_READY, &ifp->vif->sme_state);
 
         err = brcmf_config_dongle(cfg);
         if (err)
@@ -5509,13 +5280,16 @@ static s32 __brcmf_cfg80211_up(struct brcmf_cfg80211_info *cfg)
 
 static s32 __brcmf_cfg80211_down(struct brcmf_cfg80211_info *cfg)
 {
+        struct net_device *ndev = cfg_to_ndev(cfg);
+        struct brcmf_if *ifp = netdev_priv(ndev);
+
         /*
          * While going down, if associated with AP disassociate
          * from AP to save power
          */
-        if ((test_bit(WL_STATUS_CONNECTED, &cfg->status) ||
-             test_bit(WL_STATUS_CONNECTING, &cfg->status)) &&
-             test_bit(WL_STATUS_READY, &cfg->status)) {
+        if ((test_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state) ||
+             test_bit(BRCMF_VIF_STATUS_CONNECTING, &ifp->vif->sme_state)) &&
+             check_vif_up(ifp->vif)) {
                 WL_INFO("Disassociating from AP");
                 brcmf_link_down(cfg);
 
@@ -5527,9 +5301,7 @@ static s32 __brcmf_cfg80211_down(struct brcmf_cfg80211_info *cfg)
         }
 
         brcmf_abort_scanning(cfg);
-        clear_bit(WL_STATUS_READY, &cfg->status);
-
-        brcmf_debugfs_remove_netdev(cfg);
+        clear_bit(BRCMF_VIF_STATUS_READY, &ifp->vif->sme_state);
 
         return 0;
 }
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
index 71ced174748a..1dd96f148ef7 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
@@ -127,15 +127,15 @@ do {								\
 #define WL_AUTH_SHARED_KEY              1       /* d11 shared authentication */
 #define IE_MAX_LEN                      512
 
-/* dongle status */
-enum wl_status {
-        WL_STATUS_READY,
-        WL_STATUS_SCANNING,
-        WL_STATUS_SCAN_ABORTING,
-        WL_STATUS_CONNECTING,
-        WL_STATUS_CONNECTED,
-        WL_STATUS_AP_CREATING,
-        WL_STATUS_AP_CREATED
+/**
+ * enum brcmf_scan_status - dongle scan status
+ *
+ * @BRCMF_SCAN_STATUS_BUSY: scanning in progress on dongle.
+ * @BRCMF_SCAN_STATUS_ABORT: scan being aborted on dongle.
+ */
+enum brcmf_scan_status {
+        BRCMF_SCAN_STATUS_BUSY,
+        BRCMF_SCAN_STATUS_ABORT,
 };
 
 /* wi-fi mode */
@@ -145,19 +145,6 @@ enum wl_mode {
         WL_MODE_AP
 };
 
-/* dongle profile list */
-enum wl_prof_list {
-        WL_PROF_MODE,
-        WL_PROF_SSID,
-        WL_PROF_SEC,
-        WL_PROF_IBSS,
-        WL_PROF_BAND,
-        WL_PROF_BSSID,
-        WL_PROF_ACT,
-        WL_PROF_BEACONINT,
-        WL_PROF_DTIMPERIOD
-};
-
 /* dongle iscan state */
 enum wl_iscan_state {
         WL_ISCAN_STATE_IDLE,
@@ -214,25 +201,73 @@ struct brcmf_cfg80211_security {
         u32 wpa_auth;
 };
 
-/* ibss information for currently joined ibss network */
-struct brcmf_cfg80211_ibss {
-        u8 beacon_interval;     /* in millisecond */
-        u8 atim;                /* in millisecond */
-        s8 join_only;
-        u8 band;
-        u8 channel;
-};
-
-/* dongle profile */
+/**
+ * struct brcmf_cfg80211_profile - profile information.
+ *
+ * @ssid: ssid of associated/associating ap.
+ * @bssid: bssid of joined/joining ibss.
+ * @sec: security information.
+ */
 struct brcmf_cfg80211_profile {
-        u32 mode;
         struct brcmf_ssid ssid;
         u8 bssid[ETH_ALEN];
-        u16 beacon_interval;
-        u8 dtim_period;
         struct brcmf_cfg80211_security sec;
-        struct brcmf_cfg80211_ibss ibss;
-        s32 band;
+};
+
+/**
+ * enum brcmf_vif_status - bit indices for vif status.
+ *
+ * @BRCMF_VIF_STATUS_READY: ready for operation.
+ * @BRCMF_VIF_STATUS_CONNECTING: connect/join in progress.
+ * @BRCMF_VIF_STATUS_CONNECTED: connected/joined succesfully.
+ * @BRCMF_VIF_STATUS_AP_CREATING: interface configured for AP operation.
+ * @BRCMF_VIF_STATUS_AP_CREATED: AP operation started.
+ */
+enum brcmf_vif_status {
+        BRCMF_VIF_STATUS_READY,
+        BRCMF_VIF_STATUS_CONNECTING,
+        BRCMF_VIF_STATUS_CONNECTED,
+        BRCMF_VIF_STATUS_AP_CREATING,
+        BRCMF_VIF_STATUS_AP_CREATED
+};
+
+/**
+ * struct vif_saved_ie - holds saved IEs for a virtual interface.
+ *
+ * @probe_res_ie: IE info for probe response.
+ * @beacon_ie: IE info for beacon frame.
+ * @probe_res_ie_len: IE info length for probe response.
+ * @beacon_ie_len: IE info length for beacon frame.
+ */
+struct vif_saved_ie {
+        u8  probe_res_ie[IE_MAX_LEN];
+        u8  beacon_ie[IE_MAX_LEN];
+        u32 probe_res_ie_len;
+        u32 beacon_ie_len;
+};
+
+/**
+ * struct brcmf_cfg80211_vif - virtual interface specific information.
+ *
+ * @ifp: lower layer interface pointer
+ * @wdev: wireless device.
+ * @profile: profile information.
+ * @mode: operating mode.
+ * @roam_off: roaming state.
+ * @sme_state: SME state using enum brcmf_vif_status bits.
+ * @pm_block: power-management blocked.
+ * @list: linked list.
+ */
+struct brcmf_cfg80211_vif {
+        struct brcmf_if *ifp;
+        struct wireless_dev wdev;
+        struct brcmf_cfg80211_profile profile;
+        s32 mode;
+        s32 roam_off;
+        unsigned long sme_state;
+        bool pm_block;
+        struct vif_saved_ie saved_ie;
+        struct list_head list;
 };
 
 /* dongle iscan event loop */
@@ -383,7 +418,7 @@ struct brcmf_pno_scanresults_le {
 /**
  * struct brcmf_cfg80211_info - dongle private data of cfg80211 interface
  *
- * @wdev: representing wl cfg80211 device.
+ * @wiphy: wiphy object for cfg80211 interface.
  * @conf: dongle configuration.
  * @scan_request: cfg80211 scan request object.
  * @el: main event loop.
@@ -395,12 +430,11 @@ struct brcmf_pno_scanresults_le {
  * @scan_req_int: internal scan request object.
  * @bss_info: bss information for cfg80211 layer.
  * @ie: information element object for internal purpose.
- * @profile: holding dongle profile.
  * @iscan: iscan controller information.
  * @conn_info: association info.
  * @pmk_list: wpa2 pmk list.
  * @event_work: event handler work struct.
- * @status: current dongle status.
+ * @scan_status: scan activity on the dongle.
  * @pub: common driver information.
  * @channel: current channel.
  * @iscan_on: iscan on/off switch.
@@ -422,10 +456,11 @@ struct brcmf_pno_scanresults_le {
  * @escan_timeout_work: scan timeout worker.
  * @escan_ioctl_buf: dongle command buffer for escan commands.
  * @ap_info: host ap information.
- * @ci: used to link this structure to netdev private data.
+ * @vif_list: linked list of vif instances.
+ * @vif_cnt: number of vif instances.
  */
 struct brcmf_cfg80211_info {
-        struct wireless_dev *wdev;
+        struct wiphy *wiphy;
         struct brcmf_cfg80211_conf *conf;
         struct cfg80211_scan_request *scan_request;
         struct brcmf_cfg80211_event_loop el;
@@ -437,12 +472,11 @@ struct brcmf_cfg80211_info {
         struct brcmf_cfg80211_scan_req *scan_req_int;
         struct wl_cfg80211_bss_info *bss_info;
         struct brcmf_cfg80211_ie ie;
-        struct brcmf_cfg80211_profile *profile;
         struct brcmf_cfg80211_iscan_ctrl *iscan;
         struct brcmf_cfg80211_connect_info conn_info;
         struct brcmf_cfg80211_pmk_list *pmk_list;
         struct work_struct event_work;
-        unsigned long status;
+        unsigned long scan_status;
         struct brcmf_pub *pub;
         u32 channel;
         bool iscan_on;
@@ -464,11 +498,13 @@ struct brcmf_cfg80211_info {
         struct work_struct escan_timeout_work;
         u8 *escan_ioctl_buf;
         struct ap_info *ap_info;
+        struct list_head vif_list;
+        u8 vif_cnt;
 };
 
-static inline struct wiphy *cfg_to_wiphy(struct brcmf_cfg80211_info *w)
+static inline struct wiphy *cfg_to_wiphy(struct brcmf_cfg80211_info *cfg)
 {
-        return w->wdev->wiphy;
+        return cfg->wiphy;
 }
 
 static inline struct brcmf_cfg80211_info *wiphy_to_cfg(struct wiphy *w)
@@ -481,9 +517,12 @@ static inline struct brcmf_cfg80211_info *wdev_to_cfg(struct wireless_dev *wd)
         return (struct brcmf_cfg80211_info *)(wdev_priv(wd));
 }
 
-static inline struct net_device *cfg_to_ndev(struct brcmf_cfg80211_info *cfg)
+static inline
+struct net_device *cfg_to_ndev(struct brcmf_cfg80211_info *cfg)
 {
-        return cfg->wdev->netdev;
+        struct brcmf_cfg80211_vif *vif;
+        vif = list_first_entry(&cfg->vif_list, struct brcmf_cfg80211_vif, list);
+        return vif->wdev.netdev;
 }
 
 static inline struct brcmf_cfg80211_info *ndev_to_cfg(struct net_device *ndev)
@@ -491,6 +530,12 @@ static inline struct brcmf_cfg80211_info *ndev_to_cfg(struct net_device *ndev)
         return wdev_to_cfg(ndev->ieee80211_ptr);
 }
 
+static inline struct brcmf_cfg80211_profile *ndev_to_prof(struct net_device *nd)
+{
+        struct brcmf_if *ifp = netdev_priv(nd);
+        return &ifp->vif->profile;
+}
+
 #define iscan_to_cfg(i) ((struct brcmf_cfg80211_info *)(i->data))
 #define cfg_to_iscan(w) (w->iscan)
 
@@ -500,9 +545,7 @@ brcmf_cfg80211_connect_info *cfg_to_conn(struct brcmf_cfg80211_info *cfg)
         return &cfg->conn_info;
 }
 
-struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct net_device *ndev,
-                                                  struct device *busdev,
-                                                  struct brcmf_pub *drvr);
+struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr);
 void brcmf_cfg80211_detach(struct brcmf_cfg80211_info *cfg);
 
 /* event handler from dongle */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
index b89f1272b93f..de96290f5ccd 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
@@ -692,7 +692,7 @@ void ai_pci_up(struct si_pub *sih)
         sii = container_of(sih, struct si_info, pub);
 
         if (sii->icbus->hosttype == BCMA_HOSTTYPE_PCI)
-                bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, true);
+                bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci[0], true);
 }
 
 /* Unconfigure and/or apply various WARs when going down */
@@ -703,7 +703,7 @@ void ai_pci_down(struct si_pub *sih)
         sii = container_of(sih, struct si_info, pub);
 
         if (sii->icbus->hosttype == BCMA_HOSTTYPE_PCI)
-                bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci, false);
+                bcma_core_pci_extend_L1timer(&sii->icbus->drv_pci[0], false);
 }
 
 /* Enable BT-COEX & Ex-PA for 4313 */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 75086b37c817..565c15abbed5 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -5077,7 +5077,7 @@ static int brcms_b_up_prep(struct brcms_hardware *wlc_hw)
          * Configure pci/pcmcia here instead of in brcms_c_attach()
          * to allow mfg hotswap:  down, hotswap (chip power cycle), up.
          */
-        bcma_core_pci_irq_ctl(&wlc_hw->d11core->bus->drv_pci, wlc_hw->d11core,
+        bcma_core_pci_irq_ctl(&wlc_hw->d11core->bus->drv_pci[0], wlc_hw->d11core,
                               true);
 
         /*
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index 935120fc8c93..768bf612533e 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -10472,7 +10472,7 @@ static void ipw_handle_promiscuous_tx(struct ipw_priv *priv,
                 } else
                         len = src->len;
 
-                dst = alloc_skb(len + sizeof(*rt_hdr), GFP_ATOMIC);
+                dst = alloc_skb(len + sizeof(*rt_hdr) + sizeof(u16)*2, GFP_ATOMIC);
                 if (!dst)
                         continue;
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/devices.c b/drivers/net/wireless/iwlwifi/dvm/devices.c
index 349c205d5f62..da5862064195 100644
--- a/drivers/net/wireless/iwlwifi/dvm/devices.c
+++ b/drivers/net/wireless/iwlwifi/dvm/devices.c
@@ -518,7 +518,7 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
          * See iwlagn_mac_channel_switch.
          */
         struct iwl_rxon_context *ctx = &priv->contexts[IWL_RXON_CTX_BSS];
-        struct iwl6000_channel_switch_cmd cmd;
+        struct iwl6000_channel_switch_cmd *cmd;
         u32 switch_time_in_usec, ucode_switch_time;
         u16 ch;
         u32 tsf_low;
@@ -527,18 +527,25 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
         struct ieee80211_vif *vif = ctx->vif;
         struct iwl_host_cmd hcmd = {
                 .id = REPLY_CHANNEL_SWITCH,
-                .len = { sizeof(cmd), },
+                .len = { sizeof(*cmd), },
                 .flags = CMD_SYNC,
-                .data = { &cmd, },
+                .dataflags[0] = IWL_HCMD_DFL_NOCOPY,
         };
+        int err;
 
-        cmd.band = priv->band == IEEE80211_BAND_2GHZ;
+        cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+        if (!cmd)
+                return -ENOMEM;
+
+        hcmd.data[0] = cmd;
+
+        cmd->band = priv->band == IEEE80211_BAND_2GHZ;
         ch = ch_switch->channel->hw_value;
         IWL_DEBUG_11H(priv, "channel switch from %u to %u\n",
                       ctx->active.channel, ch);
-        cmd.channel = cpu_to_le16(ch);
-        cmd.rxon_flags = ctx->staging.flags;
-        cmd.rxon_filter_flags = ctx->staging.filter_flags;
+        cmd->channel = cpu_to_le16(ch);
+        cmd->rxon_flags = ctx->staging.flags;
+        cmd->rxon_filter_flags = ctx->staging.filter_flags;
         switch_count = ch_switch->count;
         tsf_low = ch_switch->timestamp & 0x0ffffffff;
         /*
@@ -554,23 +561,25 @@ static int iwl6000_hw_channel_switch(struct iwl_priv *priv,
                         switch_count = 0;
         }
         if (switch_count <= 1)
-                cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+                cmd->switch_time = cpu_to_le32(priv->ucode_beacon_time);
         else {
                 switch_time_in_usec =
                         vif->bss_conf.beacon_int * switch_count * TIME_UNIT;
                 ucode_switch_time = iwl_usecs_to_beacons(priv,
                                                          switch_time_in_usec,
                                                          beacon_interval);
-                cmd.switch_time = iwl_add_beacon_time(priv,
-                                                      priv->ucode_beacon_time,
-                                                      ucode_switch_time,
-                                                      beacon_interval);
+                cmd->switch_time = iwl_add_beacon_time(priv,
+                                                       priv->ucode_beacon_time,
+                                                       ucode_switch_time,
+                                                       beacon_interval);
         }
         IWL_DEBUG_11H(priv, "uCode time for the switch is 0x%x\n",
-                      cmd.switch_time);
-        cmd.expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
+                      cmd->switch_time);
+        cmd->expect_beacon = ch_switch->channel->flags & IEEE80211_CHAN_RADAR;
 
-        return iwl_dvm_send_cmd(priv, &hcmd);
+        err = iwl_dvm_send_cmd(priv, &hcmd);
+        kfree(cmd);
+        return err;
 }
 
 struct iwl_lib_ops iwl6000_lib = {
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/iwlwifi/dvm/main.c
index 7ff3f1430678..475df45c8320 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/iwlwifi/dvm/main.c
@@ -1191,8 +1191,6 @@ static void iwl_option_config(struct iwl_priv *priv)
 
 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 &&
@@ -1208,8 +1206,6 @@ static int iwl_eeprom_init_hw_params(struct iwl_priv *priv)
 
         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)
@@ -1334,6 +1330,9 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
         /* Configure transport layer */
         iwl_trans_configure(priv->trans, &trans_cfg);
 
+        trans->rx_mpdu_cmd = REPLY_RX_MPDU_CMD;
+        trans->rx_mpdu_cmd_hdr_size = sizeof(struct iwl_rx_mpdu_res_start);
+
         /* At this point both hw and priv are allocated. */
 
         SET_IEEE80211_DEV(priv->hw, priv->trans->dev);
@@ -2152,8 +2151,6 @@ static int __init iwl_init(void)
 {
 
         int ret;
-        pr_info(DRV_DESCRIPTION ", " DRV_VERSION "\n");
-        pr_info(DRV_COPYRIGHT "\n");
 
         ret = iwlagn_rate_control_register();
         if (ret) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
index 59a5f78402fc..678717bf62eb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.h
@@ -25,6 +25,39 @@
  *****************************************************************************/
 
 #if !defined(__IWLWIFI_DEVICE_TRACE) || defined(TRACE_HEADER_MULTI_READ)
+#include <linux/skbuff.h>
+#include <linux/ieee80211.h>
+#include <net/cfg80211.h>
+#include "iwl-trans.h"
+#if !defined(__IWLWIFI_DEVICE_TRACE)
+static inline bool iwl_trace_data(struct sk_buff *skb)
+{
+        struct ieee80211_hdr *hdr = (void *)skb->data;
+
+        if (ieee80211_is_data(hdr->frame_control))
+                return skb->protocol != cpu_to_be16(ETH_P_PAE);
+        return false;
+}
+
+static inline size_t iwl_rx_trace_len(const struct iwl_trans *trans,
+                                      void *rxbuf, size_t len)
+{
+        struct iwl_cmd_header *cmd = (void *)((u8 *)rxbuf + sizeof(__le32));
+        struct ieee80211_hdr *hdr;
+
+        if (cmd->cmd != trans->rx_mpdu_cmd)
+                return len;
+
+        hdr = (void *)((u8 *)cmd + sizeof(struct iwl_cmd_header) +
+                        trans->rx_mpdu_cmd_hdr_size);
+        if (!ieee80211_is_data(hdr->frame_control))
+                return len;
+        /* maybe try to identify EAPOL frames? */
+        return sizeof(__le32) + sizeof(*cmd) + trans->rx_mpdu_cmd_hdr_size +
+                ieee80211_hdrlen(hdr->frame_control);
+}
+#endif
+
 #define __IWLWIFI_DEVICE_TRACE
 
 #include <linux/tracepoint.h>
@@ -235,6 +268,48 @@ TRACE_EVENT(iwlwifi_dbg,
 );
 
 #undef TRACE_SYSTEM
+#define TRACE_SYSTEM iwlwifi_data
+
+TRACE_EVENT(iwlwifi_dev_tx_data,
+        TP_PROTO(const struct device *dev,
+                 struct sk_buff *skb,
+                 void *data, size_t data_len),
+        TP_ARGS(dev, skb, data, data_len),
+        TP_STRUCT__entry(
+                DEV_ENTRY
+
+                __dynamic_array(u8, data, iwl_trace_data(skb) ? data_len : 0)
+        ),
+        TP_fast_assign(
+                DEV_ASSIGN;
+                if (iwl_trace_data(skb))
+                        memcpy(__get_dynamic_array(data), data, data_len);
+        ),
+        TP_printk("[%s] TX frame data", __get_str(dev))
+);
+
+TRACE_EVENT(iwlwifi_dev_rx_data,
+        TP_PROTO(const struct device *dev,
+                 const struct iwl_trans *trans,
+                 void *rxbuf, size_t len),
+        TP_ARGS(dev, trans, rxbuf, len),
+        TP_STRUCT__entry(
+                DEV_ENTRY
+
+                __dynamic_array(u8, data,
+                                len - iwl_rx_trace_len(trans, rxbuf, len))
+        ),
+        TP_fast_assign(
+                size_t offs = iwl_rx_trace_len(trans, rxbuf, len);
+                DEV_ASSIGN;
+                if (offs < len)
+                        memcpy(__get_dynamic_array(data),
+                               ((u8 *)rxbuf) + offs, len - offs);
+        ),
+        TP_printk("[%s] TX frame data", __get_str(dev))
+);
+
+#undef TRACE_SYSTEM
 #define TRACE_SYSTEM iwlwifi
 
 TRACE_EVENT(iwlwifi_dev_hcmd,
@@ -270,25 +345,28 @@ TRACE_EVENT(iwlwifi_dev_hcmd,
 );
 
 TRACE_EVENT(iwlwifi_dev_rx,
-        TP_PROTO(const struct device *dev, void *rxbuf, size_t len),
-        TP_ARGS(dev, rxbuf, len),
+        TP_PROTO(const struct device *dev, const struct iwl_trans *trans,
+                 void *rxbuf, size_t len),
+        TP_ARGS(dev, trans, rxbuf, len),
         TP_STRUCT__entry(
                 DEV_ENTRY
-                __dynamic_array(u8, rxbuf, len)
+                __dynamic_array(u8, rxbuf, iwl_rx_trace_len(trans, rxbuf, len))
         ),
         TP_fast_assign(
                 DEV_ASSIGN;
-                memcpy(__get_dynamic_array(rxbuf), rxbuf, len);
+                memcpy(__get_dynamic_array(rxbuf), rxbuf,
+                       iwl_rx_trace_len(trans, rxbuf, len));
         ),
         TP_printk("[%s] RX cmd %#.2x",
                   __get_str(dev), ((u8 *)__get_dynamic_array(rxbuf))[4])
 );
 
 TRACE_EVENT(iwlwifi_dev_tx,
-        TP_PROTO(const struct device *dev, void *tfd, size_t tfdlen,
+        TP_PROTO(const struct device *dev, struct sk_buff *skb,
+                 void *tfd, size_t tfdlen,
                  void *buf0, size_t buf0_len,
                  void *buf1, size_t buf1_len),
-        TP_ARGS(dev, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
+        TP_ARGS(dev, skb, tfd, tfdlen, buf0, buf0_len, buf1, buf1_len),
         TP_STRUCT__entry(
                 DEV_ENTRY
 
@@ -301,14 +379,15 @@ TRACE_EVENT(iwlwifi_dev_tx,
                  * for the possible padding).
                  */
                 __dynamic_array(u8, buf0, buf0_len)
-                __dynamic_array(u8, buf1, buf1_len)
+                __dynamic_array(u8, buf1, iwl_trace_data(skb) ? 0 : buf1_len)
         ),
         TP_fast_assign(
                 DEV_ASSIGN;
                 __entry->framelen = buf0_len + buf1_len;
                 memcpy(__get_dynamic_array(tfd), tfd, tfdlen);
                 memcpy(__get_dynamic_array(buf0), buf0, buf0_len);
-                memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
+                if (!iwl_trace_data(skb))
+                        memcpy(__get_dynamic_array(buf1), buf1, buf1_len);
         ),
         TP_printk("[%s] TX %.2x (%zu bytes)",
                   __get_str(dev), ((u8 *)__get_dynamic_array(buf0))[0],
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 3dfebfb8434f..54c41b44bffe 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -327,11 +327,11 @@ u32 iwl_read_targ_mem(struct iwl_trans *trans, u32 addr)
 EXPORT_SYMBOL_GPL(iwl_read_targ_mem);
 
 int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
-                               void *buf, int dwords)
+                               const void *buf, int dwords)
 {
         unsigned long flags;
         int offs, result = 0;
-        u32 *vals = buf;
+        const u32 *vals = buf;
 
         spin_lock_irqsave(&trans->reg_lock, flags);
         if (likely(iwl_grab_nic_access(trans))) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
index 50d3819739d1..e1aa69f66de6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -87,7 +87,7 @@ void _iwl_read_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
         } while (0)
 
 int _iwl_write_targ_mem_dwords(struct iwl_trans *trans, u32 addr,
-                               void *buf, int dwords);
+                               const 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-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 9253ef1dba72..c3a4bb41e533 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -213,6 +213,9 @@
 #define SCD_CONTEXT_QUEUE_OFFSET(x)\
         (SCD_CONTEXT_MEM_LOWER_BOUND + ((x) * 8))
 
+#define SCD_TX_STTS_QUEUE_OFFSET(x)\
+        (SCD_TX_STTS_MEM_LOWER_BOUND + ((x) * 16))
+
 #define SCD_TRANS_TBL_OFFSET_QUEUE(x) \
         ((SCD_TRANS_TBL_MEM_LOWER_BOUND + ((x) * 2)) & 0xfffc)
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index ff1154232885..f75ea6d73ffc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -444,6 +444,10 @@ enum iwl_trans_state {
  * @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.
+ * @rx_mpdu_cmd: MPDU RX command ID, must be assigned by opmode before
+ *      starting the firmware, used for tracing
+ * @rx_mpdu_cmd_hdr_size: used for tracing, amount of data before the
+ *      start of the 802.11 header in the @rx_mpdu_cmd
  */
 struct iwl_trans {
         const struct iwl_trans_ops *ops;
@@ -457,6 +461,8 @@ struct iwl_trans {
         u32 hw_id;
         char hw_id_str[52];
 
+        u8 rx_mpdu_cmd, rx_mpdu_cmd_hdr_size;
+
         bool pm_support;
 
         wait_queue_head_t wait_command_queue;
@@ -516,6 +522,8 @@ static inline int iwl_trans_start_fw(struct iwl_trans *trans,
 {
         might_sleep();
 
+        WARN_ON_ONCE(!trans->rx_mpdu_cmd);
+
         return trans->ops->start_fw(trans, fw);
 }
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index 17c8e5d82681..137af4c46a6c 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -411,7 +411,8 @@ static void iwl_rx_handle_rxbuf(struct iwl_trans *trans,
 
                 len = le32_to_cpu(pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
                 len += sizeof(u32); /* account for status word */
-                trace_iwlwifi_dev_rx(trans->dev, pkt, len);
+                trace_iwlwifi_dev_rx(trans->dev, trans, pkt, len);
+                trace_iwlwifi_dev_rx_data(trans->dev, trans, pkt, len);
 
                 /* Reclaim a command buffer only if this packet is a response
                  *   to a (driver-originated) command.
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index fe0fffd04304..f95d88df7772 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -300,7 +300,7 @@ static void iwl_trans_pcie_queue_stuck_timer(unsigned long data)
         struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
         struct iwl_trans *trans = iwl_trans_pcie_get_trans(trans_pcie);
         u32 scd_sram_addr = trans_pcie->scd_base_addr +
-                SCD_TX_STTS_MEM_LOWER_BOUND + (16 * txq->q.id);
+                                SCD_TX_STTS_QUEUE_OFFSET(txq->q.id);
         u8 buf[16];
         int i;
 
@@ -1385,11 +1385,13 @@ static int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
         dma_sync_single_for_device(trans->dev, txcmd_phys, firstlen,
                                    DMA_BIDIRECTIONAL);
 
-        trace_iwlwifi_dev_tx(trans->dev,
+        trace_iwlwifi_dev_tx(trans->dev, skb,
                              &txq->tfds[txq->q.write_ptr],
                              sizeof(struct iwl_tfd),
                              &dev_cmd->hdr, firstlen,
                              skb->data + hdr_len, secondlen);
+        trace_iwlwifi_dev_tx_data(trans->dev, skb,
+                                  skb->data + hdr_len, secondlen);
 
         /* start timer if queue currently empty */
         if (txq->need_update && q->read_ptr == q->write_ptr &&
@@ -1514,14 +1516,13 @@ static void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
         struct iwl_tx_queue *txq = &trans_pcie->txq[txq_id];
         /* n_bd is usually 256 => n_bd - 1 = 0xff */
         int tfd_num = ssn & (txq->q.n_bd - 1);
-        int freed = 0;
 
         spin_lock(&txq->lock);
 
         if (txq->q.read_ptr != tfd_num) {
                 IWL_DEBUG_TX_REPLY(trans, "[Q %d] %d -> %d (%d)\n",
                                    txq_id, txq->q.read_ptr, tfd_num, ssn);
-                freed = iwl_tx_queue_reclaim(trans, txq_id, tfd_num, skbs);
+                iwl_tx_queue_reclaim(trans, txq_id, tfd_num, skbs);
                 if (iwl_queue_space(&txq->q) > txq->q.low_mark)
                         iwl_wake_queue(trans, txq);
         }
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 105e3af3c621..db3efbb84d92 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -480,21 +480,20 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
 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;
+        u32 stts_addr = trans_pcie->scd_base_addr +
+                        SCD_TX_STTS_QUEUE_OFFSET(txq_id);
+        static const u32 zero_val[4] = {};
 
         if (!test_and_clear_bit(txq_id, trans_pcie->queue_used)) {
                 WARN_ONCE(1, "queue %d not used", txq_id);
                 return;
         }
 
-        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_txq_set_inactive(trans, txq_id);
 
-        WARN_ONCE(rd_ptr != wr_ptr, "queue %d isn't empty: [%d,%d]",
-                  txq_id, rd_ptr, wr_ptr);
+        _iwl_write_targ_mem_dwords(trans, stts_addr,
+                                   zero_val, ARRAY_SIZE(zero_val));
 
-        iwl_txq_set_inactive(trans, txq_id);
         IWL_DEBUG_TX_QUEUES(trans, "Deactivate queue %d\n", txq_id);
 }
 
@@ -549,7 +548,10 @@ static int iwl_enqueue_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
          * allocated into separate TFDs, then we will need to
          * increase the size of the buffers.
          */
-        if (WARN_ON(copy_size > TFD_MAX_PAYLOAD_SIZE))
+        if (WARN(copy_size > TFD_MAX_PAYLOAD_SIZE,
+                 "Command %s (%#x) is too large (%d bytes)\n",
+                 trans_pcie_get_cmd_string(trans_pcie, cmd->id),
+                 cmd->id, copy_size))
                 return -EINVAL;
 
         spin_lock_bh(&txq->lock);
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index 9402b93b9a36..4a97acd170f7 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -58,8 +58,7 @@ mwifiex_11n_dispatch_pkt(struct mwifiex_private *priv,
                         if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
                                 mwifiex_handle_uap_rx_forward(priv, rx_tmp_ptr);
                         else
-                                mwifiex_process_rx_packet(priv->adapter,
-                                                          rx_tmp_ptr);
+                                mwifiex_process_rx_packet(priv, rx_tmp_ptr);
                 }
         }
 
@@ -106,7 +105,7 @@ mwifiex_11n_scan_and_dispatch(struct mwifiex_private *priv,
                 if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
                         mwifiex_handle_uap_rx_forward(priv, rx_tmp_ptr);
                 else
-                        mwifiex_process_rx_packet(priv->adapter, rx_tmp_ptr);
+                        mwifiex_process_rx_packet(priv, rx_tmp_ptr);
         }
 
         spin_lock_irqsave(&priv->rx_pkt_lock, flags);
@@ -442,8 +441,7 @@ int mwifiex_11n_rx_reorder_pkt(struct mwifiex_private *priv,
                         if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
                                 mwifiex_handle_uap_rx_forward(priv, payload);
                         else
-                                mwifiex_process_rx_packet(priv->adapter,
-                                                          payload);
+                                mwifiex_process_rx_packet(priv, payload);
                 }
                 return 0;
         }
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index 38a58713de6a..fdb1eb861021 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -471,13 +471,13 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
                         flag = 1;
                         first_chan = (u32) ch->hw_value;
                         next_chan = first_chan;
-                        max_pwr = ch->max_reg_power;
+                        max_pwr = ch->max_power;
                         no_of_parsed_chan = 1;
                         continue;
                 }
 
                 if (ch->hw_value == next_chan + 1 &&
-                    ch->max_reg_power == max_pwr) {
+                    ch->max_power == max_pwr) {
                         next_chan++;
                         no_of_parsed_chan++;
                 } else {
@@ -488,7 +488,7 @@ static int mwifiex_send_domain_info_cmd_fw(struct wiphy *wiphy)
                         no_of_triplet++;
                         first_chan = (u32) ch->hw_value;
                         next_chan = first_chan;
-                        max_pwr = ch->max_reg_power;
+                        max_pwr = ch->max_power;
                         no_of_parsed_chan = 1;
                 }
         }
@@ -1819,13 +1819,17 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 
         wiphy_dbg(wiphy, "info: received scan request on %s\n", dev->name);
 
-        if (atomic_read(&priv->wmm.tx_pkts_queued) >=
+        if ((request->flags & NL80211_SCAN_FLAG_LOW_PRIORITY) &&
+            atomic_read(&priv->wmm.tx_pkts_queued) >=
             MWIFIEX_MIN_TX_PENDING_TO_CANCEL_SCAN) {
                 dev_dbg(priv->adapter->dev, "scan rejected due to traffic\n");
                 return -EBUSY;
         }
 
-        priv->scan_request = request;
+        if (priv->user_scan_cfg) {
+                dev_err(priv->adapter->dev, "cmd: Scan already in process..\n");
+                return -EBUSY;
+        }
 
         priv->user_scan_cfg = kzalloc(sizeof(struct mwifiex_user_scan_cfg),
                                       GFP_KERNEL);
@@ -1834,6 +1838,8 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
                 return -ENOMEM;
         }
 
+        priv->scan_request = request;
+
         priv->user_scan_cfg->num_ssids = request->n_ssids;
         priv->user_scan_cfg->ssid_list = request->ssids;
 
@@ -1870,6 +1876,9 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
         ret = mwifiex_scan_networks(priv, priv->user_scan_cfg);
         if (ret) {
                 dev_err(priv->adapter->dev, "scan failed: %d\n", ret);
+                priv->scan_request = NULL;
+                kfree(priv->user_scan_cfg);
+                priv->user_scan_cfg = NULL;
                 return ret;
         }
 
@@ -2113,7 +2122,6 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
         }
 
         sema_init(&priv->async_sem, 1);
-        priv->scan_pending_on_block = false;
 
         dev_dbg(adapter->dev, "info: %s: Marvell 802.11 Adapter\n", dev->name);
 
@@ -2251,7 +2259,8 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
         wiphy->available_antennas_rx = BIT(adapter->number_of_antenna) - 1;
 
         wiphy->features |= NL80211_FEATURE_HT_IBSS |
-                           NL80211_FEATURE_INACTIVITY_TIMER;
+                           NL80211_FEATURE_INACTIVITY_TIMER |
+                           NL80211_FEATURE_LOW_PRIORITY_SCAN;
 
         /* Reserve space for mwifiex specific private data for BSS */
         wiphy->bss_priv_size = sizeof(struct mwifiex_bss_priv);
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index 8d465107f52b..da6c49177fcc 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -917,21 +917,24 @@ mwifiex_cmd_timeout_func(unsigned long function_context)
 
                 dev_err(adapter->dev, "last_cmd_index = %d\n",
                         adapter->dbg.last_cmd_index);
-                print_hex_dump_bytes("last_cmd_id: ", DUMP_PREFIX_OFFSET,
-                                     adapter->dbg.last_cmd_id, DBG_CMD_NUM);
-                print_hex_dump_bytes("last_cmd_act: ", DUMP_PREFIX_OFFSET,
-                                     adapter->dbg.last_cmd_act, DBG_CMD_NUM);
+                dev_err(adapter->dev, "last_cmd_id: %*ph\n",
+                        (int)sizeof(adapter->dbg.last_cmd_id),
+                        adapter->dbg.last_cmd_id);
+                dev_err(adapter->dev, "last_cmd_act: %*ph\n",
+                        (int)sizeof(adapter->dbg.last_cmd_act),
+                        adapter->dbg.last_cmd_act);
 
                 dev_err(adapter->dev, "last_cmd_resp_index = %d\n",
                         adapter->dbg.last_cmd_resp_index);
-                print_hex_dump_bytes("last_cmd_resp_id: ", DUMP_PREFIX_OFFSET,
-                                     adapter->dbg.last_cmd_resp_id,
-                                     DBG_CMD_NUM);
+                dev_err(adapter->dev, "last_cmd_resp_id: %*ph\n",
+                        (int)sizeof(adapter->dbg.last_cmd_resp_id),
+                        adapter->dbg.last_cmd_resp_id);
 
                 dev_err(adapter->dev, "last_event_index = %d\n",
                         adapter->dbg.last_event_index);
-                print_hex_dump_bytes("last_event: ", DUMP_PREFIX_OFFSET,
-                                     adapter->dbg.last_event, DBG_CMD_NUM);
+                dev_err(adapter->dev, "last_event: %*ph\n",
+                        (int)sizeof(adapter->dbg.last_event),
+                        adapter->dbg.last_event);
 
                 dev_err(adapter->dev, "data_sent=%d cmd_sent=%d\n",
                         adapter->data_sent, adapter->cmd_sent);
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index b5d37a8caa09..482faace7900 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -84,18 +84,19 @@ static void scan_delay_timer_fn(unsigned long data)
                 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;
+                        if (priv->scan_request) {
+                                dev_dbg(priv->adapter->dev,
+                                        "info: aborting scan\n");
+                                cfg80211_scan_done(priv->scan_request, 1);
+                                priv->scan_request = NULL;
+                        } else {
+                                dev_dbg(priv->adapter->dev,
+                                        "info: scan already aborted\n");
+                        }
+
                         kfree(priv->user_scan_cfg);
                         priv->user_scan_cfg = NULL;
                 }
-
-                if (priv->scan_pending_on_block) {
-                        priv->scan_pending_on_block = false;
-                        up(&priv->async_sem);
-                }
                 goto done;
         }
 
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index eb22dd248d54..1df767bc8b6e 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -472,6 +472,14 @@ mwifiex_open(struct net_device *dev)
 static int
 mwifiex_close(struct net_device *dev)
 {
+        struct mwifiex_private *priv = mwifiex_netdev_get_priv(dev);
+
+        if (priv->scan_request) {
+                dev_dbg(priv->adapter->dev, "aborting scan on ndo_stop\n");
+                cfg80211_scan_done(priv->scan_request, 1);
+                priv->scan_request = NULL;
+        }
+
         return 0;
 }
 
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index c2d0ab146af5..81f8772dcb07 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -115,8 +115,6 @@ enum {
 #define MWIFIEX_TYPE_DATA                               0
 #define MWIFIEX_TYPE_EVENT                              3
 
-#define DBG_CMD_NUM                                             5
-
 #define MAX_BITMAP_RATES_SIZE                   10
 
 #define MAX_CHANNEL_BAND_BG     14
@@ -484,7 +482,6 @@ struct mwifiex_private {
         u8 nick_name[16];
         u16 current_key_index;
         struct semaphore async_sem;
-        u8 scan_pending_on_block;
         u8 report_scan_result;
         struct cfg80211_scan_request *scan_request;
         struct mwifiex_user_scan_cfg *user_scan_cfg;
@@ -750,9 +747,9 @@ int mwifiex_shutdown_fw_complete(struct mwifiex_adapter *adapter);
 
 int mwifiex_dnld_fw(struct mwifiex_adapter *, struct mwifiex_fw_image *);
 
-int mwifiex_recv_packet(struct mwifiex_adapter *, struct sk_buff *skb);
+int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb);
 
-int mwifiex_process_mgmt_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
                                 struct sk_buff *skb);
 
 int mwifiex_process_event(struct mwifiex_adapter *adapter);
@@ -809,7 +806,7 @@ void mwifiex_hs_activated_event(struct mwifiex_private *priv,
                                         u8 activated);
 int mwifiex_ret_802_11_hs_cfg(struct mwifiex_private *priv,
                               struct host_cmd_ds_command *resp);
-int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_rx_packet(struct mwifiex_private *priv,
                               struct sk_buff *skb);
 int mwifiex_sta_prepare_cmd(struct mwifiex_private *, uint16_t cmd_no,
                             u16 cmd_action, u32 cmd_oid,
@@ -819,9 +816,9 @@ int mwifiex_uap_prepare_cmd(struct mwifiex_private *priv, uint16_t cmd_no,
                             void *data_buf, void *cmd_buf);
 int mwifiex_process_sta_cmdresp(struct mwifiex_private *, u16 cmdresp_no,
                                 struct host_cmd_ds_command *resp);
-int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *,
+int mwifiex_process_sta_rx_packet(struct mwifiex_private *,
                                   struct sk_buff *skb);
-int mwifiex_process_uap_rx_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_uap_rx_packet(struct mwifiex_private *priv,
                                   struct sk_buff *skb);
 int mwifiex_handle_uap_rx_forward(struct mwifiex_private *priv,
                                   struct sk_buff *skb);
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index 5896b1fb4a2d..9189a32b7844 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -941,6 +941,11 @@ mwifiex_config_scan(struct mwifiex_private *priv,
                                  chan_idx)->chan_scan_mode_bitmap
                                         &= ~MWIFIEX_PASSIVE_SCAN;
 
+                        if (*filtered_scan)
+                                (scan_chan_list +
+                                 chan_idx)->chan_scan_mode_bitmap
+                                        |= MWIFIEX_DISABLE_CHAN_FILT;
+
                         if (user_scan_in->chan_list[chan_idx].scan_time) {
                                 scan_dur = (u16) user_scan_in->
                                         chan_list[chan_idx].scan_time;
@@ -1762,26 +1767,39 @@ int mwifiex_ret_802_11_scan(struct mwifiex_private *priv,
                 }
                 if (priv->report_scan_result)
                         priv->report_scan_result = false;
-                if (priv->scan_pending_on_block) {
-                        priv->scan_pending_on_block = false;
-                        up(&priv->async_sem);
-                }
 
                 if (priv->user_scan_cfg) {
-                        dev_dbg(priv->adapter->dev,
-                                "info: %s: sending scan results\n", __func__);
-                        cfg80211_scan_done(priv->scan_request, 0);
-                        priv->scan_request = NULL;
+                        if (priv->scan_request) {
+                                dev_dbg(priv->adapter->dev,
+                                        "info: notifying scan done\n");
+                                cfg80211_scan_done(priv->scan_request, 0);
+                                priv->scan_request = NULL;
+                        } else {
+                                dev_dbg(priv->adapter->dev,
+                                        "info: scan already aborted\n");
+                        }
+
                         kfree(priv->user_scan_cfg);
                         priv->user_scan_cfg = NULL;
                 }
         } else {
-                if (!mwifiex_wmm_lists_empty(adapter)) {
+                if (priv->user_scan_cfg && !priv->scan_request) {
+                        spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
+                                               flags);
+                        adapter->scan_delay_cnt = MWIFIEX_MAX_SCAN_DELAY_CNT;
+                        mod_timer(&priv->scan_delay_timer, jiffies);
+                        dev_dbg(priv->adapter->dev,
+                                "info: %s: triggerring scan abort\n", __func__);
+                } else if (!mwifiex_wmm_lists_empty(adapter) &&
+                           (priv->scan_request && (priv->scan_request->flags &
+                                            NL80211_SCAN_FLAG_LOW_PRIORITY))) {
                         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));
+                        dev_dbg(priv->adapter->dev,
+                                "info: %s: deferring scan\n", __func__);
                 } else {
                         /* Get scan command from scan_pending_q and put to
                            cmd_pending_q */
@@ -1846,21 +1864,18 @@ static int mwifiex_scan_specific_ssid(struct mwifiex_private *priv,
                                       struct cfg80211_ssid *req_ssid)
 {
         struct mwifiex_adapter *adapter = priv->adapter;
-        int ret = 0;
+        int ret;
         struct mwifiex_user_scan_cfg *scan_cfg;
 
-        if (!req_ssid)
-                return -1;
-
         if (adapter->scan_processing) {
-                dev_dbg(adapter->dev, "cmd: Scan already in process...\n");
-                return ret;
+                dev_err(adapter->dev, "cmd: Scan already in process...\n");
+                return -EBUSY;
         }
 
         if (priv->scan_block) {
-                dev_dbg(adapter->dev,
+                dev_err(adapter->dev,
                         "cmd: Scan is blocked during association...\n");
-                return ret;
+                return -EBUSY;
         }
 
         scan_cfg = kzalloc(sizeof(struct mwifiex_user_scan_cfg), GFP_KERNEL);
@@ -1897,7 +1912,6 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
                         __func__);
                 return -1;
         }
-        priv->scan_pending_on_block = true;
 
         priv->adapter->scan_wait_q_woken = false;
 
@@ -1911,10 +1925,7 @@ int mwifiex_request_scan(struct mwifiex_private *priv,
         if (!ret)
                 ret = mwifiex_wait_queue_complete(priv->adapter);
 
-        if (ret == -1) {
-                priv->scan_pending_on_block = false;
-                up(&priv->async_sem);
-        }
+        up(&priv->async_sem);
 
         return ret;
 }
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 09e6a267f566..65c12eb3e5e7 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -85,10 +85,6 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
                 spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
                 if (priv->report_scan_result)
                         priv->report_scan_result = false;
-                if (priv->scan_pending_on_block) {
-                        priv->scan_pending_on_block = false;
-                        up(&priv->async_sem);
-                }
                 break;
 
         case HostCmd_CMD_MAC_CONTROL:
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/mwifiex/sta_rx.c
index 07d32b73783e..b5c109504393 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/mwifiex/sta_rx.c
@@ -38,14 +38,10 @@
  *
  * The completion callback is called after processing in complete.
  */
-int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_rx_packet(struct mwifiex_private *priv,
                               struct sk_buff *skb)
 {
         int ret;
-        struct mwifiex_rxinfo *rx_info = MWIFIEX_SKB_RXCB(skb);
-        struct mwifiex_private *priv =
-                        mwifiex_get_priv_by_id(adapter, rx_info->bss_num,
-                                               rx_info->bss_type);
         struct rx_packet_hdr *rx_pkt_hdr;
         struct rxpd *local_rx_pd;
         int hdr_chop;
@@ -98,9 +94,9 @@ int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
 
         priv->rxpd_htinfo = local_rx_pd->ht_info;
 
-        ret = mwifiex_recv_packet(adapter, skb);
+        ret = mwifiex_recv_packet(priv, skb);
         if (ret == -1)
-                dev_err(adapter->dev, "recv packet failed\n");
+                dev_err(priv->adapter->dev, "recv packet failed\n");
 
         return ret;
 }
@@ -117,21 +113,15 @@ int mwifiex_process_rx_packet(struct mwifiex_adapter *adapter,
  *
  * The completion callback is called after processing in complete.
  */
-int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_sta_rx_packet(struct mwifiex_private *priv,
                                   struct sk_buff *skb)
 {
+        struct mwifiex_adapter *adapter = priv->adapter;
         int ret = 0;
         struct rxpd *local_rx_pd;
-        struct mwifiex_rxinfo *rx_info = MWIFIEX_SKB_RXCB(skb);
         struct rx_packet_hdr *rx_pkt_hdr;
         u8 ta[ETH_ALEN];
         u16 rx_pkt_type, rx_pkt_offset, rx_pkt_length, seq_num;
-        struct mwifiex_private *priv =
-                        mwifiex_get_priv_by_id(adapter, rx_info->bss_num,
-                                               rx_info->bss_type);
-
-        if (!priv)
-                return -1;
 
         local_rx_pd = (struct rxpd *) (skb->data);
         rx_pkt_type = le16_to_cpu(local_rx_pd->rx_pkt_type);
@@ -169,13 +159,13 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *adapter,
 
                 while (!skb_queue_empty(&list)) {
                         rx_skb = __skb_dequeue(&list);
-                        ret = mwifiex_recv_packet(adapter, rx_skb);
+                        ret = mwifiex_recv_packet(priv, rx_skb);
                         if (ret == -1)
                                 dev_err(adapter->dev, "Rx of A-MSDU failed");
                 }
                 return 0;
         } else if (rx_pkt_type == PKT_TYPE_MGMT) {
-                ret = mwifiex_process_mgmt_packet(adapter, skb);
+                ret = mwifiex_process_mgmt_packet(priv, skb);
                 if (ret)
                         dev_err(adapter->dev, "Rx of mgmt packet failed");
                 dev_kfree_skb_any(skb);
@@ -188,7 +178,7 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_adapter *adapter,
          */
         if (!IS_11N_ENABLED(priv) ||
             memcmp(priv->curr_addr, rx_pkt_hdr->eth803_hdr.h_dest, ETH_ALEN)) {
-                mwifiex_process_rx_packet(adapter, skb);
+                mwifiex_process_rx_packet(priv, skb);
                 return ret;
         }
 
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/mwifiex/txrx.c
index 2af263992e83..5cb3f7af8749 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/mwifiex/txrx.c
@@ -48,13 +48,19 @@ int mwifiex_handle_rx_packet(struct mwifiex_adapter *adapter,
         if (!priv)
                 priv = mwifiex_get_priv(adapter, MWIFIEX_BSS_ROLE_ANY);
 
+        if (!priv) {
+                dev_err(adapter->dev, "data: priv not found. Drop RX packet\n");
+                dev_kfree_skb_any(skb);
+                return -1;
+        }
+
         rx_info->bss_num = priv->bss_num;
         rx_info->bss_type = priv->bss_type;
 
         if (priv->bss_role == MWIFIEX_BSS_ROLE_UAP)
-                return mwifiex_process_uap_rx_packet(adapter, skb);
+                return mwifiex_process_uap_rx_packet(priv, skb);
 
-        return mwifiex_process_sta_rx_packet(adapter, skb);
+        return mwifiex_process_sta_rx_packet(priv, skb);
 }
 EXPORT_SYMBOL_GPL(mwifiex_handle_rx_packet);
 
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/mwifiex/uap_cmd.c
index d95a2d558fcf..8dd72240f162 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/mwifiex/uap_cmd.c
@@ -188,10 +188,19 @@ mwifiex_set_uap_rates(struct mwifiex_uap_bss_param *bss_cfg,
         int var_offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
         const u8 *var_pos = params->beacon.head + var_offset;
         int len = params->beacon.head_len - var_offset;
+        u8 rate_len = 0;
 
         rate_ie = (void *)cfg80211_find_ie(WLAN_EID_SUPP_RATES, var_pos, len);
-        if (rate_ie)
+        if (rate_ie) {
                 memcpy(bss_cfg->rates, rate_ie + 1, rate_ie->len);
+                rate_len = rate_ie->len;
+        }
+
+        rate_ie = (void *)cfg80211_find_ie(WLAN_EID_EXT_SUPP_RATES,
+                                           params->beacon.tail,
+                                           params->beacon.tail_len);
+        if (rate_ie)
+                memcpy(bss_cfg->rates + rate_len, rate_ie + 1, rate_ie->len);
 
         return;
 }
diff --git a/drivers/net/wireless/mwifiex/uap_txrx.c b/drivers/net/wireless/mwifiex/uap_txrx.c
index 0966ac24b3b4..a018e42d117e 100644
--- a/drivers/net/wireless/mwifiex/uap_txrx.c
+++ b/drivers/net/wireless/mwifiex/uap_txrx.c
@@ -146,7 +146,7 @@ int mwifiex_handle_uap_rx_forward(struct mwifiex_private *priv,
         }
 
         /* Forward unicat/Inter-BSS packets to kernel. */
-        return mwifiex_process_rx_packet(adapter, skb);
+        return mwifiex_process_rx_packet(priv, skb);
 }
 
 /*
@@ -159,24 +159,17 @@ int mwifiex_handle_uap_rx_forward(struct mwifiex_private *priv,
  *
  * The completion callback is called after processing is complete.
  */
-int mwifiex_process_uap_rx_packet(struct mwifiex_adapter *adapter,
+int mwifiex_process_uap_rx_packet(struct mwifiex_private *priv,
                                   struct sk_buff *skb)
 {
+        struct mwifiex_adapter *adapter = priv->adapter;
         int ret;
         struct uap_rxpd *uap_rx_pd;
-        struct mwifiex_rxinfo *rx_info = MWIFIEX_SKB_RXCB(skb);
         struct rx_packet_hdr *rx_pkt_hdr;
         u16 rx_pkt_type;
         u8 ta[ETH_ALEN], pkt_type;
         struct mwifiex_sta_node *node;
 
-        struct mwifiex_private *priv =
-                        mwifiex_get_priv_by_id(adapter, rx_info->bss_num,
-                                               rx_info->bss_type);
-
-        if (!priv)
-                return -1;
-
         uap_rx_pd = (struct uap_rxpd *)(skb->data);
         rx_pkt_type = le16_to_cpu(uap_rx_pd->rx_pkt_type);
         rx_pkt_hdr = (void *)uap_rx_pd + le16_to_cpu(uap_rx_pd->rx_pkt_offset);
@@ -210,7 +203,7 @@ int mwifiex_process_uap_rx_packet(struct mwifiex_adapter *adapter,
 
                 while (!skb_queue_empty(&list)) {
                         rx_skb = __skb_dequeue(&list);
-                        ret = mwifiex_recv_packet(adapter, rx_skb);
+                        ret = mwifiex_recv_packet(priv, rx_skb);
                         if (ret)
                                 dev_err(adapter->dev,
                                         "AP:Rx A-MSDU failed");
@@ -218,7 +211,7 @@ int mwifiex_process_uap_rx_packet(struct mwifiex_adapter *adapter,
 
                 return 0;
         } else if (rx_pkt_type == PKT_TYPE_MGMT) {
-                ret = mwifiex_process_mgmt_packet(adapter, skb);
+                ret = mwifiex_process_mgmt_packet(priv, skb);
                 if (ret)
                         dev_err(adapter->dev, "Rx of mgmt packet failed");
                 dev_kfree_skb_any(skb);
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index ae88f80cf86b..0982375ba3b1 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -146,20 +146,16 @@ int mwifiex_get_debug_info(struct mwifiex_private *priv,
  * to the kernel.
  */
 int
-mwifiex_process_mgmt_packet(struct mwifiex_adapter *adapter,
+mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
                             struct sk_buff *skb)
 {
         struct rxpd *rx_pd;
-        struct mwifiex_private *priv;
         u16 pkt_len;
 
         if (!skb)
                 return -1;
 
         rx_pd = (struct rxpd *)skb->data;
-        priv = mwifiex_get_priv_by_id(adapter, rx_pd->bss_num, rx_pd->bss_type);
-        if (!priv)
-                return -1;
 
         skb_pull(skb, le16_to_cpu(rx_pd->rx_pkt_offset));
         skb_pull(skb, sizeof(pkt_len));
@@ -190,20 +186,11 @@ mwifiex_process_mgmt_packet(struct mwifiex_adapter *adapter,
  * the function creates a blank SKB, fills it with the data from the
  * received buffer and then sends this new SKB to the kernel.
  */
-int mwifiex_recv_packet(struct mwifiex_adapter *adapter, struct sk_buff *skb)
+int mwifiex_recv_packet(struct mwifiex_private *priv, struct sk_buff *skb)
 {
-        struct mwifiex_rxinfo *rx_info;
-        struct mwifiex_private *priv;
-
         if (!skb)
                 return -1;
 
-        rx_info = MWIFIEX_SKB_RXCB(skb);
-        priv = mwifiex_get_priv_by_id(adapter, rx_info->bss_num,
-                                      rx_info->bss_type);
-        if (!priv)
-                return -1;
-
         skb->dev = priv->netdev;
         skb->protocol = eth_type_trans(skb, priv->netdev);
         skb->ip_summed = CHECKSUM_NONE;
@@ -225,7 +212,7 @@ int mwifiex_recv_packet(struct mwifiex_adapter *adapter, struct sk_buff *skb)
          * fragments. Currently we fail the Filesndl-ht.scr script
          * for UDP, hence this fix
          */
-        if ((adapter->iface_type == MWIFIEX_USB) &&
+        if ((priv->adapter->iface_type == MWIFIEX_USB) &&
             (skb->truesize > MWIFIEX_RX_DATA_BUF_SIZE))
                 skb->truesize += (skb->len - MWIFIEX_RX_DATA_BUF_SIZE);
 
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index 7f53cea2f205..01624dcaf73e 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -865,7 +865,7 @@ static int ezusb_firmware_download(struct ezusb_priv *upriv,
 static int ezusb_access_ltv(struct ezusb_priv *upriv,
                             struct request_context *ctx,
                             u16 length, const void *data, u16 frame_type,
-                            void *ans_buff, int ans_size, u16 *ans_length)
+                            void *ans_buff, unsigned ans_size, u16 *ans_length)
 {
         int req_size;
         int retval = 0;
@@ -933,7 +933,7 @@ static int ezusb_access_ltv(struct ezusb_priv *upriv,
         }
         if (ctx->in_rid) {
                 struct ezusb_packet *ans = ctx->buf;
-                int exp_len;
+                unsigned exp_len;
 
                 if (ans->hermes_len != 0)
                         exp_len = le16_to_cpu(ans->hermes_len) * 2 + 12;
@@ -949,8 +949,7 @@ static int ezusb_access_ltv(struct ezusb_priv *upriv,
                 }
 
                 if (ans_buff)
-                        memcpy(ans_buff, ans->data,
-                               min_t(int, exp_len, ans_size));
+                        memcpy(ans_buff, ans->data, min(exp_len, ans_size));
                 if (ans_length)
                         *ans_length = le16_to_cpu(ans->hermes_len);
         }
@@ -995,7 +994,7 @@ static int ezusb_read_ltv(struct hermes *hw, int bap, u16 rid,
         struct ezusb_priv *upriv = hw->priv;
         struct request_context *ctx;
 
-        if ((bufsize < 0) || (bufsize % 2))
+        if (bufsize % 2)
                 return -EINVAL;
 
         ctx = ezusb_alloc_ctx(upriv, rid, rid);
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index a12e84f892be..6b2e1e431dd2 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1988,6 +1988,7 @@ static struct usb_driver rt2500usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 01dc8891070c..3bc206d06cd1 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2520,20 +2520,37 @@ static int rt2800_get_txpower_bw_comp(struct rt2x00_dev *rt2x00dev,
         return comp_value;
 }
 
+static int rt2800_get_txpower_reg_delta(struct rt2x00_dev *rt2x00dev,
+                                        int power_level, int max_power)
+{
+        int delta;
+
+        if (test_bit(CAPABILITY_POWER_LIMIT, &rt2x00dev->cap_flags))
+                return 0;
+
+        /*
+         * XXX: We don't know the maximum transmit power of our hardware since
+         * the EEPROM doesn't expose it. We only know that we are calibrated
+         * to 100% tx power.
+         *
+         * Hence, we assume the regulatory limit that cfg80211 calulated for
+         * the current channel is our maximum and if we are requested to lower
+         * the value we just reduce our tx power accordingly.
+         */
+        delta = power_level - max_power;
+        return min(delta, 0);
+}
+
 static u8 rt2800_compensate_txpower(struct rt2x00_dev *rt2x00dev, int is_rate_b,
                                    enum ieee80211_band band, int power_level,
                                    u8 txpower, int delta)
 {
-        u32 reg;
         u16 eeprom;
         u8 criterion;
         u8 eirp_txpower;
         u8 eirp_txpower_criterion;
         u8 reg_limit;
 
-        if (!((band == IEEE80211_BAND_5GHZ) && is_rate_b))
-                return txpower;
-
         if (test_bit(CAPABILITY_POWER_LIMIT, &rt2x00dev->cap_flags)) {
                 /*
                  * Check if eirp txpower exceed txpower_limit.
@@ -2542,11 +2559,13 @@ static u8 rt2800_compensate_txpower(struct rt2x00_dev *rt2x00dev, int is_rate_b,
                  * .11b data rate need add additional 4dbm
                  * when calculating eirp txpower.
                  */
-                rt2800_register_read(rt2x00dev, TX_PWR_CFG_0, &reg);
-                criterion = rt2x00_get_field32(reg, TX_PWR_CFG_0_6MBS);
+                rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_BYRATE + 1,
+                                   &eeprom);
+                criterion = rt2x00_get_field16(eeprom,
+                                               EEPROM_TXPOWER_BYRATE_RATE0);
 
-                rt2x00_eeprom_read(rt2x00dev,
-                                   EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+                rt2x00_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER,
+                                   &eeprom);
 
                 if (band == IEEE80211_BAND_2GHZ)
                         eirp_txpower_criterion = rt2x00_get_field16(eeprom,
@@ -2563,36 +2582,71 @@ static u8 rt2800_compensate_txpower(struct rt2x00_dev *rt2x00dev, int is_rate_b,
         } else
                 reg_limit = 0;
 
-        return txpower + delta - reg_limit;
+        txpower = max(0, txpower + delta - reg_limit);
+        return min_t(u8, txpower, 0xc);
 }
 
+/*
+ * We configure transmit power using MAC TX_PWR_CFG_{0,...,N} registers and
+ * BBP R1 register. TX_PWR_CFG_X allow to configure per rate TX power values,
+ * 4 bits for each rate (tune from 0 to 15 dBm). BBP_R1 controls transmit power
+ * for all rates, but allow to set only 4 discrete values: -12, -6, 0 and 6 dBm.
+ * Reference per rate transmit power values are located in the EEPROM at
+ * EEPROM_TXPOWER_BYRATE offset. We adjust them and BBP R1 settings according to
+ * current conditions (i.e. band, bandwidth, temperature, user settings).
+ */
 static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
-                                  enum ieee80211_band band,
+                                  struct ieee80211_channel *chan,
                                   int power_level)
 {
-        u8 txpower;
+        u8 txpower, r1;
         u16 eeprom;
-        int i, is_rate_b;
-        u32 reg;
-        u8 r1;
-        u32 offset;
-        int delta;
+        u32 reg, offset;
+        int i, is_rate_b, delta, power_ctrl;
+        enum ieee80211_band band = chan->band;
 
         /*
-         * Calculate HT40 compensation delta
+         * Calculate HT40 compensation. For 40MHz we need to add or subtract
+         * value read from EEPROM (different for 2GHz and for 5GHz).
          */
         delta = rt2800_get_txpower_bw_comp(rt2x00dev, band);
 
         /*
-         * calculate temperature compensation delta
+         * Calculate temperature compensation. Depends on measurement of current
+         * TSSI (Transmitter Signal Strength Indication) we know TX power (due
+         * to temperature or maybe other factors) is smaller or bigger than
+         * expected. We adjust it, based on TSSI reference and boundaries values
+         * provided in EEPROM.
          */
         delta += rt2800_get_gain_calibration_delta(rt2x00dev);
 
         /*
-         * set to normal bbp tx power control mode: +/- 0dBm
+         * Decrease power according to user settings, on devices with unknown
+         * maximum tx power. For other devices we take user power_level into
+         * consideration on rt2800_compensate_txpower().
+         */
+        delta += rt2800_get_txpower_reg_delta(rt2x00dev, power_level,
+                                              chan->max_power);
+
+        /*
+         * BBP_R1 controls TX power for all rates, it allow to set the following
+         * gains -12, -6, 0, +6 dBm by setting values 2, 1, 0, 3 respectively.
+         *
+         * TODO: we do not use +6 dBm option to do not increase power beyond
+         * regulatory limit, however this could be utilized for devices with
+         * CAPABILITY_POWER_LIMIT.
          */
         rt2800_bbp_read(rt2x00dev, 1, &r1);
-        rt2x00_set_field8(&r1, BBP1_TX_POWER_CTRL, 0);
+        if (delta <= -12) {
+                power_ctrl = 2;
+                delta += 12;
+        } else if (delta <= -6) {
+                power_ctrl = 1;
+                delta += 6;
+        } else {
+                power_ctrl = 0;
+        }
+        rt2x00_set_field8(&r1, BBP1_TX_POWER_CTRL, power_ctrl);
         rt2800_bbp_write(rt2x00dev, 1, r1);
         offset = TX_PWR_CFG_0;
 
@@ -2710,7 +2764,7 @@ static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
 
 void rt2800_gain_calibration(struct rt2x00_dev *rt2x00dev)
 {
-        rt2800_config_txpower(rt2x00dev, rt2x00dev->curr_band,
+        rt2800_config_txpower(rt2x00dev, rt2x00dev->hw->conf.channel,
                               rt2x00dev->tx_power);
 }
 EXPORT_SYMBOL_GPL(rt2800_gain_calibration);
@@ -2845,11 +2899,11 @@ void rt2800_config(struct rt2x00_dev *rt2x00dev,
         if (flags & IEEE80211_CONF_CHANGE_CHANNEL) {
                 rt2800_config_channel(rt2x00dev, libconf->conf,
                                       &libconf->rf, &libconf->channel);
-                rt2800_config_txpower(rt2x00dev, libconf->conf->channel->band,
+                rt2800_config_txpower(rt2x00dev, libconf->conf->channel,
                                       libconf->conf->power_level);
         }
         if (flags & IEEE80211_CONF_CHANGE_POWER)
-                rt2800_config_txpower(rt2x00dev, libconf->conf->channel->band,
+                rt2800_config_txpower(rt2x00dev, libconf->conf->channel,
                                       libconf->conf->power_level);
         if (flags & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
                 rt2800_config_retry_limit(rt2x00dev, libconf);
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index c9e9370eb789..3b8fb5a603f2 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -1282,6 +1282,7 @@ static struct usb_driver rt2800usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index e5eb43b3eee7..24eec66e9fd2 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -2535,6 +2535,7 @@ static struct usb_driver rt73usb_driver = {
         .disconnect     = rt2x00usb_disconnect,
         .suspend        = rt2x00usb_suspend,
         .resume         = rt2x00usb_resume,
+        .reset_resume   = rt2x00usb_resume,
         .disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/rtlwifi/cam.c b/drivers/net/wireless/rtlwifi/cam.c
index 5b4b4d4eaf9e..ca69e35e50f1 100644
--- a/drivers/net/wireless/rtlwifi/cam.c
+++ b/drivers/net/wireless/rtlwifi/cam.c
@@ -52,11 +52,8 @@ static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
         u32 target_content = 0;
         u8 entry_i;
 
-        RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
-                 "key_cont_128:\n %x:%x:%x:%x:%x:%x\n",
-                 key_cont_128[0], key_cont_128[1],
-                 key_cont_128[2], key_cont_128[3],
-                 key_cont_128[4], key_cont_128[5]);
+        RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "key_cont_128: %6phC\n",
+                 key_cont_128);
 
         for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
                 target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index 86d73b32d995..038c02c9afed 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -1918,10 +1918,8 @@ static void rtl92ce_update_hal_rate_mask(struct ieee80211_hw *hw,
                                      (ratr_index << 28);
         rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
         RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
-                 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x\n",
-                 ratr_index, ratr_bitmap,
-                 rate_mask[0], rate_mask[1], rate_mask[2], rate_mask[3],
-                 rate_mask[4]);
+                 "Rate_index:%x, ratr_val:%x, %5phC\n",
+                 ratr_index, ratr_bitmap, rate_mask);
         rtl92c_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
 
         if (macid != 0)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 4bbb711a36c5..7d36a94263b0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -2169,10 +2169,8 @@ void rtl92cu_update_hal_rate_mask(struct ieee80211_hw *hw, u8 rssi_level)
                                       ratr_index << 28);
         rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
         RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
-                 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x\n",
-                 ratr_index, ratr_bitmap,
-                 rate_mask[0], rate_mask[1], rate_mask[2], rate_mask[3],
-                 rate_mask[4]);
+                 "Rate_index:%x, ratr_val:%x, %5phC\n",
+                 ratr_index, ratr_bitmap, rate_mask);
         rtl92c_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
 }
 
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 030beb45d8b0..e3ea4b346889 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -673,7 +673,7 @@ static int rtl_usb_start(struct ieee80211_hw *hw)
                 set_hal_start(rtlhal);
 
                 /* Start bulk IN */
-                _rtl_usb_receive(hw);
+                err = _rtl_usb_receive(hw);
         }
 
         return err;
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index bf05831fdf09..36c359043f54 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -2,7 +2,7 @@
 # Makefile for nfc devices
 #
 
-obj-$(CONFIG_PN544_HCI_NFC)     += pn544_hci.o
+obj-$(CONFIG_PN544_HCI_NFC)     += pn544/
 obj-$(CONFIG_NFC_PN533)         += pn533.o
 obj-$(CONFIG_NFC_WILINK)        += nfcwilink.o
 
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index 97c440a8cd61..18e279d3e836 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -84,6 +84,10 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_LISTEN_TIME 2
 
 /* frame definitions */
+#define PN533_NORMAL_FRAME_MAX_LEN 262  /* 6   (PREAMBLE, SOF, LEN, LCS, TFI)
+                                           254 (DATA)
+                                           2   (DCS, postamble) */
+
 #define PN533_FRAME_TAIL_SIZE 2
 #define PN533_FRAME_SIZE(f) (sizeof(struct pn533_frame) + f->datalen + \
                                 PN533_FRAME_TAIL_SIZE)
@@ -1165,8 +1169,7 @@ static void pn533_poll_create_mod_list(struct pn533 *dev,
                 pn533_poll_add_mod(dev, PN533_LISTEN_MOD);
 }
 
-static int pn533_start_poll_complete(struct pn533 *dev, void *arg,
-                                     u8 *params, int params_len)
+static int pn533_start_poll_complete(struct pn533 *dev, u8 *params, int params_len)
 {
         struct pn533_poll_response *resp;
         int rc;
@@ -1304,8 +1307,7 @@ static void pn533_wq_tg_get_data(struct work_struct *work)
 }
 
 #define ATR_REQ_GB_OFFSET 17
-static int pn533_init_target_complete(struct pn533 *dev, void *arg,
-                                      u8 *params, int params_len)
+static int pn533_init_target_complete(struct pn533 *dev, u8 *params, int params_len)
 {
         struct pn533_cmd_init_target_response *resp;
         u8 frame, comm_mode = NFC_COMM_PASSIVE, *gb;
@@ -1402,9 +1404,9 @@ static int pn533_poll_complete(struct pn533 *dev, void *arg,
         if (cur_mod->len == 0) {
                 del_timer(&dev->listen_timer);
 
-                return pn533_init_target_complete(dev, arg, params, params_len);
+                return pn533_init_target_complete(dev, params, params_len);
         } else {
-                rc = pn533_start_poll_complete(dev, arg, params, params_len);
+                rc = pn533_start_poll_complete(dev, params, params_len);
                 if (!rc)
                         return rc;
         }
@@ -2373,9 +2375,9 @@ static int pn533_probe(struct usb_interface *interface,
                 goto error;
         }
 
-        dev->in_frame = kmalloc(dev->in_maxlen, GFP_KERNEL);
+        dev->in_frame = kmalloc(PN533_NORMAL_FRAME_MAX_LEN, GFP_KERNEL);
         dev->in_urb = usb_alloc_urb(0, GFP_KERNEL);
-        dev->out_frame = kmalloc(dev->out_maxlen, GFP_KERNEL);
+        dev->out_frame = kmalloc(PN533_NORMAL_FRAME_MAX_LEN, GFP_KERNEL);
         dev->out_urb = usb_alloc_urb(0, GFP_KERNEL);
 
         if (!dev->in_frame || !dev->out_frame ||
diff --git a/drivers/nfc/pn544/Makefile b/drivers/nfc/pn544/Makefile
new file mode 100644
index 000000000000..725733881eb3
--- /dev/null
+++ b/drivers/nfc/pn544/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for PN544 HCI based NFC driver
+#
+
+obj-$(CONFIG_PN544_HCI_NFC)     += pn544_i2c.o
+
+pn544_i2c-y             := pn544.o i2c.o
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
new file mode 100644
index 000000000000..fb430d882352
--- /dev/null
+++ b/drivers/nfc/pn544/i2c.c
@@ -0,0 +1,500 @@
+/*
+ * I2C Link Layer for PN544 HCI based Driver
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * 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.
+ *
+ * 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.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/crc-ccitt.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/gpio.h>
+#include <linux/miscdevice.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/nfc/pn544.h>
+
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+
+#include "pn544.h"
+
+#define PN544_I2C_FRAME_HEADROOM 1
+#define PN544_I2C_FRAME_TAILROOM 2
+
+/* framing in HCI mode */
+#define PN544_HCI_I2C_LLC_LEN           1
+#define PN544_HCI_I2C_LLC_CRC           2
+#define PN544_HCI_I2C_LLC_LEN_CRC       (PN544_HCI_I2C_LLC_LEN + \
+                                         PN544_HCI_I2C_LLC_CRC)
+#define PN544_HCI_I2C_LLC_MIN_SIZE      (1 + PN544_HCI_I2C_LLC_LEN_CRC)
+#define PN544_HCI_I2C_LLC_MAX_PAYLOAD   29
+#define PN544_HCI_I2C_LLC_MAX_SIZE      (PN544_HCI_I2C_LLC_LEN_CRC + 1 + \
+                                         PN544_HCI_I2C_LLC_MAX_PAYLOAD)
+
+static struct i2c_device_id pn544_hci_i2c_id_table[] = {
+        {"pn544", 0},
+        {}
+};
+
+MODULE_DEVICE_TABLE(i2c, pn544_hci_i2c_id_table);
+
+#define PN544_HCI_I2C_DRIVER_NAME "pn544_hci_i2c"
+
+struct pn544_i2c_phy {
+        struct i2c_client *i2c_dev;
+        struct nfc_hci_dev *hdev;
+
+        unsigned int gpio_en;
+        unsigned int gpio_irq;
+        unsigned int gpio_fw;
+        unsigned int en_polarity;
+
+        int powered;
+
+        int hard_fault;         /*
+                                 * < 0 if hardware error occured (e.g. i2c err)
+                                 * and prevents normal operation.
+                                 */
+};
+
+#define I2C_DUMP_SKB(info, skb)                                 \
+do {                                                            \
+        pr_debug("%s:\n", info);                                \
+        print_hex_dump(KERN_DEBUG, "i2c: ", DUMP_PREFIX_OFFSET, \
+                       16, 1, (skb)->data, (skb)->len, 0);      \
+} while (0)
+
+static void pn544_hci_i2c_platform_init(struct pn544_i2c_phy *phy)
+{
+        int polarity, retry, ret;
+        char rset_cmd[] = { 0x05, 0xF9, 0x04, 0x00, 0xC3, 0xE5 };
+        int count = sizeof(rset_cmd);
+
+        pr_info(DRIVER_DESC ": %s\n", __func__);
+        dev_info(&phy->i2c_dev->dev, "Detecting nfc_en polarity\n");
+
+        /* Disable fw download */
+        gpio_set_value(phy->gpio_fw, 0);
+
+        for (polarity = 0; polarity < 2; polarity++) {
+                phy->en_polarity = polarity;
+                retry = 3;
+                while (retry--) {
+                        /* power off */
+                        gpio_set_value(phy->gpio_en, !phy->en_polarity);
+                        usleep_range(10000, 15000);
+
+                        /* power on */
+                        gpio_set_value(phy->gpio_en, phy->en_polarity);
+                        usleep_range(10000, 15000);
+
+                        /* send reset */
+                        dev_dbg(&phy->i2c_dev->dev, "Sending reset cmd\n");
+                        ret = i2c_master_send(phy->i2c_dev, rset_cmd, count);
+                        if (ret == count) {
+                                dev_info(&phy->i2c_dev->dev,
+                                         "nfc_en polarity : active %s\n",
+                                         (polarity == 0 ? "low" : "high"));
+                                goto out;
+                        }
+                }
+        }
+
+        dev_err(&phy->i2c_dev->dev,
+                "Could not detect nfc_en polarity, fallback to active high\n");
+
+out:
+        gpio_set_value(phy->gpio_en, !phy->en_polarity);
+}
+
+static int pn544_hci_i2c_enable(void *phy_id)
+{
+        struct pn544_i2c_phy *phy = phy_id;
+
+        pr_info(DRIVER_DESC ": %s\n", __func__);
+
+        gpio_set_value(phy->gpio_fw, 0);
+        gpio_set_value(phy->gpio_en, phy->en_polarity);
+        usleep_range(10000, 15000);
+
+        phy->powered = 1;
+
+        return 0;
+}
+
+static void pn544_hci_i2c_disable(void *phy_id)
+{
+        struct pn544_i2c_phy *phy = phy_id;
+
+        pr_info(DRIVER_DESC ": %s\n", __func__);
+
+        gpio_set_value(phy->gpio_fw, 0);
+        gpio_set_value(phy->gpio_en, !phy->en_polarity);
+        usleep_range(10000, 15000);
+
+        gpio_set_value(phy->gpio_en, phy->en_polarity);
+        usleep_range(10000, 15000);
+
+        gpio_set_value(phy->gpio_en, !phy->en_polarity);
+        usleep_range(10000, 15000);
+
+        phy->powered = 0;
+}
+
+static void pn544_hci_i2c_add_len_crc(struct sk_buff *skb)
+{
+        u16 crc;
+        int len;
+
+        len = skb->len + 2;
+        *skb_push(skb, 1) = len;
+
+        crc = crc_ccitt(0xffff, skb->data, skb->len);
+        crc = ~crc;
+        *skb_put(skb, 1) = crc & 0xff;
+        *skb_put(skb, 1) = crc >> 8;
+}
+
+static void pn544_hci_i2c_remove_len_crc(struct sk_buff *skb)
+{
+        skb_pull(skb, PN544_I2C_FRAME_HEADROOM);
+        skb_trim(skb, PN544_I2C_FRAME_TAILROOM);
+}
+
+/*
+ * Writing a frame must not return the number of written bytes.
+ * It must return either zero for success, or <0 for error.
+ * In addition, it must not alter the skb
+ */
+static int pn544_hci_i2c_write(void *phy_id, struct sk_buff *skb)
+{
+        int r;
+        struct pn544_i2c_phy *phy = phy_id;
+        struct i2c_client *client = phy->i2c_dev;
+
+        if (phy->hard_fault != 0)
+                return phy->hard_fault;
+
+        usleep_range(3000, 6000);
+
+        pn544_hci_i2c_add_len_crc(skb);
+
+        I2C_DUMP_SKB("i2c frame written", skb);
+
+        r = i2c_master_send(client, skb->data, skb->len);
+
+        if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
+                usleep_range(6000, 10000);
+                r = i2c_master_send(client, skb->data, skb->len);
+        }
+
+        if (r >= 0) {
+                if (r != skb->len)
+                        r = -EREMOTEIO;
+                else
+                        r = 0;
+        }
+
+        pn544_hci_i2c_remove_len_crc(skb);
+
+        return r;
+}
+
+static int check_crc(u8 *buf, int buflen)
+{
+        int len;
+        u16 crc;
+
+        len = buf[0] + 1;
+        crc = crc_ccitt(0xffff, buf, len - 2);
+        crc = ~crc;
+
+        if (buf[len - 2] != (crc & 0xff) || buf[len - 1] != (crc >> 8)) {
+                pr_err(PN544_HCI_I2C_DRIVER_NAME
+                       ": CRC error 0x%x != 0x%x 0x%x\n",
+                       crc, buf[len - 1], buf[len - 2]);
+
+                pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
+                print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
+                               16, 2, buf, buflen, false);
+                return -EPERM;
+        }
+        return 0;
+}
+
+/*
+ * Reads an shdlc frame and returns it in a newly allocated sk_buff. Guarantees
+ * that i2c bus will be flushed and that next read will start on a new frame.
+ * returned skb contains only LLC header and payload.
+ * returns:
+ * -EREMOTEIO : i2c read error (fatal)
+ * -EBADMSG : frame was incorrect and discarded
+ * -ENOMEM : cannot allocate skb, frame dropped
+ */
+static int pn544_hci_i2c_read(struct pn544_i2c_phy *phy, struct sk_buff **skb)
+{
+        int r;
+        u8 len;
+        u8 tmp[PN544_HCI_I2C_LLC_MAX_SIZE - 1];
+        struct i2c_client *client = phy->i2c_dev;
+
+        r = i2c_master_recv(client, &len, 1);
+        if (r != 1) {
+                dev_err(&client->dev, "cannot read len byte\n");
+                return -EREMOTEIO;
+        }
+
+        if ((len < (PN544_HCI_I2C_LLC_MIN_SIZE - 1)) ||
+            (len > (PN544_HCI_I2C_LLC_MAX_SIZE - 1))) {
+                dev_err(&client->dev, "invalid len byte\n");
+                r = -EBADMSG;
+                goto flush;
+        }
+
+        *skb = alloc_skb(1 + len, GFP_KERNEL);
+        if (*skb == NULL) {
+                r = -ENOMEM;
+                goto flush;
+        }
+
+        *skb_put(*skb, 1) = len;
+
+        r = i2c_master_recv(client, skb_put(*skb, len), len);
+        if (r != len) {
+                kfree_skb(*skb);
+                return -EREMOTEIO;
+        }
+
+        I2C_DUMP_SKB("i2c frame read", *skb);
+
+        r = check_crc((*skb)->data, (*skb)->len);
+        if (r != 0) {
+                kfree_skb(*skb);
+                r = -EBADMSG;
+                goto flush;
+        }
+
+        skb_pull(*skb, 1);
+        skb_trim(*skb, (*skb)->len - 2);
+
+        usleep_range(3000, 6000);
+
+        return 0;
+
+flush:
+        if (i2c_master_recv(client, tmp, sizeof(tmp)) < 0)
+                r = -EREMOTEIO;
+
+        usleep_range(3000, 6000);
+
+        return r;
+}
+
+/*
+ * Reads an shdlc frame from the chip. This is not as straightforward as it
+ * seems. There are cases where we could loose the frame start synchronization.
+ * The frame format is len-data-crc, and corruption can occur anywhere while
+ * transiting on i2c bus, such that we could read an invalid len.
+ * In order to recover synchronization with the next frame, we must be sure
+ * to read the real amount of data without using the len byte. We do this by
+ * assuming the following:
+ * - the chip will always present only one single complete frame on the bus
+ *   before triggering the interrupt
+ * - the chip will not present a new frame until we have completely read
+ *   the previous one (or until we have handled the interrupt).
+ * The tricky case is when we read a corrupted len that is less than the real
+ * len. We must detect this here in order to determine that we need to flush
+ * the bus. This is the reason why we check the crc here.
+ */
+static irqreturn_t pn544_hci_i2c_irq_thread_fn(int irq, void *phy_id)
+{
+        struct pn544_i2c_phy *phy = phy_id;
+        struct i2c_client *client;
+        struct sk_buff *skb = NULL;
+        int r;
+
+        if (!phy || irq != phy->i2c_dev->irq) {
+                WARN_ON_ONCE(1);
+                return IRQ_NONE;
+        }
+
+        client = phy->i2c_dev;
+        dev_dbg(&client->dev, "IRQ\n");
+
+        if (phy->hard_fault != 0)
+                return IRQ_HANDLED;
+
+        r = pn544_hci_i2c_read(phy, &skb);
+        if (r == -EREMOTEIO) {
+                phy->hard_fault = r;
+
+                nfc_hci_recv_frame(phy->hdev, NULL);
+
+                return IRQ_HANDLED;
+        } else if ((r == -ENOMEM) || (r == -EBADMSG)) {
+                return IRQ_HANDLED;
+        }
+
+        nfc_hci_recv_frame(phy->hdev, skb);
+
+        return IRQ_HANDLED;
+}
+
+static struct nfc_phy_ops i2c_phy_ops = {
+        .write = pn544_hci_i2c_write,
+        .enable = pn544_hci_i2c_enable,
+        .disable = pn544_hci_i2c_disable,
+};
+
+static int __devinit pn544_hci_i2c_probe(struct i2c_client *client,
+                                     const struct i2c_device_id *id)
+{
+        struct pn544_i2c_phy *phy;
+        struct pn544_nfc_platform_data *pdata;
+        int r = 0;
+
+        dev_dbg(&client->dev, "%s\n", __func__);
+        dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
+
+        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+                dev_err(&client->dev, "Need I2C_FUNC_I2C\n");
+                return -ENODEV;
+        }
+
+        phy = kzalloc(sizeof(struct pn544_i2c_phy), GFP_KERNEL);
+        if (!phy) {
+                dev_err(&client->dev,
+                        "Cannot allocate memory for pn544 i2c phy.\n");
+                r = -ENOMEM;
+                goto err_phy_alloc;
+        }
+
+        phy->i2c_dev = client;
+        i2c_set_clientdata(client, phy);
+
+        pdata = client->dev.platform_data;
+        if (pdata == NULL) {
+                dev_err(&client->dev, "No platform data\n");
+                r = -EINVAL;
+                goto err_pdata;
+        }
+
+        if (pdata->request_resources == NULL) {
+                dev_err(&client->dev, "request_resources() missing\n");
+                r = -EINVAL;
+                goto err_pdata;
+        }
+
+        r = pdata->request_resources(client);
+        if (r) {
+                dev_err(&client->dev, "Cannot get platform resources\n");
+                goto err_pdata;
+        }
+
+        phy->gpio_en = pdata->get_gpio(NFC_GPIO_ENABLE);
+        phy->gpio_fw = pdata->get_gpio(NFC_GPIO_FW_RESET);
+        phy->gpio_irq = pdata->get_gpio(NFC_GPIO_IRQ);
+
+        pn544_hci_i2c_platform_init(phy);
+
+        r = request_threaded_irq(client->irq, NULL, pn544_hci_i2c_irq_thread_fn,
+                                 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+                                 PN544_HCI_I2C_DRIVER_NAME, phy);
+        if (r < 0) {
+                dev_err(&client->dev, "Unable to register IRQ handler\n");
+                goto err_rti;
+        }
+
+        r = pn544_hci_probe(phy, &i2c_phy_ops, LLC_SHDLC_NAME,
+                            PN544_I2C_FRAME_HEADROOM, PN544_I2C_FRAME_TAILROOM,
+                            PN544_HCI_I2C_LLC_MAX_PAYLOAD, &phy->hdev);
+        if (r < 0)
+                goto err_hci;
+
+        return 0;
+
+err_hci:
+        free_irq(client->irq, phy);
+
+err_rti:
+        if (pdata->free_resources != NULL)
+                pdata->free_resources();
+
+err_pdata:
+        kfree(phy);
+
+err_phy_alloc:
+        return r;
+}
+
+static __devexit int pn544_hci_i2c_remove(struct i2c_client *client)
+{
+        struct pn544_i2c_phy *phy = i2c_get_clientdata(client);
+        struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
+
+        dev_dbg(&client->dev, "%s\n", __func__);
+
+        pn544_hci_remove(phy->hdev);
+
+        if (phy->powered)
+                pn544_hci_i2c_disable(phy);
+
+        free_irq(client->irq, phy);
+        if (pdata->free_resources)
+                pdata->free_resources();
+
+        kfree(phy);
+
+        return 0;
+}
+
+static struct i2c_driver pn544_hci_i2c_driver = {
+        .driver = {
+                   .name = PN544_HCI_I2C_DRIVER_NAME,
+                  },
+        .probe = pn544_hci_i2c_probe,
+        .id_table = pn544_hci_i2c_id_table,
+        .remove = __devexit_p(pn544_hci_i2c_remove),
+};
+
+static int __init pn544_hci_i2c_init(void)
+{
+        int r;
+
+        pr_debug(DRIVER_DESC ": %s\n", __func__);
+
+        r = i2c_add_driver(&pn544_hci_i2c_driver);
+        if (r) {
+                pr_err(PN544_HCI_I2C_DRIVER_NAME
+                       ": driver registration failed\n");
+                return r;
+        }
+
+        return 0;
+}
+
+static void __exit pn544_hci_i2c_exit(void)
+{
+        i2c_del_driver(&pn544_hci_i2c_driver);
+}
+
+module_init(pn544_hci_i2c_init);
+module_exit(pn544_hci_i2c_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nfc/pn544_hci.c b/drivers/nfc/pn544/pn544.c
index c9c8570273ab..cc666de3b8e5 100644
--- a/drivers/nfc/pn544_hci.c
+++ b/drivers/nfc/pn544/pn544.c
@@ -18,47 +18,21 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
-#include <linux/crc-ccitt.h>
-#include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
-#include <linux/miscdevice.h>
-#include <linux/interrupt.h>
-#include <linux/gpio.h>
-#include <linux/i2c.h>
 
 #include <linux/nfc.h>
 #include <net/nfc/hci.h>
 #include <net/nfc/llc.h>
 
-#include <linux/nfc/pn544.h>
-
-#define DRIVER_DESC "HCI NFC driver for PN544"
-
-#define PN544_HCI_DRIVER_NAME "pn544_hci"
+#include "pn544.h"
 
 /* Timing restrictions (ms) */
 #define PN544_HCI_RESETVEN_TIME         30
 
-static struct i2c_device_id pn544_hci_id_table[] = {
-        {"pn544", 0},
-        {}
-};
-
-MODULE_DEVICE_TABLE(i2c, pn544_hci_id_table);
-
 #define HCI_MODE 0
 #define FW_MODE 1
 
-/* framing in HCI mode */
-#define PN544_HCI_LLC_LEN               1
-#define PN544_HCI_LLC_CRC               2
-#define PN544_HCI_LLC_LEN_CRC           (PN544_HCI_LLC_LEN + PN544_HCI_LLC_CRC)
-#define PN544_HCI_LLC_MIN_SIZE          (1 + PN544_HCI_LLC_LEN_CRC)
-#define PN544_HCI_LLC_MAX_PAYLOAD       29
-#define PN544_HCI_LLC_MAX_SIZE          (PN544_HCI_LLC_LEN_CRC + 1 + \
-                                         PN544_HCI_LLC_MAX_PAYLOAD)
-
 enum pn544_state {
         PN544_ST_COLD,
         PN544_ST_FW_READY,
@@ -100,6 +74,10 @@ enum pn544_state {
 #define PN544_SYS_MGMT_INFO_NOTIFICATION        0x02
 
 #define PN544_POLLING_LOOP_MGMT_GATE            0x94
+#define PN544_DEP_MODE                          0x01
+#define PN544_DEP_ATR_REQ                       0x02
+#define PN544_DEP_ATR_RES                       0x03
+#define PN544_DEP_MERGE                         0x0D
 #define PN544_PL_RDPHASES                       0x06
 #define PN544_PL_EMULATION                      0x07
 #define PN544_PL_NFCT_DEACTIVATED               0x09
@@ -108,6 +86,15 @@ enum pn544_state {
 
 #define PN544_NFC_WI_MGMT_GATE                  0xA1
 
+#define PN544_HCI_EVT_SND_DATA                  0x01
+#define PN544_HCI_EVT_ACTIVATED                 0x02
+#define PN544_HCI_EVT_DEACTIVATED               0x03
+#define PN544_HCI_EVT_RCV_DATA                  0x04
+#define PN544_HCI_EVT_CONTINUE_MI               0x05
+
+#define PN544_HCI_CMD_ATTREQUEST                0x12
+#define PN544_HCI_CMD_CONTINUE_ACTIVATION       0x13
+
 static struct nfc_hci_gate pn544_gates[] = {
         {NFC_HCI_ADMIN_GATE, NFC_HCI_INVALID_PIPE},
         {NFC_HCI_LOOPBACK_GATE, NFC_HCI_INVALID_PIPE},
@@ -128,259 +115,22 @@ static struct nfc_hci_gate pn544_gates[] = {
 
 /* Largest headroom needed for outgoing custom commands */
 #define PN544_CMDS_HEADROOM     2
-#define PN544_FRAME_HEADROOM 1
-#define PN544_FRAME_TAILROOM 2
 
 struct pn544_hci_info {
-        struct i2c_client *i2c_dev;
+        struct nfc_phy_ops *phy_ops;
+        void *phy_id;
+
         struct nfc_hci_dev *hdev;
 
         enum pn544_state state;
 
         struct mutex info_lock;
 
-        unsigned int gpio_en;
-        unsigned int gpio_irq;
-        unsigned int gpio_fw;
-        unsigned int en_polarity;
-
-        int hard_fault;         /*
-                                 * < 0 if hardware error occured (e.g. i2c err)
-                                 * and prevents normal operation.
-                                 */
         int async_cb_type;
         data_exchange_cb_t async_cb;
         void *async_cb_context;
 };
 
-static void pn544_hci_platform_init(struct pn544_hci_info *info)
-{
-        int polarity, retry, ret;
-        char rset_cmd[] = { 0x05, 0xF9, 0x04, 0x00, 0xC3, 0xE5 };
-        int count = sizeof(rset_cmd);
-
-        pr_info(DRIVER_DESC ": %s\n", __func__);
-        dev_info(&info->i2c_dev->dev, "Detecting nfc_en polarity\n");
-
-        /* Disable fw download */
-        gpio_set_value(info->gpio_fw, 0);
-
-        for (polarity = 0; polarity < 2; polarity++) {
-                info->en_polarity = polarity;
-                retry = 3;
-                while (retry--) {
-                        /* power off */
-                        gpio_set_value(info->gpio_en, !info->en_polarity);
-                        usleep_range(10000, 15000);
-
-                        /* power on */
-                        gpio_set_value(info->gpio_en, info->en_polarity);
-                        usleep_range(10000, 15000);
-
-                        /* send reset */
-                        dev_dbg(&info->i2c_dev->dev, "Sending reset cmd\n");
-                        ret = i2c_master_send(info->i2c_dev, rset_cmd, count);
-                        if (ret == count) {
-                                dev_info(&info->i2c_dev->dev,
-                                         "nfc_en polarity : active %s\n",
-                                         (polarity == 0 ? "low" : "high"));
-                                goto out;
-                        }
-                }
-        }
-
-        dev_err(&info->i2c_dev->dev,
-                "Could not detect nfc_en polarity, fallback to active high\n");
-
-out:
-        gpio_set_value(info->gpio_en, !info->en_polarity);
-}
-
-static int pn544_hci_enable(struct pn544_hci_info *info, int mode)
-{
-        pr_info(DRIVER_DESC ": %s\n", __func__);
-
-        gpio_set_value(info->gpio_fw, 0);
-        gpio_set_value(info->gpio_en, info->en_polarity);
-        usleep_range(10000, 15000);
-
-        return 0;
-}
-
-static void pn544_hci_disable(struct pn544_hci_info *info)
-{
-        pr_info(DRIVER_DESC ": %s\n", __func__);
-
-        gpio_set_value(info->gpio_fw, 0);
-        gpio_set_value(info->gpio_en, !info->en_polarity);
-        usleep_range(10000, 15000);
-
-        gpio_set_value(info->gpio_en, info->en_polarity);
-        usleep_range(10000, 15000);
-
-        gpio_set_value(info->gpio_en, !info->en_polarity);
-        usleep_range(10000, 15000);
-}
-
-static int pn544_hci_i2c_write(struct i2c_client *client, u8 *buf, int len)
-{
-        int r;
-
-        usleep_range(3000, 6000);
-
-        r = i2c_master_send(client, buf, len);
-
-        if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
-                usleep_range(6000, 10000);
-                r = i2c_master_send(client, buf, len);
-        }
-
-        if (r >= 0) {
-                if (r != len)
-                        return -EREMOTEIO;
-                else
-                        return 0;
-        }
-
-        return r;
-}
-
-static int check_crc(u8 *buf, int buflen)
-{
-        int len;
-        u16 crc;
-
-        len = buf[0] + 1;
-        crc = crc_ccitt(0xffff, buf, len - 2);
-        crc = ~crc;
-
-        if (buf[len - 2] != (crc & 0xff) || buf[len - 1] != (crc >> 8)) {
-                pr_err(PN544_HCI_DRIVER_NAME ": CRC error 0x%x != 0x%x 0x%x\n",
-                       crc, buf[len - 1], buf[len - 2]);
-
-                pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
-                print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
-                               16, 2, buf, buflen, false);
-                return -EPERM;
-        }
-        return 0;
-}
-
-/*
- * Reads an shdlc frame and returns it in a newly allocated sk_buff. Guarantees
- * that i2c bus will be flushed and that next read will start on a new frame.
- * returned skb contains only LLC header and payload.
- * returns:
- * -EREMOTEIO : i2c read error (fatal)
- * -EBADMSG : frame was incorrect and discarded
- * -ENOMEM : cannot allocate skb, frame dropped
- */
-static int pn544_hci_i2c_read(struct i2c_client *client, struct sk_buff **skb)
-{
-        int r;
-        u8 len;
-        u8 tmp[PN544_HCI_LLC_MAX_SIZE - 1];
-
-        r = i2c_master_recv(client, &len, 1);
-        if (r != 1) {
-                dev_err(&client->dev, "cannot read len byte\n");
-                return -EREMOTEIO;
-        }
-
-        if ((len < (PN544_HCI_LLC_MIN_SIZE - 1)) ||
-            (len > (PN544_HCI_LLC_MAX_SIZE - 1))) {
-                dev_err(&client->dev, "invalid len byte\n");
-                r = -EBADMSG;
-                goto flush;
-        }
-
-        *skb = alloc_skb(1 + len, GFP_KERNEL);
-        if (*skb == NULL) {
-                r = -ENOMEM;
-                goto flush;
-        }
-
-        *skb_put(*skb, 1) = len;
-
-        r = i2c_master_recv(client, skb_put(*skb, len), len);
-        if (r != len) {
-                kfree_skb(*skb);
-                return -EREMOTEIO;
-        }
-
-        r = check_crc((*skb)->data, (*skb)->len);
-        if (r != 0) {
-                kfree_skb(*skb);
-                r = -EBADMSG;
-                goto flush;
-        }
-
-        skb_pull(*skb, 1);
-        skb_trim(*skb, (*skb)->len - 2);
-
-        usleep_range(3000, 6000);
-
-        return 0;
-
-flush:
-        if (i2c_master_recv(client, tmp, sizeof(tmp)) < 0)
-                r = -EREMOTEIO;
-
-        usleep_range(3000, 6000);
-
-        return r;
-}
-
-/*
- * Reads an shdlc frame from the chip. This is not as straightforward as it
- * seems. There are cases where we could loose the frame start synchronization.
- * The frame format is len-data-crc, and corruption can occur anywhere while
- * transiting on i2c bus, such that we could read an invalid len.
- * In order to recover synchronization with the next frame, we must be sure
- * to read the real amount of data without using the len byte. We do this by
- * assuming the following:
- * - the chip will always present only one single complete frame on the bus
- *   before triggering the interrupt
- * - the chip will not present a new frame until we have completely read
- *   the previous one (or until we have handled the interrupt).
- * The tricky case is when we read a corrupted len that is less than the real
- * len. We must detect this here in order to determine that we need to flush
- * the bus. This is the reason why we check the crc here.
- */
-static irqreturn_t pn544_hci_irq_thread_fn(int irq, void *dev_id)
-{
-        struct pn544_hci_info *info = dev_id;
-        struct i2c_client *client;
-        struct sk_buff *skb = NULL;
-        int r;
-
-        if (!info || irq != info->i2c_dev->irq) {
-                WARN_ON_ONCE(1);
-                return IRQ_NONE;
-        }
-
-        client = info->i2c_dev;
-        dev_dbg(&client->dev, "IRQ\n");
-
-        if (info->hard_fault != 0)
-                return IRQ_HANDLED;
-
-        r = pn544_hci_i2c_read(client, &skb);
-        if (r == -EREMOTEIO) {
-                info->hard_fault = r;
-
-                nfc_hci_recv_frame(info->hdev, NULL);
-
-                return IRQ_HANDLED;
-        } else if ((r == -ENOMEM) || (r == -EBADMSG)) {
-                return IRQ_HANDLED;
-        }
-
-        nfc_hci_recv_frame(info->hdev, skb);
-
-        return IRQ_HANDLED;
-}
-
 static int pn544_hci_open(struct nfc_hci_dev *hdev)
 {
         struct pn544_hci_info *info = nfc_hci_get_clientdata(hdev);
@@ -393,7 +143,7 @@ static int pn544_hci_open(struct nfc_hci_dev *hdev)
                 goto out;
         }
 
-        r = pn544_hci_enable(info, HCI_MODE);
+        r = info->phy_ops->enable(info->phy_id);
 
         if (r == 0)
                 info->state = PN544_ST_READY;
@@ -412,7 +162,7 @@ static void pn544_hci_close(struct nfc_hci_dev *hdev)
         if (info->state == PN544_ST_COLD)
                 goto out;
 
-        pn544_hci_disable(info);
+        info->phy_ops->disable(info->phy_id);
 
         info->state = PN544_ST_COLD;
 
@@ -587,40 +337,11 @@ static int pn544_hci_ready(struct nfc_hci_dev *hdev)
         return 0;
 }
 
-static void pn544_hci_add_len_crc(struct sk_buff *skb)
-{
-        u16 crc;
-        int len;
-
-        len = skb->len + 2;
-        *skb_push(skb, 1) = len;
-
-        crc = crc_ccitt(0xffff, skb->data, skb->len);
-        crc = ~crc;
-        *skb_put(skb, 1) = crc & 0xff;
-        *skb_put(skb, 1) = crc >> 8;
-}
-
-static void pn544_hci_remove_len_crc(struct sk_buff *skb)
-{
-        skb_pull(skb, PN544_FRAME_HEADROOM);
-        skb_trim(skb, PN544_FRAME_TAILROOM);
-}
-
 static int pn544_hci_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 {
         struct pn544_hci_info *info = nfc_hci_get_clientdata(hdev);
-        struct i2c_client *client = info->i2c_dev;
-        int r;
 
-        if (info->hard_fault != 0)
-                return info->hard_fault;
-
-        pn544_hci_add_len_crc(skb);
-        r = pn544_hci_i2c_write(client, skb->data, skb->len);
-        pn544_hci_remove_len_crc(skb);
-
-        return r;
+        return info->phy_ops->write(info->phy_id, skb);
 }
 
 static int pn544_hci_start_poll(struct nfc_hci_dev *hdev,
@@ -630,6 +351,9 @@ static int pn544_hci_start_poll(struct nfc_hci_dev *hdev,
         int r;
         u8 duration[2];
         u8 activated;
+        u8 i_mode = 0x3f; /* Enable all supported modes */
+        u8 t_mode = 0x0f;
+        u8 t_merge = 0x01; /* Enable merge by default */
 
         pr_info(DRIVER_DESC ": %s protocols 0x%x 0x%x\n",
                 __func__, im_protocols, tm_protocols);
@@ -667,6 +391,61 @@ static int pn544_hci_start_poll(struct nfc_hci_dev *hdev,
         if (r < 0)
                 return r;
 
+        if ((im_protocols | tm_protocols) & NFC_PROTO_NFC_DEP_MASK) {
+                hdev->gb = nfc_get_local_general_bytes(hdev->ndev,
+                                                        &hdev->gb_len);
+                pr_debug("generate local bytes %p", hdev->gb);
+                if (hdev->gb == NULL || hdev->gb_len == 0) {
+                        im_protocols &= ~NFC_PROTO_NFC_DEP_MASK;
+                        tm_protocols &= ~NFC_PROTO_NFC_DEP_MASK;
+                }
+        }
+
+        if (im_protocols & NFC_PROTO_NFC_DEP_MASK) {
+                r = nfc_hci_send_event(hdev,
+                                PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                NFC_HCI_EVT_END_OPERATION, NULL, 0);
+                if (r < 0)
+                        return r;
+
+                r = nfc_hci_set_param(hdev,
+                                PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                PN544_DEP_MODE, &i_mode, 1);
+                if (r < 0)
+                        return r;
+
+                r = nfc_hci_set_param(hdev,
+                                PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                PN544_DEP_ATR_REQ, hdev->gb, hdev->gb_len);
+                if (r < 0)
+                        return r;
+
+                r = nfc_hci_send_event(hdev,
+                                PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
+                if (r < 0)
+                        nfc_hci_send_event(hdev,
+                                        PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                        NFC_HCI_EVT_END_OPERATION, NULL, 0);
+        }
+
+        if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
+                r = nfc_hci_set_param(hdev, PN544_RF_READER_NFCIP1_TARGET_GATE,
+                                PN544_DEP_MODE, &t_mode, 1);
+                if (r < 0)
+                        return r;
+
+                r = nfc_hci_set_param(hdev, PN544_RF_READER_NFCIP1_TARGET_GATE,
+                                PN544_DEP_ATR_RES, hdev->gb, hdev->gb_len);
+                if (r < 0)
+                        return r;
+
+                r = nfc_hci_set_param(hdev, PN544_RF_READER_NFCIP1_TARGET_GATE,
+                                PN544_DEP_MERGE, &t_merge, 1);
+                if (r < 0)
+                        return r;
+        }
+
         r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
                                NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
         if (r < 0)
@@ -676,6 +455,43 @@ static int pn544_hci_start_poll(struct nfc_hci_dev *hdev,
         return r;
 }
 
+static int pn544_hci_dep_link_up(struct nfc_hci_dev *hdev,
+                                struct nfc_target *target, u8 comm_mode,
+                                u8 *gb, size_t gb_len)
+{
+        struct sk_buff *rgb_skb = NULL;
+        int r;
+
+        r = nfc_hci_get_param(hdev, target->hci_reader_gate,
+                                PN544_DEP_ATR_RES, &rgb_skb);
+        if (r < 0)
+                return r;
+
+        if (rgb_skb->len == 0 || rgb_skb->len > NFC_GB_MAXSIZE) {
+                r = -EPROTO;
+                goto exit;
+        }
+        print_hex_dump(KERN_DEBUG, "remote gb: ", DUMP_PREFIX_OFFSET,
+                        16, 1, rgb_skb->data, rgb_skb->len, true);
+
+        r = nfc_set_remote_general_bytes(hdev->ndev, rgb_skb->data,
+                                                rgb_skb->len);
+
+        if (r == 0)
+                r = nfc_dep_link_is_up(hdev->ndev, target->idx, comm_mode,
+                                        NFC_RF_INITIATOR);
+exit:
+        kfree_skb(rgb_skb);
+        return r;
+}
+
+static int pn544_hci_dep_link_down(struct nfc_hci_dev *hdev)
+{
+
+        return nfc_hci_send_event(hdev, PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                        NFC_HCI_EVT_END_OPERATION, NULL, 0);
+}
+
 static int pn544_hci_target_from_gate(struct nfc_hci_dev *hdev, u8 gate,
                                       struct nfc_target *target)
 {
@@ -687,6 +503,9 @@ static int pn544_hci_target_from_gate(struct nfc_hci_dev *hdev, u8 gate,
                 target->supported_protocols = NFC_PROTO_JEWEL_MASK;
                 target->sens_res = 0x0c00;
                 break;
+        case PN544_RF_READER_NFCIP1_INITIATOR_GATE:
+                target->supported_protocols = NFC_PROTO_NFC_DEP_MASK;
+                break;
         default:
                 return -EPROTO;
         }
@@ -701,7 +520,18 @@ static int pn544_hci_complete_target_discovered(struct nfc_hci_dev *hdev,
         struct sk_buff *uid_skb;
         int r = 0;
 
-        if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+        if (gate == PN544_RF_READER_NFCIP1_INITIATOR_GATE)
+                return r;
+
+        if (target->supported_protocols & NFC_PROTO_NFC_DEP_MASK) {
+                r = nfc_hci_send_cmd(hdev,
+                        PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                        PN544_HCI_CMD_CONTINUE_ACTIVATION, NULL, 0, NULL);
+                if (r < 0)
+                        return r;
+
+                target->hci_reader_gate = PN544_RF_READER_NFCIP1_INITIATOR_GATE;
+        } else if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
                 if (target->nfcid1_len != 4 && target->nfcid1_len != 7 &&
                     target->nfcid1_len != 10)
                         return -EPROTO;
@@ -724,6 +554,16 @@ static int pn544_hci_complete_target_discovered(struct nfc_hci_dev *hdev,
                                      PN544_RF_READER_CMD_ACTIVATE_NEXT,
                                      uid_skb->data, uid_skb->len, NULL);
                 kfree_skb(uid_skb);
+
+                r = nfc_hci_send_cmd(hdev,
+                                        PN544_RF_READER_NFCIP1_INITIATOR_GATE,
+                                        PN544_HCI_CMD_CONTINUE_ACTIVATION,
+                                        NULL, 0, NULL);
+                if (r < 0)
+                        return r;
+
+                target->hci_reader_gate = PN544_RF_READER_NFCIP1_INITIATOR_GATE;
+                target->supported_protocols = NFC_PROTO_NFC_DEP_MASK;
         } else if (target->supported_protocols & NFC_PROTO_ISO14443_MASK) {
                 /*
                  * TODO: maybe other ISO 14443 require some kind of continue
@@ -769,7 +609,7 @@ static void pn544_hci_data_exchange_cb(void *context, struct sk_buff *skb,
  * <= 0: driver handled the data exchange
  *    1: driver doesn't especially handle, please do standard processing
  */
-static int pn544_hci_data_exchange(struct nfc_hci_dev *hdev,
+static int pn544_hci_im_transceive(struct nfc_hci_dev *hdev,
                                    struct nfc_target *target,
                                    struct sk_buff *skb, data_exchange_cb_t cb,
                                    void *cb_context)
@@ -822,17 +662,110 @@ static int pn544_hci_data_exchange(struct nfc_hci_dev *hdev,
                 return nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
                                               PN544_JEWEL_RAW_CMD, skb->data,
                                               skb->len, cb, cb_context);
+        case PN544_RF_READER_NFCIP1_INITIATOR_GATE:
+                *skb_push(skb, 1) = 0;
+
+                return nfc_hci_send_event(hdev, target->hci_reader_gate,
+                                        PN544_HCI_EVT_SND_DATA, skb->data,
+                                        skb->len);
         default:
                 return 1;
         }
 }
 
+static int pn544_hci_tm_send(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+        /* Set default false for multiple information chaining */
+        *skb_push(skb, 1) = 0;
+
+        return nfc_hci_send_event(hdev, PN544_RF_READER_NFCIP1_TARGET_GATE,
+                                PN544_HCI_EVT_SND_DATA, skb->data, skb->len);
+}
+
 static int pn544_hci_check_presence(struct nfc_hci_dev *hdev,
                                    struct nfc_target *target)
 {
-        return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
-                                PN544_RF_READER_CMD_PRESENCE_CHECK,
-                                NULL, 0, NULL);
+        pr_debug("supported protocol %d", target->supported_protocols);
+        if (target->supported_protocols & (NFC_PROTO_ISO14443_MASK |
+                                        NFC_PROTO_ISO14443_B_MASK)) {
+                return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+                                        PN544_RF_READER_CMD_PRESENCE_CHECK,
+                                        NULL, 0, NULL);
+        } else if (target->supported_protocols & NFC_PROTO_MIFARE_MASK) {
+                if (target->nfcid1_len != 4 && target->nfcid1_len != 7 &&
+                    target->nfcid1_len != 10)
+                        return -EOPNOTSUPP;
+
+                 return nfc_hci_send_cmd(hdev, NFC_HCI_RF_READER_A_GATE,
+                                     PN544_RF_READER_CMD_ACTIVATE_NEXT,
+                                     target->nfcid1, target->nfcid1_len, NULL);
+        } else if (target->supported_protocols & NFC_PROTO_JEWEL_MASK) {
+                return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+                                        PN544_JEWEL_RAW_CMD, NULL, 0, NULL);
+        } else if (target->supported_protocols & NFC_PROTO_FELICA_MASK) {
+                return nfc_hci_send_cmd(hdev, PN544_RF_READER_F_GATE,
+                                        PN544_FELICA_RAW, NULL, 0, NULL);
+        } else if (target->supported_protocols & NFC_PROTO_NFC_DEP_MASK) {
+                return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
+                                        PN544_HCI_CMD_ATTREQUEST,
+                                        NULL, 0, NULL);
+        }
+
+        return 0;
+}
+
+static void pn544_hci_event_received(struct nfc_hci_dev *hdev, u8 gate,
+                                        u8 event, struct sk_buff *skb)
+{
+        struct sk_buff *rgb_skb = NULL;
+        int r = 0;
+
+        pr_debug("hci event %d", event);
+        switch (event) {
+        case PN544_HCI_EVT_ACTIVATED:
+                if (gate == PN544_RF_READER_NFCIP1_INITIATOR_GATE)
+                        nfc_hci_target_discovered(hdev, gate);
+                else if (gate == PN544_RF_READER_NFCIP1_TARGET_GATE) {
+                        r = nfc_hci_get_param(hdev, gate, PN544_DEP_ATR_REQ,
+                                                &rgb_skb);
+
+                        if (r < 0)
+                                goto exit;
+
+                        nfc_tm_activated(hdev->ndev, NFC_PROTO_NFC_DEP_MASK,
+                                        NFC_COMM_PASSIVE, rgb_skb->data,
+                                        rgb_skb->len);
+
+                        kfree_skb(rgb_skb);
+                }
+
+                break;
+        case PN544_HCI_EVT_DEACTIVATED:
+                nfc_hci_send_event(hdev, gate,
+                        NFC_HCI_EVT_END_OPERATION, NULL, 0);
+                break;
+        case PN544_HCI_EVT_RCV_DATA:
+                if (skb->len < 2) {
+                        r = -EPROTO;
+                        goto exit;
+                }
+
+                if (skb->data[0] != 0) {
+                        pr_debug("data0 %d", skb->data[0]);
+                        r = -EPROTO;
+                        goto exit;
+                }
+
+                skb_pull(skb, 2);
+                nfc_tm_data_received(hdev->ndev, skb);
+
+                return;
+        default:
+                break;
+        }
+
+exit:
+        kfree_skb(skb);
 }
 
 static struct nfc_hci_ops pn544_hci_ops = {
@@ -841,74 +774,36 @@ static struct nfc_hci_ops pn544_hci_ops = {
         .hci_ready = pn544_hci_ready,
         .xmit = pn544_hci_xmit,
         .start_poll = pn544_hci_start_poll,
+        .dep_link_up = pn544_hci_dep_link_up,
+        .dep_link_down = pn544_hci_dep_link_down,
         .target_from_gate = pn544_hci_target_from_gate,
         .complete_target_discovered = pn544_hci_complete_target_discovered,
-        .data_exchange = pn544_hci_data_exchange,
+        .im_transceive = pn544_hci_im_transceive,
+        .tm_send = pn544_hci_tm_send,
         .check_presence = pn544_hci_check_presence,
+        .event_received = pn544_hci_event_received,
 };
 
-static int __devinit pn544_hci_probe(struct i2c_client *client,
-                                     const struct i2c_device_id *id)
+int pn544_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
+                    int phy_headroom, int phy_tailroom, int phy_payload,
+                    struct nfc_hci_dev **hdev)
 {
         struct pn544_hci_info *info;
-        struct pn544_nfc_platform_data *pdata;
-        int r = 0;
         u32 protocols;
         struct nfc_hci_init_data init_data;
-
-        dev_dbg(&client->dev, "%s\n", __func__);
-        dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
-
-        if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-                dev_err(&client->dev, "Need I2C_FUNC_I2C\n");
-                return -ENODEV;
-        }
+        int r;
 
         info = kzalloc(sizeof(struct pn544_hci_info), GFP_KERNEL);
         if (!info) {
-                dev_err(&client->dev,
-                        "Cannot allocate memory for pn544_hci_info.\n");
+                pr_err("Cannot allocate memory for pn544_hci_info.\n");
                 r = -ENOMEM;
                 goto err_info_alloc;
         }
 
-        info->i2c_dev = client;
+        info->phy_ops = phy_ops;
+        info->phy_id = phy_id;
         info->state = PN544_ST_COLD;
         mutex_init(&info->info_lock);
-        i2c_set_clientdata(client, info);
-
-        pdata = client->dev.platform_data;
-        if (pdata == NULL) {
-                dev_err(&client->dev, "No platform data\n");
-                r = -EINVAL;
-                goto err_pdata;
-        }
-
-        if (pdata->request_resources == NULL) {
-                dev_err(&client->dev, "request_resources() missing\n");
-                r = -EINVAL;
-                goto err_pdata;
-        }
-
-        r = pdata->request_resources(client);
-        if (r) {
-                dev_err(&client->dev, "Cannot get platform resources\n");
-                goto err_pdata;
-        }
-
-        info->gpio_en = pdata->get_gpio(NFC_GPIO_ENABLE);
-        info->gpio_fw = pdata->get_gpio(NFC_GPIO_FW_RESET);
-        info->gpio_irq = pdata->get_gpio(NFC_GPIO_IRQ);
-
-        pn544_hci_platform_init(info);
-
-        r = request_threaded_irq(client->irq, NULL, pn544_hci_irq_thread_fn,
-                                 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-                                 PN544_HCI_DRIVER_NAME, info);
-        if (r < 0) {
-                dev_err(&client->dev, "Unable to register IRQ handler\n");
-                goto err_rti;
-        }
 
         init_data.gate_count = ARRAY_SIZE(pn544_gates);
 
@@ -928,13 +823,11 @@ static int __devinit pn544_hci_probe(struct i2c_client *client,
                     NFC_PROTO_NFC_DEP_MASK;
 
         info->hdev = nfc_hci_allocate_device(&pn544_hci_ops, &init_data,
-                                             protocols, LLC_SHDLC_NAME,
-                                             PN544_FRAME_HEADROOM +
-                                             PN544_CMDS_HEADROOM,
-                                             PN544_FRAME_TAILROOM,
-                                             PN544_HCI_LLC_MAX_PAYLOAD);
+                                             protocols, llc_name,
+                                             phy_headroom + PN544_CMDS_HEADROOM,
+                                             phy_tailroom, phy_payload);
         if (!info->hdev) {
-                dev_err(&client->dev, "Cannot allocate nfc hdev.\n");
+                pr_err("Cannot allocate nfc hdev.\n");
                 r = -ENOMEM;
                 goto err_alloc_hdev;
         }
@@ -945,79 +838,25 @@ static int __devinit pn544_hci_probe(struct i2c_client *client,
         if (r)
                 goto err_regdev;
 
+        *hdev = info->hdev;
+
         return 0;
 
 err_regdev:
         nfc_hci_free_device(info->hdev);
 
 err_alloc_hdev:
-        free_irq(client->irq, info);
-
-err_rti:
-        if (pdata->free_resources != NULL)
-                pdata->free_resources();
-
-err_pdata:
         kfree(info);
 
 err_info_alloc:
         return r;
 }
 
-static __devexit int pn544_hci_remove(struct i2c_client *client)
+void pn544_hci_remove(struct nfc_hci_dev *hdev)
 {
-        struct pn544_hci_info *info = i2c_get_clientdata(client);
-        struct pn544_nfc_platform_data *pdata = client->dev.platform_data;
-
-        dev_dbg(&client->dev, "%s\n", __func__);
-
-        nfc_hci_free_device(info->hdev);
-
-        if (info->state != PN544_ST_COLD) {
-                if (pdata->disable)
-                        pdata->disable();
-        }
-
-        free_irq(client->irq, info);
-        if (pdata->free_resources)
-                pdata->free_resources();
+        struct pn544_hci_info *info = nfc_hci_get_clientdata(hdev);
 
+        nfc_hci_unregister_device(hdev);
+        nfc_hci_free_device(hdev);
         kfree(info);
-
-        return 0;
 }
-
-static struct i2c_driver pn544_hci_driver = {
-        .driver = {
-                   .name = PN544_HCI_DRIVER_NAME,
-                  },
-        .probe = pn544_hci_probe,
-        .id_table = pn544_hci_id_table,
-        .remove = __devexit_p(pn544_hci_remove),
-};
-
-static int __init pn544_hci_init(void)
-{
-        int r;
-
-        pr_debug(DRIVER_DESC ": %s\n", __func__);
-
-        r = i2c_add_driver(&pn544_hci_driver);
-        if (r) {
-                pr_err(PN544_HCI_DRIVER_NAME ": driver registration failed\n");
-                return r;
-        }
-
-        return 0;
-}
-
-static void __exit pn544_hci_exit(void)
-{
-        i2c_del_driver(&pn544_hci_driver);
-}
-
-module_init(pn544_hci_init);
-module_exit(pn544_hci_exit);
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION(DRIVER_DESC);
diff --git a/drivers/nfc/pn544/pn544.h b/drivers/nfc/pn544/pn544.h
new file mode 100644
index 000000000000..f47c6454914b
--- /dev/null
+++ b/drivers/nfc/pn544/pn544.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2011 - 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __LOCAL_PN544_H_
+#define __LOCAL_PN544_H_
+
+#include <net/nfc/hci.h>
+
+#define DRIVER_DESC "HCI NFC driver for PN544"
+
+int pn544_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
+                    int phy_headroom, int phy_tailroom, int phy_payload,
+                    struct nfc_hci_dev **hdev);
+void pn544_hci_remove(struct nfc_hci_dev *hdev);
+
+#endif /* __LOCAL_PN544_H_ */
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index 266aa1648a02..19396dc4ee47 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -37,6 +37,7 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4329) },
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432b) },
         { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432c) },
+        { PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4350) },
         { 0, },
 };
 MODULE_DEVICE_TABLE(pci, b43_pci_bridge_tbl);
diff --git a/drivers/ssb/driver_chipcommon_pmu.c b/drivers/ssb/driver_chipcommon_pmu.c
index b58fef780ea0..d7d58044b4bc 100644
--- a/drivers/ssb/driver_chipcommon_pmu.c
+++ b/drivers/ssb/driver_chipcommon_pmu.c
@@ -346,6 +346,8 @@ static void ssb_pmu_pll_init(struct ssb_chipcommon *cc)
                         chipco_write32(cc, SSB_CHIPCO_PLLCTL_DATA, 0x380005C0);
                 }
                 break;
+        case 43222:
+                break;
         default:
                 ssb_printk(KERN_ERR PFX
                            "ERROR: PLL init unknown for device %04X\n",
@@ -434,6 +436,7 @@ static void ssb_pmu_resources_init(struct ssb_chipcommon *cc)
                  min_msk = 0xCBB;
                  break;
         case 0x4322:
+        case 43222:
                 /* We keep the default settings:
                  * min_msk = 0xCBB
                  * max_msk = 0x7FFFF
diff --git a/drivers/ssb/driver_mipscore.c b/drivers/ssb/driver_mipscore.c
index c6250867a95d..b918ba922306 100644
--- a/drivers/ssb/driver_mipscore.c
+++ b/drivers/ssb/driver_mipscore.c
@@ -192,9 +192,10 @@ static void ssb_mips_flash_detect(struct ssb_mipscore *mcore)
 
         /* When there is no chipcommon on the bus there is 4MB flash */
         if (!bus->chipco.dev) {
-                mcore->flash_buswidth = 2;
-                mcore->flash_window = SSB_FLASH1;
-                mcore->flash_window_size = SSB_FLASH1_SZ;
+                mcore->pflash.present = true;
+                mcore->pflash.buswidth = 2;
+                mcore->pflash.window = SSB_FLASH1;
+                mcore->pflash.window_size = SSB_FLASH1_SZ;
                 return;
         }
 
@@ -206,13 +207,14 @@ static void ssb_mips_flash_detect(struct ssb_mipscore *mcore)
                 break;
         case SSB_CHIPCO_FLASHT_PARA:
                 pr_debug("Found parallel flash\n");
-                mcore->flash_window = SSB_FLASH2;
-                mcore->flash_window_size = SSB_FLASH2_SZ;
+                mcore->pflash.present = true;
+                mcore->pflash.window = SSB_FLASH2;
+                mcore->pflash.window_size = SSB_FLASH2_SZ;
                 if ((ssb_read32(bus->chipco.dev, SSB_CHIPCO_FLASH_CFG)
                                & SSB_CHIPCO_CFG_DS16) == 0)
-                        mcore->flash_buswidth = 1;
+                        mcore->pflash.buswidth = 1;
                 else
-                        mcore->flash_buswidth = 2;
+                        mcore->pflash.buswidth = 2;
                 break;
         }
 }
diff --git a/include/linux/bcma/bcma.h b/include/linux/bcma/bcma.h
index 4180eb78d575..fd15d9829705 100644
--- a/include/linux/bcma/bcma.h
+++ b/include/linux/bcma/bcma.h
@@ -251,7 +251,7 @@ struct bcma_bus {
         u8 num;
 
         struct bcma_drv_cc drv_cc;
-        struct bcma_drv_pci drv_pci;
+        struct bcma_drv_pci drv_pci[2];
         struct bcma_drv_mips drv_mips;
         struct bcma_drv_gmac_cmn drv_gmac_cmn;
 
diff --git a/include/linux/bcma/bcma_driver_chipcommon.h b/include/linux/bcma/bcma_driver_chipcommon.h
index 1cf1749440ac..145f3c56227f 100644
--- a/include/linux/bcma/bcma_driver_chipcommon.h
+++ b/include/linux/bcma/bcma_driver_chipcommon.h
@@ -510,6 +510,7 @@ struct bcma_chipcommon_pmu {
 
 #ifdef CONFIG_BCMA_DRIVER_MIPS
 struct bcma_pflash {
+        bool present;
         u8 buswidth;
         u32 window;
         u32 window_size;
@@ -532,6 +533,7 @@ struct mtd_info;
 
 struct bcma_nflash {
         bool present;
+        bool boot;              /* This is the flash the SoC boots from */
 
         struct mtd_info *mtd;
 };
@@ -552,6 +554,7 @@ struct bcma_drv_cc {
         u32 capabilities;
         u32 capabilities_ext;
         u8 setup_done:1;
+        u8 early_setup_done:1;
         /* Fast Powerup Delay constant */
         u16 fast_pwrup_delay;
         struct bcma_chipcommon_pmu pmu;
@@ -583,6 +586,7 @@ struct bcma_drv_cc {
         bcma_cc_write32(cc, offset, (bcma_cc_read32(cc, offset) & (mask)) | (set))
 
 extern void bcma_core_chipcommon_init(struct bcma_drv_cc *cc);
+extern void bcma_core_chipcommon_early_init(struct bcma_drv_cc *cc);
 
 extern void bcma_chipco_suspend(struct bcma_drv_cc *cc);
 extern void bcma_chipco_resume(struct bcma_drv_cc *cc);
@@ -606,6 +610,7 @@ u32 bcma_chipco_gpio_polarity(struct bcma_drv_cc *cc, u32 mask, u32 value);
 
 /* PMU support */
 extern void bcma_pmu_init(struct bcma_drv_cc *cc);
+extern void bcma_pmu_early_init(struct bcma_drv_cc *cc);
 
 extern void bcma_chipco_pll_write(struct bcma_drv_cc *cc, u32 offset,
                                   u32 value);
diff --git a/include/linux/bcma/bcma_driver_mips.h b/include/linux/bcma/bcma_driver_mips.h
index c0043645cdcb..0baf8a56b794 100644
--- a/include/linux/bcma/bcma_driver_mips.h
+++ b/include/linux/bcma/bcma_driver_mips.h
@@ -35,13 +35,16 @@ struct bcma_device;
 struct bcma_drv_mips {
         struct bcma_device *core;
         u8 setup_done:1;
+        u8 early_setup_done:1;
         unsigned int assigned_irqs;
 };
 
 #ifdef CONFIG_BCMA_DRIVER_MIPS
 extern void bcma_core_mips_init(struct bcma_drv_mips *mcore);
+extern void bcma_core_mips_early_init(struct bcma_drv_mips *mcore);
 #else
 static inline void bcma_core_mips_init(struct bcma_drv_mips *mcore) { }
+static inline void bcma_core_mips_early_init(struct bcma_drv_mips *mcore) { }
 #endif
 
 extern u32 bcma_cpu_clock(struct bcma_drv_mips *mcore);
diff --git a/include/linux/bcma/bcma_regs.h b/include/linux/bcma/bcma_regs.h
index 6c9cb93ae3de..7e8104bb7a7e 100644
--- a/include/linux/bcma/bcma_regs.h
+++ b/include/linux/bcma/bcma_regs.h
@@ -85,6 +85,9 @@
                                                          * (2 ZettaBytes), high 32 bits
                                                          */
 
-#define BCMA_SFLASH                     0x1c000000
+#define BCMA_SOC_FLASH1                 0x1fc00000      /* MIPS Flash Region 1 */
+#define BCMA_SOC_FLASH1_SZ              0x00400000      /* MIPS Size of Flash Region 1 */
+#define BCMA_SOC_FLASH2                 0x1c000000      /* Flash Region 2 (region 1 shadowed here) */
+#define BCMA_SOC_FLASH2_SZ              0x02000000      /* Size of Flash Region 2 */
 
 #endif /* LINUX_BCMA_REGS_H_ */
diff --git a/include/linux/ssb/ssb_driver_mips.h b/include/linux/ssb/ssb_driver_mips.h
index 5f44e9740cd2..07a9c7a2e088 100644
--- a/include/linux/ssb/ssb_driver_mips.h
+++ b/include/linux/ssb/ssb_driver_mips.h
@@ -13,6 +13,12 @@ struct ssb_serial_port {
         unsigned int reg_shift;
 };
 
+struct ssb_pflash {
+        bool present;
+        u8 buswidth;
+        u32 window;
+        u32 window_size;
+};
 
 struct ssb_mipscore {
         struct ssb_device *dev;
@@ -20,9 +26,7 @@ struct ssb_mipscore {
         int nr_serial_ports;
         struct ssb_serial_port serial_ports[4];
 
-        u8 flash_buswidth;
-        u32 flash_window;
-        u32 flash_window_size;
+        struct ssb_pflash pflash;
 };
 
 extern void ssb_mipscore_init(struct ssb_mipscore *mcore);
diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a0705e..42f21766c538 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -19,13 +19,25 @@
 
 #define A2MP_FEAT_EXT   0x8000
 
+enum amp_mgr_state {
+        READ_LOC_AMP_INFO,
+        READ_LOC_AMP_ASSOC,
+        READ_LOC_AMP_ASSOC_FINAL,
+};
+
 struct amp_mgr {
+        struct list_head        list;
         struct l2cap_conn       *l2cap_conn;
         struct l2cap_chan       *a2mp_chan;
+        struct l2cap_chan       *bredr_chan;
         struct kref             kref;
         __u8                    ident;
         __u8                    handle;
+        enum amp_mgr_state      state;
         unsigned long           flags;
+
+        struct list_head        amp_ctrls;
+        struct mutex            amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
@@ -118,9 +130,19 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS            0x05
 #define A2MP_STATUS_SECURITY_VIOLATION          0x06
 
-void amp_mgr_get(struct amp_mgr *mgr);
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
+struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+u8 __next_ident(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
                                        struct sk_buff *skb);
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_discover_amp(struct l2cap_chan *chan);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
+void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 000000000000..2e7c79ea0463
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,50 @@
+/*
+   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 __AMP_H
+#define __AMP_H
+
+struct amp_ctrl {
+        struct list_head        list;
+        struct kref             kref;
+        __u8                    id;
+        __u16                   assoc_len_so_far;
+        __u16                   assoc_rem_len;
+        __u16                   assoc_len;
+        __u8                    *assoc;
+};
+
+int amp_ctrl_put(struct amp_ctrl *ctrl);
+void amp_ctrl_get(struct amp_ctrl *ctrl);
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id);
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id);
+void amp_ctrl_list_flush(struct amp_mgr *mgr);
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+                             u8 remote_id, bool out);
+
+int phylink_gen_key(struct hci_conn *hcon, u8 *data, u8 *len, u8 *type);
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+                                   struct hci_conn *hcon);
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon);
+void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon);
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index ede036977ae8..2554b3f5222a 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -180,7 +180,6 @@ static inline void bacpy(bdaddr_t *dst, bdaddr_t *src)
 }
 
 void baswap(bdaddr_t *dst, bdaddr_t *src);
-char *batostr(bdaddr_t *ba);
 
 /* Common socket structures and functions */
 
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 76b2b6bdcf36..88cbbda61027 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE       16
 #define HCI_AMP_LINK_KEY_SIZE   (2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE  672
+
 /* HCI dev events */
 #define HCI_DEV_REG                     1
 #define HCI_DEV_UNREG                   2
@@ -196,6 +198,7 @@ enum {
 #define ACL_START_NO_FLUSH      0x00
 #define ACL_CONT                0x01
 #define ACL_START               0x02
+#define ACL_COMPLETE            0x03
 #define ACL_ACTIVE_BCAST        0x04
 #define ACL_PICO_BCAST          0x08
 
@@ -205,6 +208,7 @@ enum {
 #define ESCO_LINK       0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK         0x80
+#define AMP_LINK        0x81
 
 /* LMP features */
 #define LMP_3SLOT       0x01
@@ -556,12 +560,46 @@ struct hci_cp_accept_phy_link {
         __u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK 0x0437
+#define HCI_OP_DISCONN_PHY_LINK         0x0437
 struct hci_cp_disconn_phy_link {
         __u8     phy_handle;
         __u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+        __u8       id;
+        __u8       stype;
+        __le16     msdu;
+        __le32     sdu_itime;
+        __le32     acc_lat;
+        __le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK      0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK      0x0439
+struct hci_cp_create_accept_logical_link {
+        __u8                  phy_handle;
+        struct ext_flow_spec  tx_flow_spec;
+        struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK     0x043a
+struct hci_cp_disconn_logical_link {
+        __le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL      0x043b
+struct hci_cp_logical_link_cancel {
+        __u8     phy_handle;
+        __u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+        __u8     status;
+        __u8     phy_handle;
+        __u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE               0x0803
 struct hci_cp_sniff_mode {
         __le16   handle;
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e7d454609881..9fe8e2dec870 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -73,6 +73,7 @@ struct discovery_state {
 struct hci_conn_hash {
         struct list_head list;
         unsigned int     acl_num;
+        unsigned int     amp_num;
         unsigned int     sco_num;
         unsigned int     le_num;
 };
@@ -124,6 +125,14 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH       10
 
+struct amp_assoc {
+        __u16   len;
+        __u16   offset;
+        __u16   rem_len;
+        __u16   len_so_far;
+        __u8    data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
         struct list_head list;
@@ -177,6 +186,8 @@ struct hci_dev {
         __u32           amp_max_flush_to;
         __u32           amp_be_flush_to;
 
+        struct amp_assoc        loc_assoc;
+
         __u8            flow_ctl_mode;
 
         unsigned int    auto_accept_delay;
@@ -252,8 +263,6 @@ struct hci_dev {
 
         struct sk_buff_head     driver_init;
 
-        void                    *core_data;
-
         atomic_t                promisc;
 
         struct dentry           *debugfs;
@@ -277,6 +286,8 @@ struct hci_dev {
         int (*ioctl)(struct hci_dev *hdev, unsigned int cmd, unsigned long arg);
 };
 
+#define HCI_PHY_HANDLE(handle)  (handle & 0xff)
+
 struct hci_conn {
         struct list_head list;
 
@@ -310,6 +321,7 @@ struct hci_conn {
 
         __u8            remote_cap;
         __u8            remote_auth;
+        __u8            remote_id;
         bool            flush_key;
 
         unsigned int    sent;
@@ -339,7 +351,7 @@ struct hci_conn {
 
 struct hci_chan {
         struct list_head list;
-
+        __u16 handle;
         struct hci_conn *conn;
         struct sk_buff_head data_q;
         unsigned int    sent;
@@ -438,6 +450,9 @@ static inline void hci_conn_hash_add(struct hci_dev *hdev, struct hci_conn *c)
         case ACL_LINK:
                 h->acl_num++;
                 break;
+        case AMP_LINK:
+                h->amp_num++;
+                break;
         case LE_LINK:
                 h->le_num++;
                 break;
@@ -459,6 +474,9 @@ static inline void hci_conn_hash_del(struct hci_dev *hdev, struct hci_conn *c)
         case ACL_LINK:
                 h->acl_num--;
                 break;
+        case AMP_LINK:
+                h->amp_num--;
+                break;
         case LE_LINK:
                 h->le_num--;
                 break;
@@ -475,6 +493,8 @@ static inline unsigned int hci_conn_num(struct hci_dev *hdev, __u8 type)
         switch (type) {
         case ACL_LINK:
                 return h->acl_num;
+        case AMP_LINK:
+                return h->amp_num;
         case LE_LINK:
                 return h->le_num;
         case SCO_LINK:
@@ -556,6 +576,7 @@ void hci_conn_check_pending(struct hci_dev *hdev);
 struct hci_chan *hci_chan_create(struct hci_conn *conn);
 void hci_chan_del(struct hci_chan *chan);
 void hci_chan_list_flush(struct hci_conn *conn);
+struct hci_chan *hci_chan_lookup_handle(struct hci_dev *hdev, __u16 handle);
 
 struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
                              __u8 dst_type, __u8 sec_level, __u8 auth_type);
@@ -584,7 +605,10 @@ static inline void hci_conn_put(struct hci_conn *conn)
 
         if (atomic_dec_and_test(&conn->refcnt)) {
                 unsigned long timeo;
-                if (conn->type == ACL_LINK || conn->type == LE_LINK) {
+
+                switch (conn->type) {
+                case ACL_LINK:
+                case LE_LINK:
                         del_timer(&conn->idle_timer);
                         if (conn->state == BT_CONNECTED) {
                                 timeo = conn->disc_timeout;
@@ -593,12 +617,20 @@ static inline void hci_conn_put(struct hci_conn *conn)
                         } else {
                                 timeo = msecs_to_jiffies(10);
                         }
-                } else {
+                        break;
+
+                case AMP_LINK:
+                        timeo = conn->disc_timeout;
+                        break;
+
+                default:
                         timeo = msecs_to_jiffies(10);
+                        break;
                 }
+
                 cancel_delayed_work(&conn->disc_work);
                 queue_delayed_work(conn->hdev->workqueue,
-                                        &conn->disc_work, timeo);
+                                   &conn->disc_work, timeo);
         }
 }
 
@@ -789,6 +821,10 @@ static inline void hci_proto_disconn_cfm(struct hci_conn *conn, __u8 reason)
                 sco_disconn_cfm(conn, reason);
                 break;
 
+        /* L2CAP would be handled for BREDR chan */
+        case AMP_LINK:
+                break;
+
         default:
                 BT_ERR("unknown link type %d", conn->type);
                 break;
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7ed8e356425a..6e23afdf65c1 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -32,13 +32,14 @@
 /* L2CAP defaults */
 #define L2CAP_DEFAULT_MTU               672
 #define L2CAP_DEFAULT_MIN_MTU           48
-#define L2CAP_DEFAULT_FLUSH_TO          0xffff
+#define L2CAP_DEFAULT_FLUSH_TO          0xFFFF
+#define L2CAP_EFS_DEFAULT_FLUSH_TO      0xFFFFFFFF
 #define L2CAP_DEFAULT_TX_WINDOW         63
 #define L2CAP_DEFAULT_EXT_WINDOW        0x3FFF
 #define L2CAP_DEFAULT_MAX_TX            3
 #define L2CAP_DEFAULT_RETRANS_TO        2000    /* 2 seconds */
 #define L2CAP_DEFAULT_MONITOR_TO        12000   /* 12 seconds */
-#define L2CAP_DEFAULT_MAX_PDU_SIZE      1009    /* Sized for 3-DH5 packet */
+#define L2CAP_DEFAULT_MAX_PDU_SIZE      1492    /* Sized for AMP packet */
 #define L2CAP_DEFAULT_ACK_TO            200
 #define L2CAP_DEFAULT_MAX_SDU_SIZE      0xFFFF
 #define L2CAP_DEFAULT_SDU_ITIME         0xFFFFFFFF
@@ -508,6 +509,8 @@ struct l2cap_chan {
         __u32           remote_acc_lat;
         __u32           remote_flush_to;
 
+        __u8            ctrl_id;
+
         struct delayed_work     chan_timer;
         struct delayed_work     retrans_timer;
         struct delayed_work     monitor_timer;
@@ -538,6 +541,7 @@ struct l2cap_ops {
         void                    (*state_change) (struct l2cap_chan *chan,
                                                  int state);
         void                    (*ready) (struct l2cap_chan *chan);
+        void                    (*defer) (struct l2cap_chan *chan);
         struct sk_buff          *(*alloc_skb) (struct l2cap_chan *chan,
                                                unsigned long len, int nb);
 };
@@ -745,6 +749,10 @@ static inline void l2cap_chan_no_ready(struct l2cap_chan *chan)
 {
 }
 
+static inline void l2cap_chan_no_defer(struct l2cap_chan *chan)
+{
+}
+
 extern bool disable_ertm;
 
 int l2cap_init_sockets(void);
@@ -767,6 +775,8 @@ 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_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index aa0e4a12308c..c6964572890f 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -1232,6 +1232,7 @@ struct cfg80211_deauth_request {
         const u8 *ie;
         size_t ie_len;
         u16 reason_code;
+        bool local_state_change;
 };
 
 /**
@@ -2665,6 +2666,15 @@ unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb);
 unsigned int __attribute_const__ ieee80211_hdrlen(__le16 fc);
 
 /**
+ * ieee80211_get_mesh_hdrlen - get mesh extension header length
+ * @meshhdr: the mesh extension header, only the flags field
+ *      (first byte) will be accessed
+ * Returns the length of the extension header, which is always at
+ * least 6 bytes and at most 18 if address 5 and 6 are present.
+ */
+unsigned int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr);
+
+/**
  * DOC: Data path helpers
  *
  * In addition to generic utilities, cfg80211 also offers
diff --git a/include/net/nfc/hci.h b/include/net/nfc/hci.h
index e900072950cb..639f50af42df 100644
--- a/include/net/nfc/hci.h
+++ b/include/net/nfc/hci.h
@@ -24,6 +24,12 @@
 
 #include <net/nfc/nfc.h>
 
+struct nfc_phy_ops {
+        int (*write)(void *dev_id, struct sk_buff *skb);
+        int (*enable)(void *dev_id);
+        void (*disable)(void *dev_id);
+};
+
 struct nfc_hci_dev;
 
 struct nfc_hci_ops {
@@ -38,15 +44,21 @@ struct nfc_hci_ops {
         int (*xmit) (struct nfc_hci_dev *hdev, struct sk_buff *skb);
         int (*start_poll) (struct nfc_hci_dev *hdev,
                            u32 im_protocols, u32 tm_protocols);
+        int (*dep_link_up)(struct nfc_hci_dev *hdev, struct nfc_target *target,
+                           u8 comm_mode, u8 *gb, size_t gb_len);
+        int (*dep_link_down)(struct nfc_hci_dev *hdev);
         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,
                                            struct nfc_target *target);
-        int (*data_exchange) (struct nfc_hci_dev *hdev,
+        int (*im_transceive) (struct nfc_hci_dev *hdev,
                               struct nfc_target *target, struct sk_buff *skb,
                               data_exchange_cb_t cb, void *cb_context);
+        int (*tm_send)(struct nfc_hci_dev *hdev, struct sk_buff *skb);
         int (*check_presence)(struct nfc_hci_dev *hdev,
                               struct nfc_target *target);
+        void (*event_received)(struct nfc_hci_dev *hdev, u8 gate, u8 event,
+                                struct sk_buff *skb);
 };
 
 /* Pipes */
@@ -114,6 +126,9 @@ struct nfc_hci_dev {
         int async_cb_type;
         data_exchange_cb_t async_cb;
         void *async_cb_context;
+
+        u8 *gb;
+        size_t gb_len;
 };
 
 /* hci device allocation */
@@ -219,5 +234,6 @@ int nfc_hci_send_response(struct nfc_hci_dev *hdev, u8 gate, u8 response,
                           const u8 *param, size_t param_len);
 int nfc_hci_send_event(struct nfc_hci_dev *hdev, u8 gate, u8 event,
                        const u8 *param, size_t param_len);
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate);
 
 #endif /* __NET_HCI_H */
diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index f05b10682c9d..fce80b2f9be7 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -95,7 +95,7 @@ struct nfc_genl_data {
 };
 
 struct nfc_dev {
-        unsigned int idx;
+        int idx;
         u32 target_next_idx;
         struct nfc_target *targets;
         int n_targets;
diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h
index d908d17da56d..0e63cee8d810 100644
--- a/include/uapi/linux/nfc.h
+++ b/include/uapi/linux/nfc.h
@@ -60,6 +60,13 @@
  *      target mode.
  * @NFC_EVENT_DEVICE_DEACTIVATED: event emitted when the adapter is deactivated
  *      from target mode.
+ * @NFC_CMD_LLC_GET_PARAMS: request LTO, RW, and MIUX parameters for a device
+ * @NFC_CMD_LLC_SET_PARAMS: set one or more of LTO, RW, and MIUX parameters for
+ *      a device. LTO must be set before the link is up otherwise -EINPROGRESS
+ *      is returned. RW and MIUX can be set at anytime and will be passed in
+ *      subsequent CONNECT and CC messages.
+ *      If one of the passed parameters is wrong none is set and -EINVAL is
+ *      returned.
  */
 enum nfc_commands {
         NFC_CMD_UNSPEC,
@@ -77,6 +84,8 @@ enum nfc_commands {
         NFC_EVENT_TARGET_LOST,
         NFC_EVENT_TM_ACTIVATED,
         NFC_EVENT_TM_DEACTIVATED,
+        NFC_CMD_LLC_GET_PARAMS,
+        NFC_CMD_LLC_SET_PARAMS,
 /* private: internal use only */
         __NFC_CMD_AFTER_LAST
 };
@@ -102,6 +111,9 @@ enum nfc_commands {
  * @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
+ * @NFC_ATTR_LLC_PARAM_LTO: Link TimeOut parameter
+ * @NFC_ATTR_LLC_PARAM_RW: Receive Window size parameter
+ * @NFC_ATTR_LLC_PARAM_MIUX: MIU eXtension parameter
  */
 enum nfc_attrs {
         NFC_ATTR_UNSPEC,
@@ -119,6 +131,9 @@ enum nfc_attrs {
         NFC_ATTR_DEVICE_POWERED,
         NFC_ATTR_IM_PROTOCOLS,
         NFC_ATTR_TM_PROTOCOLS,
+        NFC_ATTR_LLC_PARAM_LTO,
+        NFC_ATTR_LLC_PARAM_RW,
+        NFC_ATTR_LLC_PARAM_MIUX,
 /* private: internal use only */
         __NFC_ATTR_AFTER_LAST
 };
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d385035e..1c11d0dcd863 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
         select CRYPTO_BLKCIPHER
         select CRYPTO_AES
         select CRYPTO_ECB
+        select CRYPTO_SHA256
         help
           Bluetooth is low-cost, low-power, short-range wireless technology.
           It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a4602a..dea6a287daca 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ 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 \
-        a2mp.o
+        a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1fed6f0..d5136cfb57e2 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,11 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +42,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
         return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-                      void *data)
+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;
@@ -63,6 +67,14 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
         kfree(cmd);
 }
 
+u8 __next_ident(struct amp_mgr *mgr)
+{
+        if (++mgr->ident == 0)
+                mgr->ident = 1;
+
+        return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
         cl->id = 0;
@@ -161,6 +173,83 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
         return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                             struct a2mp_cmd *hdr)
+{
+        struct a2mp_discov_rsp *rsp = (void *) skb->data;
+        u16 len = le16_to_cpu(hdr->len);
+        struct a2mp_cl *cl;
+        u16 ext_feat;
+        bool found = false;
+
+        if (len < sizeof(*rsp))
+                return -EINVAL;
+
+        len -= sizeof(*rsp);
+        skb_pull(skb, sizeof(*rsp));
+
+        ext_feat = le16_to_cpu(rsp->ext_feat);
+
+        BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->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));
+        }
+
+        cl = (void *) skb->data;
+        while (len >= sizeof(*cl)) {
+                BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+                       cl->status);
+
+                if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+                        struct a2mp_info_req req;
+
+                        found = true;
+                        req.id = cl->id;
+                        a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+                                  sizeof(req), &req);
+                }
+
+                len -= sizeof(*cl);
+                cl = (void *) skb_pull(skb, sizeof(*cl));
+        }
+
+        /* Fall back to L2CAP init sequence */
+        if (!found) {
+                struct l2cap_conn *conn = mgr->l2cap_conn;
+                struct l2cap_chan *chan;
+
+                mutex_lock(&conn->chan_lock);
+
+                list_for_each_entry(chan, &conn->chan_l, list) {
+
+                        BT_DBG("chan %p state %s", chan,
+                               state_to_string(chan->state));
+
+                        if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+                                continue;
+
+                        l2cap_chan_lock(chan);
+
+                        if (chan->state == BT_CONNECT)
+                                l2cap_send_conn_req(chan);
+
+                        l2cap_chan_unlock(chan);
+                }
+
+                mutex_unlock(&conn->chan_lock);
+        }
+
+        return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
                               struct a2mp_cmd *hdr)
 {
@@ -181,7 +270,6 @@ 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))
@@ -189,53 +277,93 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
         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 || hdev->dev_type != HCI_AMP) {
+                struct a2mp_info_rsp rsp;
+
+                rsp.id = req->id;
+                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+                a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+                          &rsp);
+
+                goto done;
         }
 
+        mgr->state = READ_LOC_AMP_INFO;
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+
+done:
         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_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                            struct a2mp_cmd *hdr)
+{
+        struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+        struct a2mp_amp_assoc_req req;
+        struct amp_ctrl *ctrl;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+                return -EINVAL;
+
+        BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+        if (rsp->status)
+                return -EINVAL;
+
+        ctrl = amp_ctrl_add(mgr, rsp->id);
+        if (!ctrl)
+                return -ENOMEM;
+
+        req.id = rsp->id;
+        a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+                  &req);
+
+        skb_pull(skb, sizeof(*rsp));
+        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;
+        struct amp_mgr *tmp;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
 
         BT_DBG("id %d", req->id);
 
+        /* Make sure that other request is not processed */
+        tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
         hdev = hci_dev_get(req->id);
-        if (!hdev || hdev->amp_type == HCI_BREDR) {
+        if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
                 struct a2mp_amp_assoc_rsp rsp;
                 rsp.id = req->id;
-                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+                if (tmp) {
+                        rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+                        amp_mgr_put(tmp);
+                } else {
+                        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+                }
 
                 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
                           &rsp);
-                goto clean;
+
+                goto done;
         }
 
-        /* Placeholder for HCI Read AMP Assoc */
+        amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
         if (hdev)
                 hci_dev_put(hdev);
 
@@ -243,6 +371,68 @@ clean:
         return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                                struct a2mp_cmd *hdr)
+{
+        struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+        u16 len = le16_to_cpu(hdr->len);
+        struct hci_dev *hdev;
+        struct amp_ctrl *ctrl;
+        struct hci_conn *hcon;
+        size_t assoc_len;
+
+        if (len < sizeof(*rsp))
+                return -EINVAL;
+
+        assoc_len = len - sizeof(*rsp);
+
+        BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
+               assoc_len);
+
+        if (rsp->status)
+                return -EINVAL;
+
+        /* Save remote ASSOC data */
+        ctrl = amp_ctrl_lookup(mgr, rsp->id);
+        if (ctrl) {
+                u8 *assoc;
+
+                assoc = kzalloc(assoc_len, GFP_KERNEL);
+                if (!assoc) {
+                        amp_ctrl_put(ctrl);
+                        return -ENOMEM;
+                }
+
+                memcpy(assoc, rsp->amp_assoc, assoc_len);
+                ctrl->assoc = assoc;
+                ctrl->assoc_len = assoc_len;
+                ctrl->assoc_rem_len = assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+        }
+
+        /* Create Phys Link */
+        hdev = hci_dev_get(rsp->id);
+        if (!hdev)
+                return -EINVAL;
+
+        hcon = phylink_add(hdev, mgr, rsp->id, true);
+        if (!hcon)
+                goto done;
+
+        BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+        mgr->bredr_chan->ctrl_id = rsp->id;
+
+        amp_create_phylink(hdev, mgr, hcon);
+
+done:
+        hci_dev_put(hdev);
+        skb_pull(skb, len);
+        return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                                    struct a2mp_cmd *hdr)
 {
@@ -250,6 +440,8 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
         struct a2mp_physlink_rsp rsp;
         struct hci_dev *hdev;
+        struct hci_conn *hcon;
+        struct amp_ctrl *ctrl;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
@@ -265,9 +457,43 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                 goto send_rsp;
         }
 
-        /* TODO process physlink create */
+        ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
+        if (!ctrl) {
+                ctrl = amp_ctrl_add(mgr, rsp.remote_id);
+                if (ctrl) {
+                        amp_ctrl_get(ctrl);
+                } else {
+                        rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+                        goto send_rsp;
+                }
+        }
 
-        rsp.status = A2MP_STATUS_SUCCESS;
+        if (ctrl) {
+                size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
+                u8 *assoc;
+
+                assoc = kzalloc(assoc_len, GFP_KERNEL);
+                if (!assoc) {
+                        amp_ctrl_put(ctrl);
+                        return -ENOMEM;
+                }
+
+                memcpy(assoc, req->amp_assoc, assoc_len);
+                ctrl->assoc = assoc;
+                ctrl->assoc_len = assoc_len;
+                ctrl->assoc_rem_len = assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+        }
+
+        hcon = phylink_add(hdev, mgr, req->local_id, false);
+        if (hcon) {
+                amp_accept_phylink(hdev, mgr, hcon);
+                rsp.status = A2MP_STATUS_SUCCESS;
+        } else {
+                rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+        }
 
 send_rsp:
         if (hdev)
@@ -286,6 +512,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
         struct a2mp_physlink_req *req = (void *) skb->data;
         struct a2mp_physlink_rsp rsp;
         struct hci_dev *hdev;
+        struct hci_conn *hcon;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
@@ -296,14 +523,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
         rsp.remote_id = req->local_id;
         rsp.status = A2MP_STATUS_SUCCESS;
 
-        hdev = hci_dev_get(req->local_id);
+        hdev = hci_dev_get(req->remote_id);
         if (!hdev) {
                 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
                 goto send_rsp;
         }
 
+        hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+        if (!hcon) {
+                BT_ERR("No phys link exist");
+                rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+                goto clean;
+        }
+
         /* TODO Disconnect Phys Link here */
 
+clean:
         hci_dev_put(hdev);
 
 send_rsp:
@@ -377,10 +612,19 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
                         err = a2mp_discphyslink_req(mgr, skb, hdr);
                         break;
 
-                case A2MP_CHANGE_RSP:
                 case A2MP_DISCOVER_RSP:
+                        err = a2mp_discover_rsp(mgr, skb, hdr);
+                        break;
+
                 case A2MP_GETINFO_RSP:
+                        err = a2mp_getinfo_rsp(mgr, skb, hdr);
+                        break;
+
                 case A2MP_GETAMPASSOC_RSP:
+                        err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+                        break;
+
+                case A2MP_CHANGE_RSP:
                 case A2MP_CREATEPHYSLINK_RSP:
                 case A2MP_DISCONNPHYSLINK_RSP:
                         err = a2mp_cmd_rsp(mgr, skb, hdr);
@@ -455,9 +699,10 @@ static struct l2cap_ops a2mp_chan_ops = {
         .new_connection = l2cap_chan_no_new_connection,
         .teardown = l2cap_chan_no_teardown,
         .ready = l2cap_chan_no_ready,
+        .defer = l2cap_chan_no_defer,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
         struct l2cap_chan *chan;
         int err;
@@ -492,7 +737,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
         chan->conf_state = 0;
 
-        l2cap_chan_add(conn, chan);
+        if (locked)
+                __l2cap_chan_add(conn, chan);
+        else
+                l2cap_chan_add(conn, chan);
 
         chan->remote_mps = chan->omtu;
         chan->mps = chan->omtu;
@@ -503,11 +751,13 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 }
 
 /* AMP Manager functions */
-void amp_mgr_get(struct amp_mgr *mgr)
+struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
 {
         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 
         kref_get(&mgr->kref);
+
+        return mgr;
 }
 
 static void amp_mgr_destroy(struct kref *kref)
@@ -516,6 +766,11 @@ static void amp_mgr_destroy(struct kref *kref)
 
         BT_DBG("mgr %p", mgr);
 
+        mutex_lock(&amp_mgr_list_lock);
+        list_del(&mgr->list);
+        mutex_unlock(&amp_mgr_list_lock);
+
+        amp_ctrl_list_flush(mgr);
         kfree(mgr);
 }
 
@@ -526,7 +781,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
         return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
         struct amp_mgr *mgr;
         struct l2cap_chan *chan;
@@ -539,7 +794,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
         mgr->l2cap_conn = conn;
 
-        chan = a2mp_chan_open(conn);
+        chan = a2mp_chan_open(conn, locked);
         if (!chan) {
                 kfree(mgr);
                 return NULL;
@@ -552,6 +807,14 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
         kref_init(&mgr->kref);
 
+        /* Remote AMP ctrl list initialization */
+        INIT_LIST_HEAD(&mgr->amp_ctrls);
+        mutex_init(&mgr->amp_ctrls_lock);
+
+        mutex_lock(&amp_mgr_list_lock);
+        list_add(&mgr->list, &amp_mgr_list);
+        mutex_unlock(&amp_mgr_list_lock);
+
         return mgr;
 }
 
@@ -560,7 +823,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
         struct amp_mgr *mgr;
 
-        mgr = amp_mgr_create(conn);
+        mgr = amp_mgr_create(conn, false);
         if (!mgr) {
                 BT_ERR("Could not create AMP manager");
                 return NULL;
@@ -570,3 +833,139 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
         return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+        struct amp_mgr *mgr;
+
+        mutex_lock(&amp_mgr_list_lock);
+        list_for_each_entry(mgr, &amp_mgr_list, list) {
+                if (mgr->state == state) {
+                        amp_mgr_get(mgr);
+                        mutex_unlock(&amp_mgr_list_lock);
+                        return mgr;
+                }
+        }
+        mutex_unlock(&amp_mgr_list_lock);
+
+        return NULL;
+}
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+        struct amp_mgr *mgr;
+        struct a2mp_info_rsp rsp;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+        if (!mgr)
+                return;
+
+        BT_DBG("%s mgr %p", hdev->name, mgr);
+
+        rsp.id = hdev->id;
+        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+        if (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);
+        }
+
+        a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+        amp_mgr_put(mgr);
+}
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+        struct amp_mgr *mgr;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+        struct a2mp_amp_assoc_rsp *rsp;
+        size_t len;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+        if (!mgr)
+                return;
+
+        BT_DBG("%s mgr %p", hdev->name, mgr);
+
+        len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+        rsp = kzalloc(len, GFP_KERNEL);
+        if (!rsp) {
+                amp_mgr_put(mgr);
+                return;
+        }
+
+        rsp->id = hdev->id;
+
+        if (status) {
+                rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+        } else {
+                rsp->status = A2MP_STATUS_SUCCESS;
+                memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+        }
+
+        a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+        amp_mgr_put(mgr);
+        kfree(rsp);
+}
+
+void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
+{
+        struct amp_mgr *mgr;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+        struct a2mp_physlink_req *req;
+        struct l2cap_chan *bredr_chan;
+        size_t len;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
+        if (!mgr)
+                return;
+
+        len = sizeof(*req) + loc_assoc->len;
+
+        BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
+
+        req = kzalloc(len, GFP_KERNEL);
+        if (!req) {
+                amp_mgr_put(mgr);
+                return;
+        }
+
+        bredr_chan = mgr->bredr_chan;
+        if (!bredr_chan)
+                goto clean;
+
+        req->local_id = hdev->id;
+        req->remote_id = bredr_chan->ctrl_id;
+        memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+        a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+        amp_mgr_put(mgr);
+        kfree(req);
+}
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+        struct l2cap_conn *conn = chan->conn;
+        struct amp_mgr *mgr = conn->hcon->amp_mgr;
+        struct a2mp_discov_req req;
+
+        BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+        if (!mgr) {
+                mgr = amp_mgr_create(conn, true);
+                if (!mgr)
+                        return;
+        }
+
+        mgr->bredr_chan = chan;
+
+        req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+        req.ext_feat = 0;
+        a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index ba033f09196e..5355df63d39b 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -569,7 +569,6 @@ static int bt_seq_show(struct seq_file *seq, void *v)
 {
         struct bt_seq_state *s = seq->private;
         struct bt_sock_list *l = s->l;
-        bdaddr_t src_baswapped, dst_baswapped;
 
         if (v == SEQ_START_TOKEN) {
                 seq_puts(seq ,"sk               RefCnt Rmem   Wmem   User   Inode  Src Dst Parent");
@@ -583,18 +582,17 @@ static int bt_seq_show(struct seq_file *seq, void *v)
         } else {
                 struct sock *sk = sk_entry(v);
                 struct bt_sock *bt = bt_sk(sk);
-                baswap(&src_baswapped, &bt->src);
-                baswap(&dst_baswapped, &bt->dst);
 
-                seq_printf(seq, "%pK %-6d %-6u %-6u %-6u %-6lu %pM %pM %-6lu",
+                seq_printf(seq,
+                           "%pK %-6d %-6u %-6u %-6u %-6lu %pMR %pMR %-6lu",
                            sk,
                            atomic_read(&sk->sk_refcnt),
                            sk_rmem_alloc_get(sk),
                            sk_wmem_alloc_get(sk),
                            from_kuid(seq_user_ns(seq), sock_i_uid(sk)),
                            sock_i_ino(sk),
-                           &src_baswapped,
-                           &dst_baswapped,
+                           &bt->src,
+                           &bt->dst,
                            bt->parent? sock_i_ino(bt->parent): 0LU);
 
                 if (l->custom_seq_show) {
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 000000000000..231d7ef53ecb
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,374 @@
+/*
+   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.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+#include <crypto/hash.h>
+
+/* Remote AMP Controllers interface */
+void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+        BT_DBG("ctrl %p orig refcnt %d", ctrl,
+               atomic_read(&ctrl->kref.refcount));
+
+        kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+        struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+        BT_DBG("ctrl %p", ctrl);
+
+        kfree(ctrl->assoc);
+        kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+        BT_DBG("ctrl %p orig refcnt %d", ctrl,
+               atomic_read(&ctrl->kref.refcount));
+
+        return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id)
+{
+        struct amp_ctrl *ctrl;
+
+        ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+        if (!ctrl)
+                return NULL;
+
+        kref_init(&ctrl->kref);
+        ctrl->id = id;
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_add(&ctrl->list, &mgr->amp_ctrls);
+        mutex_unlock(&mgr->amp_ctrls_lock);
+
+        BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+        return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+        struct amp_ctrl *ctrl, *n;
+
+        BT_DBG("mgr %p", mgr);
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+                list_del(&ctrl->list);
+                amp_ctrl_put(ctrl);
+        }
+        mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+        struct amp_ctrl *ctrl;
+
+        BT_DBG("mgr %p id %d", mgr, id);
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+                if (ctrl->id == id) {
+                        amp_ctrl_get(ctrl);
+                        mutex_unlock(&mgr->amp_ctrls_lock);
+                        return ctrl;
+                }
+        }
+        mutex_unlock(&mgr->amp_ctrls_lock);
+
+        return NULL;
+}
+
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+        if (++mgr->handle == 0)
+                mgr->handle = 1;
+
+        return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+                             u8 remote_id, bool out)
+{
+        bdaddr_t *dst = mgr->l2cap_conn->dst;
+        struct hci_conn *hcon;
+
+        hcon = hci_conn_add(hdev, AMP_LINK, dst);
+        if (!hcon)
+                return NULL;
+
+        BT_DBG("hcon %p dst %pMR", hcon, dst);
+
+        hcon->state = BT_CONNECT;
+        hcon->attempt++;
+        hcon->handle = __next_handle(mgr);
+        hcon->remote_id = remote_id;
+        hcon->amp_mgr = amp_mgr_get(mgr);
+        hcon->out = out;
+
+        return hcon;
+}
+
+/* AMP crypto key generation interface */
+static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+        int ret = 0;
+        struct crypto_shash *tfm;
+
+        if (!ksize)
+                return -EINVAL;
+
+        tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+        if (IS_ERR(tfm)) {
+                BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+                return PTR_ERR(tfm);
+        }
+
+        ret = crypto_shash_setkey(tfm, key, ksize);
+        if (ret) {
+                BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+        } else {
+                struct {
+                        struct shash_desc shash;
+                        char ctx[crypto_shash_descsize(tfm)];
+                } desc;
+
+                desc.shash.tfm = tfm;
+                desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+                ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+                                          output);
+        }
+
+        crypto_free_shash(tfm);
+        return ret;
+}
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct link_key *key;
+        u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+        u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+        int err;
+
+        if (!hci_conn_check_link_mode(conn))
+                return -EACCES;
+
+        BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+        /* Legacy key */
+        if (conn->key_type < 3) {
+                BT_ERR("Legacy key type %d", conn->key_type);
+                return -EACCES;
+        }
+
+        *type = conn->key_type;
+        *len = HCI_AMP_LINK_KEY_SIZE;
+
+        key = hci_find_link_key(hdev, &conn->dst);
+        if (!key) {
+                BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst);
+                return -EACCES;
+        }
+
+        /* BR/EDR Link Key concatenated together with itself */
+        memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+        memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+        /* Derive Generic AMP Link Key (gamp) */
+        err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+        if (err) {
+                BT_ERR("Could not derive Generic AMP Key: err %d", err);
+                return err;
+        }
+
+        if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+                BT_DBG("Use Generic AMP Key (gamp)");
+                memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+                return err;
+        }
+
+        /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+        return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+        BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+        cp.phy_handle = phy_handle;
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+        cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+
+        memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+        memset(&cp, 0, sizeof(cp));
+
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+        mgr->state = READ_LOC_AMP_ASSOC;
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+                                   struct hci_conn *hcon)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+        struct amp_mgr *mgr = hcon->amp_mgr;
+
+        cp.phy_handle = hcon->handle;
+        cp.len_so_far = cpu_to_le16(0);
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+        mgr->state = READ_LOC_AMP_ASSOC_FINAL;
+
+        /* Read Local AMP Assoc final link information data */
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+                                     struct hci_conn *hcon)
+{
+        struct hci_cp_write_remote_amp_assoc *cp;
+        struct amp_mgr *mgr = hcon->amp_mgr;
+        struct amp_ctrl *ctrl;
+        u16 frag_len, len;
+
+        ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+        if (!ctrl)
+                return false;
+
+        if (!ctrl->assoc_rem_len) {
+                BT_DBG("all fragments are written");
+                ctrl->assoc_rem_len = ctrl->assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+                return true;
+        }
+
+        frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+        len = frag_len + sizeof(*cp);
+
+        cp = kzalloc(len, GFP_KERNEL);
+        if (!cp) {
+                amp_ctrl_put(ctrl);
+                return false;
+        }
+
+        BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+               hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+        cp->phy_handle = hcon->handle;
+        cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+        cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+        memcpy(cp->frag, ctrl->assoc, frag_len);
+
+        ctrl->assoc_len_so_far += frag_len;
+        ctrl->assoc_rem_len -= frag_len;
+
+        amp_ctrl_put(ctrl);
+
+        hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+        kfree(cp);
+
+        return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+        struct hci_conn *hcon;
+
+        BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, handle);
+        if (!hcon)
+                return;
+
+        amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+        struct hci_conn *hcon;
+
+        BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, handle);
+        if (!hcon)
+                return;
+
+        BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+        amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon)
+{
+        struct hci_cp_create_phy_link cp;
+
+        cp.phy_handle = hcon->handle;
+
+        BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+               hcon->handle);
+
+        if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+                            &cp.key_type)) {
+                BT_DBG("Cannot create link key");
+                return;
+        }
+
+        hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+
+void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon)
+{
+        struct hci_cp_accept_phy_link cp;
+
+        cp.phy_handle = hcon->handle;
+
+        BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+               hcon->handle);
+
+        if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+                            &cp.key_type)) {
+                BT_DBG("Cannot create link key");
+                return;
+        }
+
+        hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index 4a6620bc1570..a5b639702637 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -182,8 +182,7 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
                         a2 = data;
                         data += ETH_ALEN;
 
-                        BT_DBG("mc filter %s -> %s",
-                                batostr((void *) a1), batostr((void *) a2));
+                        BT_DBG("mc filter %pMR -> %pMR", a1, a2);
 
                         /* Iterate from a1 to a2 */
                         set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
diff --git a/net/bluetooth/cmtp/core.c b/net/bluetooth/cmtp/core.c
index 6c9c1fd601ca..e0a6ebf2baa6 100644
--- a/net/bluetooth/cmtp/core.c
+++ b/net/bluetooth/cmtp/core.c
@@ -353,7 +353,7 @@ int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 
         BT_DBG("mtu %d", session->mtu);
 
-        sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst));
+        sprintf(session->name, "%pMR", &bt_sk(sock->sk)->dst);
 
         session->sock  = sock;
         session->state = BT_CONFIG;
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index b9196a44f759..fe646211c61f 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -130,6 +130,20 @@ void hci_acl_disconn(struct hci_conn *conn, __u8 reason)
         hci_send_cmd(conn->hdev, HCI_OP_DISCONNECT, sizeof(cp), &cp);
 }
 
+static void hci_amp_disconn(struct hci_conn *conn, __u8 reason)
+{
+        struct hci_cp_disconn_phy_link cp;
+
+        BT_DBG("hcon %p", conn);
+
+        conn->state = BT_DISCONN;
+
+        cp.phy_handle = HCI_PHY_HANDLE(conn->handle);
+        cp.reason = reason;
+        hci_send_cmd(conn->hdev, HCI_OP_DISCONN_PHY_LINK,
+                     sizeof(cp), &cp);
+}
+
 static void hci_add_sco(struct hci_conn *conn, __u16 handle)
 {
         struct hci_dev *hdev = conn->hdev;
@@ -230,11 +244,24 @@ void hci_sco_setup(struct hci_conn *conn, __u8 status)
         }
 }
 
+static void hci_conn_disconnect(struct hci_conn *conn)
+{
+        __u8 reason = hci_proto_disconn_ind(conn);
+
+        switch (conn->type) {
+        case ACL_LINK:
+                hci_acl_disconn(conn, reason);
+                break;
+        case AMP_LINK:
+                hci_amp_disconn(conn, reason);
+                break;
+        }
+}
+
 static void hci_conn_timeout(struct work_struct *work)
 {
         struct hci_conn *conn = container_of(work, struct hci_conn,
                                              disc_work.work);
-        __u8 reason;
 
         BT_DBG("hcon %p state %s", conn, state_to_string(conn->state));
 
@@ -253,8 +280,7 @@ static void hci_conn_timeout(struct work_struct *work)
                 break;
         case BT_CONFIG:
         case BT_CONNECTED:
-                reason = hci_proto_disconn_ind(conn);
-                hci_acl_disconn(conn, reason);
+                hci_conn_disconnect(conn);
                 break;
         default:
                 conn->state = BT_CLOSED;
@@ -320,7 +346,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 {
         struct hci_conn *conn;
 
-        BT_DBG("%s dst %s", hdev->name, batostr(dst));
+        BT_DBG("%s dst %pMR", hdev->name, dst);
 
         conn = kzalloc(sizeof(struct hci_conn), GFP_KERNEL);
         if (!conn)
@@ -437,7 +463,7 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
         int use_src = bacmp(src, BDADDR_ANY);
         struct hci_dev *hdev = NULL, *d;
 
-        BT_DBG("%s -> %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         read_lock(&hci_dev_list_lock);
 
@@ -567,7 +593,7 @@ static struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type,
 struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
                              __u8 dst_type, __u8 sec_level, __u8 auth_type)
 {
-        BT_DBG("%s dst %s type 0x%x", hdev->name, batostr(dst), type);
+        BT_DBG("%s dst %pMR type 0x%x", hdev->name, dst, type);
 
         switch (type) {
         case LE_LINK:
@@ -963,3 +989,35 @@ void hci_chan_list_flush(struct hci_conn *conn)
         list_for_each_entry_safe(chan, n, &conn->chan_list, list)
                 hci_chan_del(chan);
 }
+
+static struct hci_chan *__hci_chan_lookup_handle(struct hci_conn *hcon,
+                                                 __u16 handle)
+{
+        struct hci_chan *hchan;
+
+        list_for_each_entry(hchan, &hcon->chan_list, list) {
+                if (hchan->handle == handle)
+                        return hchan;
+        }
+
+        return NULL;
+}
+
+struct hci_chan *hci_chan_lookup_handle(struct hci_dev *hdev, __u16 handle)
+{
+        struct hci_conn_hash *h = &hdev->conn_hash;
+        struct hci_conn *hcon;
+        struct hci_chan *hchan = NULL;
+
+        rcu_read_lock();
+
+        list_for_each_entry_rcu(hcon, &h->list, list) {
+                hchan = __hci_chan_lookup_handle(hcon, handle);
+                if (hchan)
+                        break;
+        }
+
+        rcu_read_unlock();
+
+        return hchan;
+}
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 8a0ce706aebd..5a3f941b610f 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -405,7 +405,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p, %s", cache, batostr(bdaddr));
+        BT_DBG("cache %p, %pMR", cache, bdaddr);
 
         list_for_each_entry(e, &cache->all, all) {
                 if (!bacmp(&e->data.bdaddr, bdaddr))
@@ -421,7 +421,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup_unknown(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p, %s", cache, batostr(bdaddr));
+        BT_DBG("cache %p, %pMR", cache, bdaddr);
 
         list_for_each_entry(e, &cache->unknown, list) {
                 if (!bacmp(&e->data.bdaddr, bdaddr))
@@ -438,7 +438,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup_resolve(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p bdaddr %s state %d", cache, batostr(bdaddr), state);
+        BT_DBG("cache %p bdaddr %pMR state %d", cache, bdaddr, state);
 
         list_for_each_entry(e, &cache->resolve, list) {
                 if (!bacmp(bdaddr, BDADDR_ANY) && e->name_state == state)
@@ -475,7 +475,7 @@ bool hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *ie;
 
-        BT_DBG("cache %p, %s", cache, batostr(&data->bdaddr));
+        BT_DBG("cache %p, %pMR", cache, &data->bdaddr);
 
         if (ssp)
                 *ssp = data->ssp_mode;
@@ -1259,7 +1259,7 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
                 list_add(&key->list, &hdev->link_keys);
         }
 
-        BT_DBG("%s key for %s type %u", hdev->name, batostr(bdaddr), type);
+        BT_DBG("%s key for %pMR type %u", hdev->name, bdaddr, type);
 
         /* Some buggy controller combinations generate a changed
          * combination key for legacy pairing even when there's no
@@ -1338,7 +1338,7 @@ int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
         if (!key)
                 return -ENOENT;
 
-        BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
         list_del(&key->list);
         kfree(key);
@@ -1354,7 +1354,7 @@ int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr)
                 if (bacmp(bdaddr, &k->bdaddr))
                         continue;
 
-                BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+                BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
                 list_del(&k->list);
                 kfree(k);
@@ -1401,7 +1401,7 @@ int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr)
         if (!data)
                 return -ENOENT;
 
-        BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
         list_del(&data->list);
         kfree(data);
@@ -1440,7 +1440,7 @@ int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *hash,
         memcpy(data->hash, hash, sizeof(data->hash));
         memcpy(data->randomizer, randomizer, sizeof(data->randomizer));
 
-        BT_DBG("%s for %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s for %pMR", hdev->name, bdaddr);
 
         return 0;
 }
@@ -2153,9 +2153,10 @@ static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
         hdr->dlen   = cpu_to_le16(len);
 }
 
-static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
+static void hci_queue_acl(struct hci_chan *chan, struct sk_buff_head *queue,
                           struct sk_buff *skb, __u16 flags)
 {
+        struct hci_conn *conn = chan->conn;
         struct hci_dev *hdev = conn->hdev;
         struct sk_buff *list;
 
@@ -2163,7 +2164,18 @@ static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
         skb->data_len = 0;
 
         bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
-        hci_add_acl_hdr(skb, conn->handle, flags);
+
+        switch (hdev->dev_type) {
+        case HCI_BREDR:
+                hci_add_acl_hdr(skb, conn->handle, flags);
+                break;
+        case HCI_AMP:
+                hci_add_acl_hdr(skb, chan->handle, flags);
+                break;
+        default:
+                BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type);
+                return;
+        }
 
         list = skb_shinfo(skb)->frag_list;
         if (!list) {
@@ -2202,14 +2214,13 @@ static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
 
 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 {
-        struct hci_conn *conn = chan->conn;
-        struct hci_dev *hdev = conn->hdev;
+        struct hci_dev *hdev = chan->conn->hdev;
 
         BT_DBG("%s chan %p flags 0x%4.4x", hdev->name, chan, flags);
 
         skb->dev = (void *) hdev;
 
-        hci_queue_acl(conn, &chan->data_q, skb, flags);
+        hci_queue_acl(chan, &chan->data_q, skb, flags);
 
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
@@ -2311,8 +2322,8 @@ static void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
         /* Kill stalled connections */
         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));
+                        BT_ERR("%s killing stalled connection %pMR",
+                               hdev->name, &c->dst);
                         hci_acl_disconn(c, HCI_ERROR_REMOTE_USER_TERM);
                 }
         }
@@ -2381,6 +2392,9 @@ static struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
         case ACL_LINK:
                 cnt = hdev->acl_cnt;
                 break;
+        case AMP_LINK:
+                cnt = hdev->block_cnt;
+                break;
         case SCO_LINK:
         case ESCO_LINK:
                 cnt = hdev->sco_cnt;
@@ -2510,11 +2524,19 @@ static void hci_sched_acl_blk(struct hci_dev *hdev)
         struct hci_chan *chan;
         struct sk_buff *skb;
         int quote;
+        u8 type;
 
         __check_timeout(hdev, cnt);
 
+        BT_DBG("%s", hdev->name);
+
+        if (hdev->dev_type == HCI_AMP)
+                type = AMP_LINK;
+        else
+                type = ACL_LINK;
+
         while (hdev->block_cnt > 0 &&
-               (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
+               (chan = hci_chan_sent(hdev, type, &quote))) {
                 u32 priority = (skb_peek(&chan->data_q))->priority;
                 while (quote > 0 && (skb = skb_peek(&chan->data_q))) {
                         int blocks;
@@ -2547,14 +2569,19 @@ static void hci_sched_acl_blk(struct hci_dev *hdev)
         }
 
         if (cnt != hdev->block_cnt)
-                hci_prio_recalculate(hdev, ACL_LINK);
+                hci_prio_recalculate(hdev, type);
 }
 
 static void hci_sched_acl(struct hci_dev *hdev)
 {
         BT_DBG("%s", hdev->name);
 
-        if (!hci_conn_num(hdev, ACL_LINK))
+        /* No ACL link over BR/EDR controller */
+        if (!hci_conn_num(hdev, ACL_LINK) && hdev->dev_type == HCI_BREDR)
+                return;
+
+        /* No AMP link over AMP controller */
+        if (!hci_conn_num(hdev, AMP_LINK) && hdev->dev_type == HCI_AMP)
                 return;
 
         switch (hdev->flow_ctl_mode) {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 2022b43c7353..0383635f91fb 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,8 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +848,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
         BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
         if (rp->status)
-                return;
+                goto a2mp_rsp;
 
         hdev->amp_status = rp->amp_status;
         hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +862,46 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
         hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
         hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+        a2mp_send_getinfo_rsp(hdev);
+}
+
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+                                        struct sk_buff *skb)
+{
+        struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+        struct amp_assoc *assoc = &hdev->loc_assoc;
+        size_t rem_len, frag_len;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+        if (rp->status)
+                goto a2mp_rsp;
+
+        frag_len = skb->len - sizeof(*rp);
+        rem_len = __le16_to_cpu(rp->rem_len);
+
+        if (rem_len > frag_len) {
+                BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len);
+
+                memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+                assoc->offset += frag_len;
+
+                /* Read other fragments */
+                amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+                return;
+        }
+
+        memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+        assoc->len = assoc->offset + rem_len;
+        assoc->offset = 0;
+
+a2mp_rsp:
+        /* Send A2MP Rsp when all fragments are received */
+        a2mp_send_getampassoc_rsp(hdev, rp->status);
+        a2mp_send_create_phy_link_req(hdev, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
@@ -1174,6 +1216,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
         hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+                                          struct sk_buff *skb)
+{
+        struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+               hdev->name, rp->status, rp->phy_handle);
+
+        if (rp->status)
+                return;
+
+        amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1210,7 +1266,7 @@ static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
 
         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 
-        BT_DBG("%s bdaddr %s hcon %p", hdev->name, batostr(&cp->bdaddr), conn);
+        BT_DBG("%s bdaddr %pMR hcon %p", hdev->name, &cp->bdaddr, conn);
 
         if (status) {
                 if (conn && conn->state == BT_CONNECT) {
@@ -1639,8 +1695,7 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
                         return;
                 }
 
-                BT_DBG("%s bdaddr %s conn %p", hdev->name, batostr(&conn->dst),
-                       conn);
+                BT_DBG("%s bdaddr %pMR conn %p", hdev->name, &conn->dst, conn);
 
                 conn->state = BT_CLOSED;
                 mgmt_connect_failed(hdev, &conn->dst, conn->type,
@@ -1657,6 +1712,38 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+        struct hci_cp_create_phy_link *cp;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        if (status)
+                return;
+
+        cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
+        if (!cp)
+                return;
+
+        amp_write_remote_assoc(hdev, cp->phy_handle);
+}
+
+static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
+{
+        struct hci_cp_accept_phy_link *cp;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        if (status)
+                return;
+
+        cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
+        if (!cp)
+                return;
+
+        amp_write_remote_assoc(hdev, cp->phy_handle);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -1822,7 +1909,7 @@ 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),
+        BT_DBG("%s bdaddr %pMR type 0x%x", hdev->name, &ev->bdaddr,
                ev->link_type);
 
         mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type);
@@ -2314,6 +2401,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_read_local_amp_info(hdev, skb);
                 break;
 
+        case HCI_OP_READ_LOCAL_AMP_ASSOC:
+                hci_cc_read_local_amp_assoc(hdev, skb);
+                break;
+
         case HCI_OP_DELETE_STORED_LINK_KEY:
                 hci_cc_delete_stored_link_key(hdev, skb);
                 break;
@@ -2386,6 +2477,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_write_le_host_supported(hdev, skb);
                 break;
 
+        case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+                hci_cc_write_remote_amp_assoc(hdev, skb);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                 break;
@@ -2467,6 +2562,14 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_le_start_enc(hdev, ev->status);
                 break;
 
+        case HCI_OP_CREATE_PHY_LINK:
+                hci_cs_create_phylink(hdev, ev->status);
+                break;
+
+        case HCI_OP_ACCEPT_PHY_LINK:
+                hci_cs_accept_phylink(hdev, ev->status);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                 break;
@@ -2574,6 +2677,27 @@ static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
 
+static struct hci_conn *__hci_conn_lookup_handle(struct hci_dev *hdev,
+                                                 __u16 handle)
+{
+        struct hci_chan *chan;
+
+        switch (hdev->dev_type) {
+        case HCI_BREDR:
+                return hci_conn_hash_lookup_handle(hdev, handle);
+        case HCI_AMP:
+                chan = hci_chan_lookup_handle(hdev, handle);
+                if (chan)
+                        return chan->conn;
+                break;
+        default:
+                BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type);
+                break;
+        }
+
+        return NULL;
+}
+
 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;
@@ -2595,13 +2719,13 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
         for (i = 0; i < ev->num_hndl; i++) {
                 struct hci_comp_blocks_info *info = &ev->handles[i];
-                struct hci_conn *conn;
+                struct hci_conn *conn = NULL;
                 __u16  handle, block_count;
 
                 handle = __le16_to_cpu(info->handle);
                 block_count = __le16_to_cpu(info->blocks);
 
-                conn = hci_conn_hash_lookup_handle(hdev, handle);
+                conn = __hci_conn_lookup_handle(hdev, handle);
                 if (!conn)
                         continue;
 
@@ -2609,6 +2733,7 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
                 switch (conn->type) {
                 case ACL_LINK:
+                case AMP_LINK:
                         hdev->block_cnt += block_count;
                         if (hdev->block_cnt > hdev->num_blocks)
                                 hdev->block_cnt = hdev->num_blocks;
@@ -2705,13 +2830,13 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
         key = hci_find_link_key(hdev, &ev->bdaddr);
         if (!key) {
-                BT_DBG("%s link key not found for %s", hdev->name,
-                       batostr(&ev->bdaddr));
+                BT_DBG("%s link key not found for %pMR", hdev->name,
+                       &ev->bdaddr);
                 goto not_found;
         }
 
-        BT_DBG("%s found key type %u for %s", hdev->name, key->type,
-               batostr(&ev->bdaddr));
+        BT_DBG("%s found key type %u for %pMR", hdev->name, key->type,
+               &ev->bdaddr);
 
         if (!test_bit(HCI_DEBUG_KEYS, &hdev->dev_flags) &&
             key->type == HCI_LK_DEBUG_COMBINATION) {
@@ -3558,6 +3683,22 @@ static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
         }
 }
 
+static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_channel_selected *ev = (void *) skb->data;
+        struct hci_conn *hcon;
+
+        BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle);
+
+        skb_pull(skb, sizeof(*ev));
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (!hcon)
+                return;
+
+        amp_read_loc_assoc_final_data(hdev, hcon);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3722,6 +3863,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_le_meta_evt(hdev, skb);
                 break;
 
+        case HCI_EV_CHANNEL_SELECTED:
+                hci_chan_selected_evt(hdev, skb);
+                break;
+
         case HCI_EV_REMOTE_OOB_DATA_REQUEST:
                 hci_remote_oob_data_request_evt(hdev, skb);
                 break;
diff --git a/net/bluetooth/hci_sysfs.c b/net/bluetooth/hci_sysfs.c
index a20e61c3653d..55cceee02a84 100644
--- a/net/bluetooth/hci_sysfs.c
+++ b/net/bluetooth/hci_sysfs.c
@@ -38,7 +38,7 @@ 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));
+        return sprintf(buf, "%pMR\n", &conn->dst);
 }
 
 static ssize_t show_link_features(struct device *dev,
@@ -224,7 +224,7 @@ 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));
+        return sprintf(buf, "%pMR\n", &hdev->bdaddr);
 }
 
 static ssize_t show_features(struct device *dev,
@@ -406,8 +406,8 @@ static int inquiry_cache_show(struct seq_file *f, void *p)
 
         list_for_each_entry(e, &cache->all, all) {
                 struct inquiry_data *data = &e->data;
-                seq_printf(f, "%s %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n",
-                           batostr(&data->bdaddr),
+                seq_printf(f, "%pMR %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n",
+                           &data->bdaddr,
                            data->pscan_rep_mode, data->pscan_period_mode,
                            data->pscan_mode, data->dev_class[2],
                            data->dev_class[1], data->dev_class[0],
@@ -440,7 +440,7 @@ static int blacklist_show(struct seq_file *f, void *p)
         hci_dev_lock(hdev);
 
         list_for_each_entry(b, &hdev->blacklist, list)
-                seq_printf(f, "%s\n", batostr(&b->bdaddr));
+                seq_printf(f, "%pMR\n", &b->bdaddr);
 
         hci_dev_unlock(hdev);
 
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index ccd985da6518..0c0028463fa3 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -932,8 +932,12 @@ static int hidp_setup_hid(struct hidp_session *session,
         hid->country = req->country;
 
         strncpy(hid->name, req->name, 128);
-        strncpy(hid->phys, batostr(&bt_sk(session->ctrl_sock->sk)->src), 64);
-        strncpy(hid->uniq, batostr(&bt_sk(session->ctrl_sock->sk)->dst), 64);
+
+        snprintf(hid->phys, sizeof(hid->phys), "%pMR",
+                 &bt_sk(session->ctrl_sock->sk)->src);
+
+        snprintf(hid->uniq, sizeof(hid->uniq), "%pMR",
+                 &bt_sk(session->ctrl_sock->sk)->dst);
 
         hid->dev.parent = &session->conn->dev;
         hid->ll_driver = &hidp_hid_driver;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a91239dcda41..08efc256c931 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -48,19 +48,20 @@ static LIST_HEAD(chan_list);
 static DEFINE_RWLOCK(chan_list_lock);
 
 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
-                                u8 code, u8 ident, u16 dlen, void *data);
+                                       u8 code, u8 ident, u16 dlen, void *data);
 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
-                                                                void *data);
+                           void *data);
 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);
+                     struct sk_buff_head *skbs, u8 event);
 
 /* ---- L2CAP channels ---- */
 
-static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
+                                                   u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -71,7 +72,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16
         return NULL;
 }
 
-static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
+                                                   u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -84,7 +86,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16
 
 /* Find channel with given SCID.
  * Returns locked channel. */
-static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
+                                                 u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -97,7 +100,8 @@ static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 ci
         return c;
 }
 
-static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
+static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
+                                                    u8 ident)
 {
         struct l2cap_chan *c;
 
@@ -178,7 +182,7 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 static void __l2cap_state_change(struct l2cap_chan *chan, int state)
 {
         BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
-                                                state_to_string(state));
+               state_to_string(state));
 
         chan->state = state;
         chan->ops->state_change(chan, state);
@@ -361,7 +365,7 @@ static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
 static void l2cap_chan_timeout(struct work_struct *work)
 {
         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
-                                                        chan_timer.work);
+                                               chan_timer.work);
         struct l2cap_conn *conn = chan->conn;
         int reason;
 
@@ -373,7 +377,7 @@ static void l2cap_chan_timeout(struct work_struct *work)
         if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
                 reason = ECONNREFUSED;
         else if (chan->state == BT_CONNECT &&
-                                        chan->sec_level != BT_SECURITY_SDP)
+                 chan->sec_level != BT_SECURITY_SDP)
                 reason = ECONNREFUSED;
         else
                 reason = ETIMEDOUT;
@@ -455,7 +459,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
         set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-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)
 {
         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
                __le16_to_cpu(chan->psm), chan->dcid);
@@ -504,7 +508,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
         chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
         chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
         chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
-        chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
+        chan->local_flush_to    = L2CAP_EFS_DEFAULT_FLUSH_TO;
 
         l2cap_chan_hold(chan);
 
@@ -527,6 +531,7 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
         BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 
         if (conn) {
+                struct amp_mgr *mgr = conn->hcon->amp_mgr;
                 /* Delete from channel list */
                 list_del(&chan->list);
 
@@ -536,10 +541,12 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
                 if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
                         hci_conn_put(conn->hcon);
+
+                if (mgr && mgr->bredr_chan == chan)
+                        mgr->bredr_chan = NULL;
         }
 
-        if (chan->ops->teardown)
-                chan->ops->teardown(chan, err);
+        chan->ops->teardown(chan, err);
 
         if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
                 return;
@@ -573,19 +580,18 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
         struct l2cap_conn *conn = chan->conn;
         struct sock *sk = chan->sk;
 
-        BT_DBG("chan %p state %s sk %p", chan,
-                                        state_to_string(chan->state), sk);
+        BT_DBG("chan %p state %s sk %p", chan, state_to_string(chan->state),
+               sk);
 
         switch (chan->state) {
         case BT_LISTEN:
-                if (chan->ops->teardown)
-                        chan->ops->teardown(chan, 0);
+                chan->ops->teardown(chan, 0);
                 break;
 
         case BT_CONNECTED:
         case BT_CONFIG:
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
-                                        conn->hcon->type == ACL_LINK) {
+                    conn->hcon->type == ACL_LINK) {
                         __set_chan_timer(chan, sk->sk_sndtimeo);
                         l2cap_send_disconn_req(conn, chan, reason);
                 } else
@@ -594,7 +600,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 
         case BT_CONNECT2:
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
-                                        conn->hcon->type == ACL_LINK) {
+                    conn->hcon->type == ACL_LINK) {
                         struct l2cap_conn_rsp rsp;
                         __u16 result;
 
@@ -609,7 +615,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                         rsp.result = cpu_to_le16(result);
                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
                 }
 
                 l2cap_chan_del(chan, reason);
@@ -621,8 +627,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                 break;
 
         default:
-                if (chan->ops->teardown)
-                        chan->ops->teardown(chan, 0);
+                chan->ops->teardown(chan, 0);
                 break;
         }
 }
@@ -691,7 +696,8 @@ static u8 l2cap_get_ident(struct l2cap_conn *conn)
         return id;
 }
 
-static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
+                           void *data)
 {
         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
         u8 flags;
@@ -718,10 +724,10 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
         u16 flags;
 
         BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
-                                                        skb->priority);
+               skb->priority);
 
         if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
-                                        lmp_no_flush_capable(hcon->hdev))
+            lmp_no_flush_capable(hcon->hdev))
                 flags = ACL_START_NO_FLUSH;
         else
                 flags = ACL_START;
@@ -946,7 +952,19 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
         return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+        struct l2cap_conn *conn = chan->conn;
+
+        if (enable_hs &&
+            chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+            conn->fixed_chan_mask & L2CAP_FC_A2MP)
+                return true;
+        else
+                return false;
+}
+
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
         struct l2cap_conn_req req;
@@ -972,6 +990,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
         chan->ops->ready(chan);
 }
 
+static void l2cap_start_connection(struct l2cap_chan *chan)
+{
+        if (__amp_capable(chan)) {
+                BT_DBG("chan %p AMP capable: discover AMPs", chan);
+                a2mp_discover_amp(chan);
+        } else {
+                l2cap_send_conn_req(chan);
+        }
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1014,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
                         return;
 
                 if (l2cap_chan_check_security(chan) &&
-                                __l2cap_no_conn_pending(chan))
-                        l2cap_send_conn_req(chan);
+                    __l2cap_no_conn_pending(chan)) {
+                        l2cap_start_connection(chan);
+                }
         } else {
                 struct l2cap_info_req req;
                 req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -997,8 +1026,8 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 
                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
 
-                l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(req), &req);
+                l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
+                               sizeof(req), &req);
         }
 }
 
@@ -1018,7 +1047,8 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
         }
 }
 
-static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
+static void l2cap_send_disconn_req(struct l2cap_conn *conn,
+                                   struct l2cap_chan *chan, int err)
 {
         struct sock *sk = chan->sk;
         struct l2cap_disconn_req req;
@@ -1033,14 +1063,14 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
         }
 
         if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
-                __l2cap_state_change(chan, BT_DISCONN);
+                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),
-                        L2CAP_DISCONN_REQ, sizeof(req), &req);
+        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
+                       sizeof(req), &req);
 
         lock_sock(sk);
         __l2cap_state_change(chan, BT_DISCONN);
@@ -1069,20 +1099,20 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 
                 if (chan->state == BT_CONNECT) {
                         if (!l2cap_chan_check_security(chan) ||
-                                        !__l2cap_no_conn_pending(chan)) {
+                            !__l2cap_no_conn_pending(chan)) {
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
                         if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
-                                        && test_bit(CONF_STATE2_DEVICE,
+                            && test_bit(CONF_STATE2_DEVICE,
                                         &chan->conf_state)) {
                                 l2cap_chan_close(chan, ECONNRESET);
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
-                        l2cap_send_conn_req(chan);
+                        l2cap_start_connection(chan);
 
                 } else if (chan->state == BT_CONNECT2) {
                         struct l2cap_conn_rsp rsp;
@@ -1094,11 +1124,9 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                                 lock_sock(sk);
                                 if (test_bit(BT_SK_DEFER_SETUP,
                                              &bt_sk(sk)->flags)) {
-                                        struct sock *parent = bt_sk(sk)->parent;
                                         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);
+                                        chan->ops->defer(chan);
 
                                 } else {
                                         __l2cap_state_change(chan, BT_CONFIG);
@@ -1112,17 +1140,17 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                         }
 
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
 
                         if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
-                                        rsp.result != L2CAP_CR_SUCCESS) {
+                            rsp.result != L2CAP_CR_SUCCESS) {
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
                         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);
+                                       l2cap_build_conf_req(chan, buf), buf);
                         chan->num_conf_req++;
                 }
 
@@ -1204,8 +1232,6 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
         bacpy(&bt_sk(sk)->src, conn->src);
         bacpy(&bt_sk(sk)->dst, conn->dst);
 
-        bt_accept_enqueue(parent, sk);
-
         l2cap_chan_add(conn, chan);
 
         l2cap_chan_ready(chan);
@@ -1270,7 +1296,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 
         list_for_each_entry(chan, &conn->chan_l, list) {
                 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
-                        __l2cap_chan_set_err(chan, err);
+                        l2cap_chan_set_err(chan, err);
         }
 
         mutex_unlock(&conn->chan_lock);
@@ -1279,7 +1305,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 static void l2cap_info_timeout(struct work_struct *work)
 {
         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
-                                                        info_timer.work);
+                                               info_timer.work);
 
         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
         conn->info_ident = 0;
@@ -1333,7 +1359,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 static void security_timeout(struct work_struct *work)
 {
         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
-                                                security_timer.work);
+                                               security_timer.work);
 
         BT_DBG("conn %p", conn);
 
@@ -1355,7 +1381,7 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
         if (!hchan)
                 return NULL;
 
-        conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
+        conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
         if (!conn) {
                 hci_chan_del(hchan);
                 return NULL;
@@ -1367,10 +1393,22 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 
         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
 
-        if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
-                conn->mtu = hcon->hdev->le_mtu;
-        else
+        switch (hcon->type) {
+        case AMP_LINK:
+                conn->mtu = hcon->hdev->block_mtu;
+                break;
+
+        case LE_LINK:
+                if (hcon->hdev->le_mtu) {
+                        conn->mtu = hcon->hdev->le_mtu;
+                        break;
+                }
+                /* fall through */
+
+        default:
                 conn->mtu = hcon->hdev->acl_mtu;
+                break;
+        }
 
         conn->src = &hcon->hdev->bdaddr;
         conn->dst = &hcon->dst;
@@ -1448,7 +1486,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
         __u8 auth_type;
         int err;
 
-        BT_DBG("%s -> %s (type %u) psm 0x%2.2x", batostr(src), batostr(dst),
+        BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", src, dst,
                dst_type, __le16_to_cpu(psm));
 
         hdev = hci_get_route(dst, src);
@@ -1461,7 +1499,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
 
         /* PSM must be odd and lsb of upper byte must be 0 */
         if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+            chan->chan_type != L2CAP_CHAN_RAW) {
                 err = -EINVAL;
                 goto done;
         }
@@ -1770,7 +1808,7 @@ static void l2cap_ertm_resend(struct l2cap_chan *chan)
                 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
                 if (!skb) {
                         BT_DBG("Error: Can't retransmit seq %d, frame missing",
-                                seq);
+                               seq);
                         continue;
                 }
 
@@ -1795,9 +1833,9 @@ static void l2cap_ertm_resend(struct l2cap_chan *chan)
                         /* Cloned sk_buffs are read-only, so we need a
                          * writeable copy
                          */
-                        tx_skb = skb_copy(skb, GFP_ATOMIC);
+                        tx_skb = skb_copy(skb, GFP_KERNEL);
                 } else {
-                        tx_skb = skb_clone(skb, GFP_ATOMIC);
+                        tx_skb = skb_clone(skb, GFP_KERNEL);
                 }
 
                 if (!tx_skb) {
@@ -1855,7 +1893,7 @@ static void l2cap_retransmit_all(struct l2cap_chan *chan,
         if (chan->unacked_frames) {
                 skb_queue_walk(&chan->tx_q, skb) {
                         if (bt_cb(skb)->control.txseq == control->reqseq ||
-                                skb == chan->tx_send_head)
+                            skb == chan->tx_send_head)
                                 break;
                 }
 
@@ -2156,7 +2194,7 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan,
 }
 
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
-                                                                u32 priority)
+                    u32 priority)
 {
         struct sk_buff *skb;
         int err;
@@ -2543,7 +2581,7 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
                 /* Don't send frame to the socket it came from */
                 if (skb->sk == sk)
                         continue;
-                nskb = skb_clone(skb, GFP_ATOMIC);
+                nskb = skb_clone(skb, GFP_KERNEL);
                 if (!nskb)
                         continue;
 
@@ -2569,7 +2607,7 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
         count = min_t(unsigned int, conn->mtu, len);
 
-        skb = bt_skb_alloc(count, GFP_ATOMIC);
+        skb = bt_skb_alloc(count, GFP_KERNEL);
         if (!skb)
                 return NULL;
 
@@ -2599,7 +2637,7 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
         while (len) {
                 count = min_t(unsigned int, conn->mtu, len);
 
-                *frag = bt_skb_alloc(count, GFP_ATOMIC);
+                *frag = bt_skb_alloc(count, GFP_KERNEL);
                 if (!*frag)
                         goto fail;
 
@@ -2618,7 +2656,8 @@ fail:
         return NULL;
 }
 
-static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
+static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
+                                     unsigned long *val)
 {
         struct l2cap_conf_opt *opt = *ptr;
         int len;
@@ -2692,7 +2731,7 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
                 efs.msdu        = cpu_to_le16(chan->local_msdu);
                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
                 efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
-                efs.flush_to    = __constant_cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
+                efs.flush_to    = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
                 break;
 
         case L2CAP_MODE_STREAMING:
@@ -2709,7 +2748,7 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
         }
 
         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
-                                                        (unsigned long) &efs);
+                           (unsigned long) &efs);
 }
 
 static void l2cap_ack_timeout(struct work_struct *work)
@@ -2798,13 +2837,13 @@ static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
 {
         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
-                                                __l2cap_ews_supported(chan)) {
+            __l2cap_ews_supported(chan)) {
                 /* use extended control field */
                 set_bit(FLAG_EXT_CTRL, &chan->flags);
                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
         } else {
                 chan->tx_win = min_t(u16, chan->tx_win,
-                                                L2CAP_DEFAULT_TX_WINDOW);
+                                     L2CAP_DEFAULT_TX_WINDOW);
                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
         }
         chan->ack_win = chan->tx_win;
@@ -2844,7 +2883,7 @@ done:
         switch (chan->mode) {
         case L2CAP_MODE_BASIC:
                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
-                                !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
+                    !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
                         break;
 
                 rfc.mode            = L2CAP_MODE_BASIC;
@@ -2855,7 +2894,7 @@ done:
                 rfc.max_pdu_size    = 0;
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
                 break;
 
         case L2CAP_MODE_ERTM:
@@ -2865,18 +2904,17 @@ done:
                 rfc.monitor_timeout = 0;
 
                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                             L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
+                             L2CAP_FCS_SIZE);
                 rfc.max_pdu_size = cpu_to_le16(size);
 
                 l2cap_txwin_setup(chan);
 
                 rfc.txwin_size = min_t(u16, chan->tx_win,
-                                                L2CAP_DEFAULT_TX_WINDOW);
+                                       L2CAP_DEFAULT_TX_WINDOW);
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
 
                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
                         l2cap_add_opt_efs(&ptr, chan);
@@ -2885,14 +2923,14 @@ done:
                         break;
 
                 if (chan->fcs == L2CAP_FCS_NONE ||
-                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
+                    test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                         chan->fcs = L2CAP_FCS_NONE;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                 }
 
                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
-                                                                chan->tx_win);
+                                           chan->tx_win);
                 break;
 
         case L2CAP_MODE_STREAMING:
@@ -2904,13 +2942,12 @@ done:
                 rfc.monitor_timeout = 0;
 
                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                             L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
+                             L2CAP_FCS_SIZE);
                 rfc.max_pdu_size = cpu_to_le16(size);
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
 
                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
                         l2cap_add_opt_efs(&ptr, chan);
@@ -2919,7 +2956,7 @@ done:
                         break;
 
                 if (chan->fcs == L2CAP_FCS_NONE ||
-                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
+                    test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                         chan->fcs = L2CAP_FCS_NONE;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                 }
@@ -3011,7 +3048,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
         case L2CAP_MODE_ERTM:
                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
                         chan->mode = l2cap_select_mode(rfc.mode,
-                                        chan->conn->feat_mask);
+                                                       chan->conn->feat_mask);
                         break;
                 }
 
@@ -3036,8 +3073,8 @@ done:
                 if (chan->num_conf_rsp == 1)
                         return -ECONNREFUSED;
 
-                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+                                   (unsigned long) &rfc);
         }
 
         if (result == L2CAP_CONF_SUCCESS) {
@@ -3054,8 +3091,8 @@ done:
 
                 if (remote_efs) {
                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != chan->local_stype) {
+                            efs.stype != L2CAP_SERV_NOTRAFIC &&
+                            efs.stype != chan->local_stype) {
 
                                 result = L2CAP_CONF_UNACCEPT;
 
@@ -3063,8 +3100,8 @@ done:
                                         return -ECONNREFUSED;
 
                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                                        sizeof(efs),
-                                                        (unsigned long) &efs);
+                                                   sizeof(efs),
+                                                   (unsigned long) &efs);
                         } else {
                                 /* Send PENDING Conf Rsp */
                                 result = L2CAP_CONF_PENDING;
@@ -3087,10 +3124,8 @@ done:
                         chan->remote_max_tx = rfc.max_transmit;
 
                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
-                                                chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                                     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
+                                     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
                         rfc.max_pdu_size = cpu_to_le16(size);
                         chan->remote_mps = size;
 
@@ -3102,36 +3137,35 @@ done:
                         set_bit(CONF_MODE_DONE, &chan->conf_state);
 
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                                           sizeof(rfc), (unsigned long) &rfc);
 
                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
                                 chan->remote_id = efs.id;
                                 chan->remote_stype = efs.stype;
                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
                                 chan->remote_flush_to =
-                                                le32_to_cpu(efs.flush_to);
+                                        le32_to_cpu(efs.flush_to);
                                 chan->remote_acc_lat =
-                                                le32_to_cpu(efs.acc_lat);
+                                        le32_to_cpu(efs.acc_lat);
                                 chan->remote_sdu_itime =
                                         le32_to_cpu(efs.sdu_itime);
                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                        sizeof(efs), (unsigned long) &efs);
+                                                   sizeof(efs),
+                                                   (unsigned long) &efs);
                         }
                         break;
 
                 case L2CAP_MODE_STREAMING:
                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
-                                                chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                                     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
+                                     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
                         rfc.max_pdu_size = cpu_to_le16(size);
                         chan->remote_mps = size;
 
                         set_bit(CONF_MODE_DONE, &chan->conf_state);
 
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+                                           (unsigned long) &rfc);
 
                         break;
 
@@ -3152,7 +3186,8 @@ done:
         return ptr - data;
 }
 
-static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
+static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
+                                void *data, u16 *result)
 {
         struct l2cap_conf_req *req = data;
         void *ptr = req->data;
@@ -3179,7 +3214,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                 case L2CAP_CONF_FLUSH_TO:
                         chan->flush_to = val;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
-                                                        2, chan->flush_to);
+                                           2, chan->flush_to);
                         break;
 
                 case L2CAP_CONF_RFC:
@@ -3187,13 +3222,13 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                                 memcpy(&rfc, (void *)val, olen);
 
                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
-                                                        rfc.mode != chan->mode)
+                            rfc.mode != chan->mode)
                                 return -ECONNREFUSED;
 
                         chan->fcs = 0;
 
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                                           sizeof(rfc), (unsigned long) &rfc);
                         break;
 
                 case L2CAP_CONF_EWS:
@@ -3207,12 +3242,12 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                                 memcpy(&efs, (void *)val, olen);
 
                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != chan->local_stype)
+                            efs.stype != L2CAP_SERV_NOTRAFIC &&
+                            efs.stype != chan->local_stype)
                                 return -ECONNREFUSED;
 
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                        sizeof(efs), (unsigned long) &efs);
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
+                                           (unsigned long) &efs);
                         break;
                 }
         }
@@ -3235,10 +3270,10 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
                                 chan->local_msdu = le16_to_cpu(efs.msdu);
                                 chan->local_sdu_itime =
-                                                le32_to_cpu(efs.sdu_itime);
+                                        le32_to_cpu(efs.sdu_itime);
                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
                                 chan->local_flush_to =
-                                                le32_to_cpu(efs.flush_to);
+                                        le32_to_cpu(efs.flush_to);
                         }
                         break;
 
@@ -3253,7 +3288,8 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
         return ptr - data;
 }
 
-static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
+static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
+                                u16 result, u16 flags)
 {
         struct l2cap_conf_rsp *rsp = data;
         void *ptr = rsp->data;
@@ -3277,14 +3313,13 @@ void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
         rsp.dcid   = cpu_to_le16(chan->scid);
         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);
+        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
                 return;
 
         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                        l2cap_build_conf_req(chan, buf), buf);
+                       l2cap_build_conf_req(chan, buf), buf);
         chan->num_conf_req++;
 }
 
@@ -3339,7 +3374,8 @@ static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
         }
 }
 
-static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_command_rej(struct l2cap_conn *conn,
+                                    struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
 
@@ -3347,7 +3383,7 @@ static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hd
                 return 0;
 
         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
-                                        cmd->ident == conn->info_ident) {
+            cmd->ident == conn->info_ident) {
                 cancel_delayed_work(&conn->info_timer);
 
                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
@@ -3359,7 +3395,8 @@ static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hd
         return 0;
 }
 
-static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static void l2cap_connect(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd,
+                          u8 *data, u8 rsp_code, u8 amp_id)
 {
         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
         struct l2cap_conn_rsp rsp;
@@ -3386,7 +3423,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
         /* Check if the ACL is secure enough (if not SDP) */
         if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
-                                !hci_conn_check_link_mode(conn->hcon)) {
+            !hci_conn_check_link_mode(conn->hcon)) {
                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
                 result = L2CAP_CR_SEC_BLOCK;
                 goto response;
@@ -3411,8 +3448,6 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
         chan->psm  = psm;
         chan->dcid = scid;
 
-        bt_accept_enqueue(parent, sk);
-
         __l2cap_chan_add(conn, chan);
 
         dcid = chan->scid;
@@ -3427,7 +3462,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
                                 __l2cap_state_change(chan, BT_CONNECT2);
                                 result = L2CAP_CR_PEND;
                                 status = L2CAP_CS_AUTHOR_PEND;
-                                parent->sk_data_ready(parent, 0);
+                                chan->ops->defer(chan);
                         } else {
                                 __l2cap_state_change(chan, BT_CONFIG);
                                 result = L2CAP_CR_SUCCESS;
@@ -3453,7 +3488,7 @@ sendresp:
         rsp.dcid   = cpu_to_le16(dcid);
         rsp.result = cpu_to_le16(result);
         rsp.status = cpu_to_le16(status);
-        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+        l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
 
         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
                 struct l2cap_info_req info;
@@ -3464,23 +3499,29 @@ sendresp:
 
                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
 
-                l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(info), &info);
+                l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
+                               sizeof(info), &info);
         }
 
         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
-                                result == L2CAP_CR_SUCCESS) {
+            result == L2CAP_CR_SUCCESS) {
                 u8 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);
+                               l2cap_build_conf_req(chan, buf), buf);
                 chan->num_conf_req++;
         }
+}
 
+static int l2cap_connect_req(struct l2cap_conn *conn,
+                             struct l2cap_cmd_hdr *cmd, u8 *data)
+{
+        l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
         return 0;
 }
 
-static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_connect_rsp(struct l2cap_conn *conn,
+                                    struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
         u16 scid, dcid, result, status;
@@ -3494,7 +3535,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
         status = __le16_to_cpu(rsp->status);
 
         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
-                                                dcid, scid, result, status);
+               dcid, scid, result, status);
 
         mutex_lock(&conn->chan_lock);
 
@@ -3527,7 +3568,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
                         break;
 
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                        l2cap_build_conf_req(chan, req), req);
+                               l2cap_build_conf_req(chan, req), req);
                 chan->num_conf_req++;
                 break;
 
@@ -3559,7 +3600,25 @@ static inline void set_default_fcs(struct l2cap_chan *chan)
                 chan->fcs = L2CAP_FCS_CRC16;
 }
 
-static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
+static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
+                                    u8 ident, u16 flags)
+{
+        struct l2cap_conn *conn = chan->conn;
+
+        BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
+               flags);
+
+        clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
+        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
+
+        l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
+                       l2cap_build_conf_rsp(chan, data,
+                                            L2CAP_CONF_SUCCESS, flags), data);
+}
+
+static inline int l2cap_config_req(struct l2cap_conn *conn,
+                                   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                   u8 *data)
 {
         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
         u16 dcid, flags;
@@ -3584,7 +3643,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 rej.dcid = cpu_to_le16(chan->dcid);
 
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
-                                sizeof(rej), &rej);
+                               sizeof(rej), &rej);
                 goto unlock;
         }
 
@@ -3592,8 +3651,8 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         len = cmd_len - sizeof(*req);
         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_REJECT, flags), rsp);
+                               l2cap_build_conf_rsp(chan, rsp,
+                               L2CAP_CONF_REJECT, flags), rsp);
                 goto unlock;
         }
 
@@ -3604,8 +3663,8 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         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, flags), rsp);
+                               l2cap_build_conf_rsp(chan, rsp,
+                               L2CAP_CONF_SUCCESS, flags), rsp);
                 goto unlock;
         }
 
@@ -3643,23 +3702,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
                 u8 buf[64];
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                        l2cap_build_conf_req(chan, buf), buf);
+                               l2cap_build_conf_req(chan, buf), buf);
                 chan->num_conf_req++;
         }
 
         /* Got Conf Rsp PENDING from remote side and asume we sent
            Conf Rsp PENDING in the code above */
         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
-                        test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
+            test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
 
                 /* check compatibility */
 
-                clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
-                set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
-
-                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                        l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_SUCCESS, flags), rsp);
+                /* Send rsp for BR/EDR channel */
+                if (!chan->ctrl_id)
+                        l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
+                else
+                        chan->ident = cmd->ident;
         }
 
 unlock:
@@ -3667,7 +3725,8 @@ unlock:
         return err;
 }
 
-static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_config_rsp(struct l2cap_conn *conn,
+                                   struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
         u16 scid, flags, result;
@@ -3699,7 +3758,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         char buf[64];
 
                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
-                                                                buf, &result);
+                                                   buf, &result);
                         if (len < 0) {
                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
                                 goto done;
@@ -3707,12 +3766,11 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
                         /* check compatibility */
 
-                        clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
-                        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
-
-                        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                                l2cap_build_conf_rsp(chan, buf,
-                                                L2CAP_CONF_SUCCESS, 0x0000), buf);
+                        if (!chan->ctrl_id)
+                                l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
+                                                        0);
+                        else
+                                chan->ident = cmd->ident;
                 }
                 goto done;
 
@@ -3728,14 +3786,14 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         /* throw out any old stored conf requests */
                         result = L2CAP_CONF_SUCCESS;
                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
-                                                                req, &result);
+                                                   req, &result);
                         if (len < 0) {
                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
                                 goto done;
                         }
 
                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
-                                                L2CAP_CONF_REQ, len, req);
+                                       L2CAP_CONF_REQ, len, req);
                         chan->num_conf_req++;
                         if (result != L2CAP_CONF_SUCCESS)
                                 goto done;
@@ -3773,7 +3831,8 @@ done:
         return err;
 }
 
-static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
         struct l2cap_disconn_rsp rsp;
@@ -3819,7 +3878,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
         return 0;
 }
 
-static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
         u16 dcid, scid;
@@ -3853,7 +3913,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
         return 0;
 }
 
-static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_information_req(struct l2cap_conn *conn,
+                                        struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
         u16 type;
@@ -3870,14 +3931,14 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
                 if (!disable_ertm)
                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
-                                                         | L2CAP_FEAT_FCS;
+                                | L2CAP_FEAT_FCS;
                 if (enable_hs)
                         feat_mask |= L2CAP_FEAT_EXT_FLOW
-                                                | L2CAP_FEAT_EXT_WINDOW;
+                                | L2CAP_FEAT_EXT_WINDOW;
 
                 put_unaligned_le32(feat_mask, rsp->data);
-                l2cap_send_cmd(conn, cmd->ident,
-                                        L2CAP_INFO_RSP, sizeof(buf), buf);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
+                               buf);
         } else if (type == L2CAP_IT_FIXED_CHAN) {
                 u8 buf[12];
                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
@@ -3890,20 +3951,21 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 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);
+                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 = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
-                l2cap_send_cmd(conn, cmd->ident,
-                                        L2CAP_INFO_RSP, sizeof(rsp), &rsp);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
+                               &rsp);
         }
 
         return 0;
 }
 
-static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_information_rsp(struct l2cap_conn *conn,
+                                        struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
         u16 type, result;
@@ -3915,7 +3977,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 
         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
         if (cmd->ident != conn->info_ident ||
-                        conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
+            conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
                 return 0;
 
         cancel_delayed_work(&conn->info_timer);
@@ -3940,7 +4002,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
                         conn->info_ident = l2cap_get_ident(conn);
 
                         l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(req), &req);
+                                       L2CAP_INFO_REQ, sizeof(req), &req);
                 } else {
                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
                         conn->info_ident = 0;
@@ -3962,8 +4024,8 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 }
 
 static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
-                                        void *data)
+                                           struct l2cap_cmd_hdr *cmd,
+                                           u16 cmd_len, void *data)
 {
         struct l2cap_create_chan_req *req = data;
         struct l2cap_create_chan_rsp rsp;
@@ -3993,7 +4055,8 @@ static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, void *data)
+                                           struct l2cap_cmd_hdr *cmd,
+                                           void *data)
 {
         BT_DBG("conn %p", conn);
 
@@ -4126,7 +4189,7 @@ static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
-                                                        u16 to_multiplier)
+                                         u16 to_multiplier)
 {
         u16 max_latency;
 
@@ -4147,7 +4210,8 @@ static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
 }
 
 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u8 *data)
+                                              struct l2cap_cmd_hdr *cmd,
+                                              u8 *data)
 {
         struct hci_conn *hcon = conn->hcon;
         struct l2cap_conn_param_update_req *req;
@@ -4169,7 +4233,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
         to_multiplier   = __le16_to_cpu(req->to_multiplier);
 
         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
-                                                min, max, latency, to_multiplier);
+               min, max, latency, to_multiplier);
 
         memset(&rsp, 0, sizeof(rsp));
 
@@ -4180,7 +4244,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
 
         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
-                                                        sizeof(rsp), &rsp);
+                       sizeof(rsp), &rsp);
 
         if (!err)
                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
@@ -4189,7 +4253,8 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
-                        struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
+                                      struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                      u8 *data)
 {
         int err = 0;
 
@@ -4203,6 +4268,7 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
                 break;
 
         case L2CAP_CONN_RSP:
+        case L2CAP_CREATE_CHAN_RSP:
                 err = l2cap_connect_rsp(conn, cmd, data);
                 break;
 
@@ -4241,10 +4307,6 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
                 break;
 
-        case L2CAP_CREATE_CHAN_RSP:
-                err = l2cap_create_channel_rsp(conn, cmd, data);
-                break;
-
         case L2CAP_MOVE_CHAN_REQ:
                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
                 break;
@@ -4271,7 +4333,7 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u8 *data)
+                                   struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         switch (cmd->code) {
         case L2CAP_COMMAND_REJ:
@@ -4290,7 +4352,7 @@ static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
 }
 
 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
-                                                        struct sk_buff *skb)
+                                     struct sk_buff *skb)
 {
         u8 *data = skb->data;
         int len = skb->len;
@@ -4307,7 +4369,8 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
 
                 cmd_len = le16_to_cpu(cmd.len);
 
-                BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
+                BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
+                       cmd.ident);
 
                 if (cmd_len > len || !cmd.ident) {
                         BT_DBG("corrupted command");
@@ -4326,7 +4389,8 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
 
                         /* FIXME: Map err to a valid reason */
                         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
-                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
+                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
+                                       sizeof(rej), &rej);
                 }
 
                 data += cmd_len;
@@ -4391,8 +4455,8 @@ static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
         }
 }
 
-static void append_skb_frag(struct sk_buff *skb,
-                        struct sk_buff *new_frag, struct sk_buff **last_frag)
+static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
+                            struct sk_buff **last_frag)
 {
         /* skb->len reflects data in skb as well as all fragments
          * skb->data_len reflects only data in fragments
@@ -4641,7 +4705,7 @@ static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
 
         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
-                                                                chan->tx_win) {
+                    chan->tx_win) {
                         /* See notes below regarding "double poll" and
                          * invalid packets.
                          */
@@ -4682,8 +4746,7 @@ static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
         }
 
         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
-                __seq_offset(chan, chan->expected_tx_seq,
-                             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;
         }
@@ -5323,7 +5386,7 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
         int exact = 0, lm1 = 0, lm2 = 0;
         struct l2cap_chan *c;
 
-        BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
+        BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
 
         /* Find listening sockets and check their link_mode */
         read_lock(&chan_list_lock);
@@ -5353,7 +5416,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 {
         struct l2cap_conn *conn;
 
-        BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
+        BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
 
         if (!status) {
                 conn = l2cap_conn_add(hcon, status);
@@ -5443,7 +5506,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                 }
 
                 if (!status && (chan->state == BT_CONNECTED ||
-                                                chan->state == BT_CONFIG)) {
+                                chan->state == BT_CONFIG)) {
                         struct sock *sk = chan->sk;
 
                         clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
@@ -5456,7 +5519,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
                 if (chan->state == BT_CONNECT) {
                         if (!status) {
-                                l2cap_send_conn_req(chan);
+                                l2cap_start_connection(chan);
                         } else {
                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
                         }
@@ -5470,11 +5533,9 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         if (!status) {
                                 if (test_bit(BT_SK_DEFER_SETUP,
                                              &bt_sk(sk)->flags)) {
-                                        struct sock *parent = bt_sk(sk)->parent;
                                         res = L2CAP_CR_PEND;
                                         stat = L2CAP_CS_AUTHOR_PEND;
-                                        if (parent)
-                                                parent->sk_data_ready(parent, 0);
+                                        chan->ops->defer(chan);
                                 } else {
                                         __l2cap_state_change(chan, BT_CONFIG);
                                         res = L2CAP_CR_SUCCESS;
@@ -5494,7 +5555,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         rsp.result = cpu_to_le16(res);
                         rsp.status = cpu_to_le16(stat);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
 
                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
                             res == L2CAP_CR_SUCCESS) {
@@ -5519,6 +5580,12 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 {
         struct l2cap_conn *conn = hcon->l2cap_data;
+        struct l2cap_hdr *hdr;
+        int len;
+
+        /* For AMP controller do not create l2cap conn */
+        if (!conn && hcon->hdev->dev_type != HCI_BREDR)
+                goto drop;
 
         if (!conn)
                 conn = l2cap_conn_add(hcon, 0);
@@ -5528,10 +5595,10 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
 
-        if (!(flags & ACL_CONT)) {
-                struct l2cap_hdr *hdr;
-                int len;
-
+        switch (flags) {
+        case ACL_START:
+        case ACL_START_NO_FLUSH:
+        case ACL_COMPLETE:
                 if (conn->rx_len) {
                         BT_ERR("Unexpected start frame (len %d)", skb->len);
                         kfree_skb(conn->rx_skb);
@@ -5560,20 +5627,22 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
                 if (skb->len > len) {
                         BT_ERR("Frame is too long (len %d, expected len %d)",
-                                skb->len, len);
+                               skb->len, len);
                         l2cap_conn_unreliable(conn, ECOMM);
                         goto drop;
                 }
 
                 /* Allocate skb for the complete frame (with header) */
-                conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
+                conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
                 if (!conn->rx_skb)
                         goto drop;
 
                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
-                                                                skb->len);
+                                          skb->len);
                 conn->rx_len = len - skb->len;
-        } else {
+                break;
+
+        case ACL_CONT:
                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
 
                 if (!conn->rx_len) {
@@ -5584,7 +5653,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
                 if (skb->len > conn->rx_len) {
                         BT_ERR("Fragment is too long (len %d, expected %d)",
-                                        skb->len, conn->rx_len);
+                               skb->len, conn->rx_len);
                         kfree_skb(conn->rx_skb);
                         conn->rx_skb = NULL;
                         conn->rx_len = 0;
@@ -5593,7 +5662,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
                 }
 
                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
-                                                                skb->len);
+                                          skb->len);
                 conn->rx_len -= skb->len;
 
                 if (!conn->rx_len) {
@@ -5601,6 +5670,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
                         l2cap_recv_frame(conn, conn->rx_skb);
                         conn->rx_skb = NULL;
                 }
+                break;
         }
 
 drop:
@@ -5617,12 +5687,11 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p)
         list_for_each_entry(c, &chan_list, global_l) {
                 struct sock *sk = c->sk;
 
-                seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
-                                        batostr(&bt_sk(sk)->src),
-                                        batostr(&bt_sk(sk)->dst),
-                                        c->state, __le16_to_cpu(c->psm),
-                                        c->scid, c->dcid, c->imtu, c->omtu,
-                                        c->sec_level, c->mode);
+                seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
+                           &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                           c->state, __le16_to_cpu(c->psm),
+                           c->scid, c->dcid, c->imtu, c->omtu,
+                           c->sec_level, c->mode);
         }
 
         read_unlock(&chan_list_lock);
@@ -5653,8 +5722,8 @@ int __init l2cap_init(void)
                 return err;
 
         if (bt_debugfs) {
-                l2cap_debugfs = debugfs_create_file("l2cap", 0444,
-                                        bt_debugfs, NULL, &l2cap_debugfs_fops);
+                l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
+                                                    NULL, &l2cap_debugfs_fops);
                 if (!l2cap_debugfs)
                         BT_ERR("Failed to create L2CAP debug file");
         }
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 083f2bf065d4..89f1472939ec 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -40,7 +40,8 @@ static struct bt_sock_list l2cap_sk_list = {
 
 static const struct proto_ops l2cap_sock_ops;
 static void l2cap_sock_init(struct sock *sk, struct sock *parent);
-static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio);
+static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock,
+                                     int proto, gfp_t prio);
 
 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 {
@@ -106,7 +107,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
+static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr,
+                              int alen, int flags)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -134,7 +136,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
         lock_sock(sk);
 
         err = bt_sock_wait_state(sk, BT_CONNECTED,
-                        sock_sndtimeo(sk, flags & O_NONBLOCK));
+                                 sock_sndtimeo(sk, flags & O_NONBLOCK));
 
         release_sock(sk);
 
@@ -185,7 +187,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
+static int l2cap_sock_accept(struct socket *sock, struct socket *newsock,
+                             int flags)
 {
         DECLARE_WAITQUEUE(wait, current);
         struct sock *sk = sock->sk, *nsk;
@@ -241,7 +244,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
+static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr,
+                              int *len, int peer)
 {
         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
         struct sock *sk = sock->sk;
@@ -266,7 +270,8 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
         return 0;
 }
 
-static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
+static int l2cap_sock_getsockopt_old(struct socket *sock, int optname,
+                                     char __user *optval, int __user *optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -309,7 +314,7 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
                         break;
                 case BT_SECURITY_HIGH:
                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
-                                                        L2CAP_LM_SECURE;
+                              L2CAP_LM_SECURE;
                         break;
                 default:
                         opt = 0;
@@ -353,7 +358,8 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
         return err;
 }
 
-static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
+static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname,
+                                 char __user *optval, int __user *optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -377,19 +383,20 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
         switch (optname) {
         case BT_SECURITY:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
 
                 memset(&sec, 0, sizeof(sec));
-                if (chan->conn)
+                if (chan->conn) {
                         sec.level = chan->conn->hcon->sec_level;
-                else
-                        sec.level = chan->sec_level;
 
-                if (sk->sk_state == BT_CONNECTED)
-                        sec.key_size = chan->conn->hcon->enc_key_size;
+                        if (sk->sk_state == BT_CONNECTED)
+                                sec.key_size = chan->conn->hcon->enc_key_size;
+                } else {
+                        sec.level = chan->sec_level;
+                }
 
                 len = min_t(unsigned int, len, sizeof(sec));
                 if (copy_to_user(optval, (char *) &sec, len))
@@ -411,14 +418,14 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
 
         case BT_FLUSHABLE:
                 if (put_user(test_bit(FLAG_FLUSHABLE, &chan->flags),
-                                                (u32 __user *) optval))
+                             (u32 __user *) optval))
                         err = -EFAULT;
 
                 break;
 
         case BT_POWER:
                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
-                                && sk->sk_type != SOCK_RAW) {
+                    && sk->sk_type != SOCK_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -466,7 +473,8 @@ static bool l2cap_valid_mtu(struct l2cap_chan *chan, u16 mtu)
         return true;
 }
 
-static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
+static int l2cap_sock_setsockopt_old(struct socket *sock, int optname,
+                                     char __user *optval, unsigned int optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -529,6 +537,7 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                 chan->fcs  = opts.fcs;
                 chan->max_tx = opts.max_tx;
                 chan->tx_win = opts.txwin_size;
+                chan->flush_to = opts.flush_to;
                 break;
 
         case L2CAP_LM:
@@ -564,7 +573,8 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
         return err;
 }
 
-static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
+static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
+                                 char __user *optval, unsigned int optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -587,7 +597,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
         switch (optname) {
         case BT_SECURITY:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -601,7 +611,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
                 }
 
                 if (sec.level < BT_SECURITY_LOW ||
-                                        sec.level > BT_SECURITY_HIGH) {
+                    sec.level > BT_SECURITY_HIGH) {
                         err = -EINVAL;
                         break;
                 }
@@ -627,7 +637,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 
                 /* or for ACL link */
                 } else if ((sk->sk_state == BT_CONNECT2 &&
-                           test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) ||
+                            test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) ||
                            sk->sk_state == BT_CONNECTED) {
                         if (!l2cap_chan_check_security(chan))
                                 set_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
@@ -684,7 +694,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 
         case BT_POWER:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -720,7 +730,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
                 }
 
                 if (chan->mode != L2CAP_MODE_ERTM &&
-                                chan->mode != L2CAP_MODE_STREAMING) {
+                    chan->mode != L2CAP_MODE_STREAMING) {
                         err = -EOPNOTSUPP;
                         break;
                 }
@@ -737,7 +747,8 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
         return err;
 }
 
-static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
+static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
+                              struct msghdr *msg, size_t len)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -762,7 +773,8 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
         return err;
 }
 
-static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
+static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
+                              struct msghdr *msg, size_t len, int flags)
 {
         struct sock *sk = sock->sk;
         struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -866,7 +878,7 @@ static int l2cap_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);
         }
 
         if (!err && sk->sk_err)
@@ -930,7 +942,7 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
         }
 
         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP,
-                                                                GFP_ATOMIC);
+                              GFP_ATOMIC);
         if (!sk)
                 return NULL;
 
@@ -938,6 +950,8 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
 
         l2cap_sock_init(sk, parent);
 
+        bt_accept_enqueue(parent, sk);
+
         return l2cap_pi(sk)->chan;
 }
 
@@ -1068,6 +1082,15 @@ static void l2cap_sock_ready_cb(struct l2cap_chan *chan)
         release_sock(sk);
 }
 
+static void l2cap_sock_defer_cb(struct l2cap_chan *chan)
+{
+        struct sock *sk = chan->data;
+        struct sock *parent = bt_sk(sk)->parent;
+
+        if (parent)
+                parent->sk_data_ready(parent, 0);
+}
+
 static struct l2cap_ops l2cap_chan_ops = {
         .name           = "L2CAP Socket Interface",
         .new_connection = l2cap_sock_new_connection_cb,
@@ -1076,6 +1099,7 @@ static struct l2cap_ops l2cap_chan_ops = {
         .teardown       = l2cap_sock_teardown_cb,
         .state_change   = l2cap_sock_state_change_cb,
         .ready          = l2cap_sock_ready_cb,
+        .defer          = l2cap_sock_defer_cb,
         .alloc_skb      = l2cap_sock_alloc_skb_cb,
 };
 
@@ -1083,7 +1107,8 @@ static void l2cap_sock_destruct(struct sock *sk)
 {
         BT_DBG("sk %p", sk);
 
-        l2cap_chan_put(l2cap_pi(sk)->chan);
+        if (l2cap_pi(sk)->chan)
+                l2cap_chan_put(l2cap_pi(sk)->chan);
         if (l2cap_pi(sk)->rx_busy_skb) {
                 kfree_skb(l2cap_pi(sk)->rx_busy_skb);
                 l2cap_pi(sk)->rx_busy_skb = NULL;
@@ -1159,7 +1184,8 @@ static struct proto l2cap_proto = {
         .obj_size       = sizeof(struct l2cap_pinfo)
 };
 
-static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
+static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock,
+                                     int proto, gfp_t prio)
 {
         struct sock *sk;
         struct l2cap_chan *chan;
@@ -1204,7 +1230,7 @@ static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol,
         sock->state = SS_UNCONNECTED;
 
         if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM &&
-                        sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
+            sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
                 return -ESOCKTNOSUPPORT;
 
         if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW))
@@ -1261,7 +1287,8 @@ int __init l2cap_init_sockets(void)
                 goto error;
         }
 
-        err = bt_procfs_init(THIS_MODULE, &init_net, "l2cap", &l2cap_sk_list, NULL);
+        err = bt_procfs_init(THIS_MODULE, &init_net, "l2cap", &l2cap_sk_list,
+                             NULL);
         if (err < 0) {
                 BT_ERR("Failed to create L2CAP proc file");
                 bt_sock_unregister(BTPROTO_L2CAP);
diff --git a/net/bluetooth/lib.c b/net/bluetooth/lib.c
index e1c97527e16c..b3fbc73516c4 100644
--- a/net/bluetooth/lib.c
+++ b/net/bluetooth/lib.c
@@ -41,20 +41,6 @@ void baswap(bdaddr_t *dst, bdaddr_t *src)
 }
 EXPORT_SYMBOL(baswap);
 
-char *batostr(bdaddr_t *ba)
-{
-        static char str[2][18];
-        static int i = 1;
-
-        i ^= 1;
-        sprintf(str[i], "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
-                ba->b[5], ba->b[4], ba->b[3],
-                ba->b[2], ba->b[1], ba->b[0]);
-
-        return str[i];
-}
-EXPORT_SYMBOL(batostr);
-
 /* Bluetooth error codes to Unix errno mapping */
 int bt_to_errno(__u16 code)
 {
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index aa2ea0a8142c..399e5024b5bd 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3125,6 +3125,9 @@ int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
         struct pending_cmd *cmd;
         int err;
 
+        mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
+                             hdev);
+
         cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, hdev);
         if (!cmd)
                 return -ENOENT;
@@ -3137,8 +3140,6 @@ 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);
         return err;
 }
 
diff --git a/net/bluetooth/rfcomm/core.c b/net/bluetooth/rfcomm/core.c
index c75107ef8920..201fdf737209 100644
--- a/net/bluetooth/rfcomm/core.c
+++ b/net/bluetooth/rfcomm/core.c
@@ -377,8 +377,8 @@ static int __rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst,
         int err = 0;
         u8 dlci;
 
-        BT_DBG("dlc %p state %ld %s %s channel %d",
-                        d, d->state, batostr(src), batostr(dst), channel);
+        BT_DBG("dlc %p state %ld %pMR -> %pMR channel %d",
+               d, d->state, src, dst, channel);
 
         if (channel < 1 || channel > 30)
                 return -EINVAL;
@@ -676,7 +676,7 @@ static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src,
         struct socket *sock;
         struct sock *sk;
 
-        BT_DBG("%s %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         *err = rfcomm_l2sock_create(&sock);
         if (*err < 0)
@@ -709,7 +709,7 @@ static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src,
 
         bacpy(&addr.l2_bdaddr, dst);
         addr.l2_family = AF_BLUETOOTH;
-        addr.l2_psm    = cpu_to_le16(RFCOMM_PSM);
+        addr.l2_psm    = __constant_cpu_to_le16(RFCOMM_PSM);
         addr.l2_cid    = 0;
         *err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr), O_NONBLOCK);
         if (*err == 0 || *err == -EINPROGRESS)
@@ -1987,7 +1987,7 @@ static int rfcomm_add_listener(bdaddr_t *ba)
         /* Bind socket */
         bacpy(&addr.l2_bdaddr, ba);
         addr.l2_family = AF_BLUETOOTH;
-        addr.l2_psm    = cpu_to_le16(RFCOMM_PSM);
+        addr.l2_psm    = __constant_cpu_to_le16(RFCOMM_PSM);
         addr.l2_cid    = 0;
         err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr));
         if (err < 0) {
@@ -2125,11 +2125,10 @@ static int rfcomm_dlc_debugfs_show(struct seq_file *f, void *x)
                 list_for_each_entry(d, &s->dlcs, list) {
                         struct sock *sk = s->sock->sk;
 
-                        seq_printf(f, "%s %s %ld %d %d %d %d\n",
-                                                batostr(&bt_sk(sk)->src),
-                                                batostr(&bt_sk(sk)->dst),
-                                                d->state, d->dlci, d->mtu,
-                                                d->rx_credits, d->tx_credits);
+                        seq_printf(f, "%pMR %pMR %ld %d %d %d %d\n",
+                                   &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                                   d->state, d->dlci, d->mtu,
+                                   d->rx_credits, d->tx_credits);
                 }
         }
 
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index b3226f3658cf..4ddef57d03a7 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -334,7 +334,7 @@ static int rfcomm_sock_bind(struct socket *sock, struct sockaddr *addr, int addr
         struct sock *sk = sock->sk;
         int err = 0;
 
-        BT_DBG("sk %p %s", sk, batostr(&sa->rc_bdaddr));
+        BT_DBG("sk %p %pMR", sk, &sa->rc_bdaddr);
 
         if (!addr || addr->sa_family != AF_BLUETOOTH)
                 return -EINVAL;
@@ -975,10 +975,9 @@ static int rfcomm_sock_debugfs_show(struct seq_file *f, void *p)
         read_lock(&rfcomm_sk_list.lock);
 
         sk_for_each(sk, node, &rfcomm_sk_list.head) {
-                seq_printf(f, "%s %s %d %d\n",
-                                batostr(&bt_sk(sk)->src),
-                                batostr(&bt_sk(sk)->dst),
-                                sk->sk_state, rfcomm_pi(sk)->channel);
+                seq_printf(f, "%pMR %pMR %d %d\n",
+                           &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                           sk->sk_state, rfcomm_pi(sk)->channel);
         }
 
         read_unlock(&rfcomm_sk_list.lock);
diff --git a/net/bluetooth/rfcomm/tty.c b/net/bluetooth/rfcomm/tty.c
index ccc248791d50..bd6fd0f43d2b 100644
--- a/net/bluetooth/rfcomm/tty.c
+++ b/net/bluetooth/rfcomm/tty.c
@@ -166,7 +166,7 @@ static struct device *rfcomm_get_device(struct rfcomm_dev *dev)
 static ssize_t show_address(struct device *tty_dev, struct device_attribute *attr, char *buf)
 {
         struct rfcomm_dev *dev = dev_get_drvdata(tty_dev);
-        return sprintf(buf, "%s\n", batostr(&dev->dst));
+        return sprintf(buf, "%pMR\n", &dev->dst);
 }
 
 static ssize_t show_channel(struct device *tty_dev, struct device_attribute *attr, char *buf)
@@ -663,8 +663,8 @@ static int rfcomm_tty_open(struct tty_struct *tty, struct file *filp)
         if (!dev)
                 return -ENODEV;
 
-        BT_DBG("dev %p dst %s channel %d opened %d", dev, batostr(&dev->dst),
-                                dev->channel, dev->port.count);
+        BT_DBG("dev %p dst %pMR channel %d opened %d", dev, &dev->dst,
+               dev->channel, dev->port.count);
 
         spin_lock_irqsave(&dev->port.lock, flags);
         if (++dev->port.count > 1) {
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index dc42b917aaaf..450cdcd88e5c 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -172,7 +172,7 @@ static int sco_connect(struct sock *sk)
         struct hci_dev  *hdev;
         int err, type;
 
-        BT_DBG("%s -> %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         hdev = hci_get_route(dst, src);
         if (!hdev)
@@ -460,7 +460,7 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
         struct sock *sk = sock->sk;
         int err = 0;
 
-        BT_DBG("sk %p %s", sk, batostr(&sa->sco_bdaddr));
+        BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
 
         if (!addr || addr->sa_family != AF_BLUETOOTH)
                 return -EINVAL;
@@ -893,7 +893,7 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
         struct hlist_node *node;
         int lm = 0;
 
-        BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
+        BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
 
         /* Find listening sockets */
         read_lock(&sco_sk_list.lock);
@@ -914,7 +914,7 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
 
 void sco_connect_cfm(struct hci_conn *hcon, __u8 status)
 {
-        BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
+        BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
         if (!status) {
                 struct sco_conn *conn;
 
@@ -959,8 +959,8 @@ static int sco_debugfs_show(struct seq_file *f, void *p)
         read_lock(&sco_sk_list.lock);
 
         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);
+                seq_printf(f, "%pMR %pMR %d\n", &bt_sk(sk)->src,
+                           &bt_sk(sk)->dst, sk->sk_state);
         }
 
         read_unlock(&sco_sk_list.lock);
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index 8c225ef349cd..9176bc17595c 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -32,6 +32,8 @@
 
 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
 
+#define AUTH_REQ_MASK   0x07
+
 static inline void swap128(u8 src[16], u8 dst[16])
 {
         int i;
@@ -165,7 +167,7 @@ static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
 
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->len = cpu_to_le16(sizeof(code) + dlen);
-        lh->cid = cpu_to_le16(L2CAP_CID_SMP);
+        lh->cid = __constant_cpu_to_le16(L2CAP_CID_SMP);
 
         memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
 
@@ -230,7 +232,7 @@ static void build_pairing_cmd(struct l2cap_conn *conn,
                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
                 req->init_key_dist = 0;
                 req->resp_key_dist = dist_keys;
-                req->auth_req = authreq;
+                req->auth_req = (authreq & AUTH_REQ_MASK);
                 return;
         }
 
@@ -239,7 +241,7 @@ static void build_pairing_cmd(struct l2cap_conn *conn,
         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
         rsp->init_key_dist = 0;
         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
-        rsp->auth_req = authreq;
+        rsp->auth_req = (authreq & AUTH_REQ_MASK);
 }
 
 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 3d5332e367f8..c7386b2b767e 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -1110,7 +1110,7 @@ int ieee80211_ibss_join(struct ieee80211_sub_if_data *sdata,
         sdata->u.ibss.state = IEEE80211_IBSS_MLME_SEARCH;
         sdata->u.ibss.ibss_join_req = jiffies;
 
-        memcpy(sdata->u.ibss.ssid, params->ssid, IEEE80211_MAX_SSID_LEN);
+        memcpy(sdata->u.ibss.ssid, params->ssid, params->ssid_len);
         sdata->u.ibss.ssid_len = params->ssid_len;
 
         mutex_unlock(&sdata->u.ibss.mtx);
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 944c6cf53eb7..1a6fe135f201 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -840,7 +840,7 @@ static void ieee80211_do_stop(struct ieee80211_sub_if_data *sdata,
                         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
                         if (info->control.vif == &sdata->vif) {
                                 __skb_unlink(skb, &local->pending[i]);
-                                dev_kfree_skb_irq(skb);
+                                ieee80211_free_txskb(&local->hw, skb);
                         }
                 }
         }
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 2bdf7769506f..1d1fdf0791f0 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -3177,26 +3177,37 @@ static int ieee80211_prep_channel(struct ieee80211_sub_if_data *sdata,
                                    ht_cfreq, ht_oper->primary_chan,
                                    cbss->channel->band);
                         ht_oper = NULL;
+                } else {
+                        channel_type = NL80211_CHAN_HT20;
                 }
         }
 
-        if (ht_oper) {
+        if (ht_oper && sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
+                /*
+                 * cfg80211 already verified that the channel itself can
+                 * be used, but it didn't check that we can do the right
+                 * HT type, so do that here as well. If HT40 isn't allowed
+                 * on this channel, disable 40 MHz operation.
+                 */
                 const u8 *ht_cap_ie;
                 const struct ieee80211_ht_cap *ht_cap;
                 u8 chains = 1;
 
                 channel_type = NL80211_CHAN_HT20;
 
-                if (sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
-                        switch (ht_oper->ht_param &
-                                        IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
-                        case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+                switch (ht_oper->ht_param & IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
+                case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+                        if (cbss->channel->flags & IEEE80211_CHAN_NO_HT40PLUS)
+                                ifmgd->flags |= IEEE80211_STA_DISABLE_40MHZ;
+                        else
                                 channel_type = NL80211_CHAN_HT40PLUS;
-                                break;
-                        case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+                        break;
+                case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+                        if (cbss->channel->flags & IEEE80211_CHAN_NO_HT40MINUS)
+                                ifmgd->flags |= IEEE80211_STA_DISABLE_40MHZ;
+                        else
                                 channel_type = NL80211_CHAN_HT40MINUS;
-                                break;
-                        }
+                        break;
                 }
 
                 ht_cap_ie = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
@@ -3648,6 +3659,7 @@ int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
 {
         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
+        bool tx = !req->local_state_change;
 
         mutex_lock(&ifmgd->mtx);
 
@@ -3664,12 +3676,12 @@ int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
         if (ifmgd->associated &&
             ether_addr_equal(ifmgd->associated->bssid, req->bssid)) {
                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
-                                       req->reason_code, true, frame_buf);
+                                       req->reason_code, tx, frame_buf);
         } else {
                 drv_mgd_prepare_tx(sdata->local, sdata);
                 ieee80211_send_deauth_disassoc(sdata, req->bssid,
                                                IEEE80211_STYPE_DEAUTH,
-                                               req->reason_code, true,
+                                               req->reason_code, tx,
                                                frame_buf);
         }
 
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index c0a1f53e68ab..38b382682cae 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -525,6 +525,11 @@ static ieee80211_rx_result ieee80211_rx_mesh_check(struct ieee80211_rx_data *rx)
 
                 if (ieee80211_is_action(hdr->frame_control)) {
                         u8 category;
+
+                        /* make sure category field is present */
+                        if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE)
+                                return RX_DROP_MONITOR;
+
                         mgmt = (struct ieee80211_mgmt *)hdr;
                         category = mgmt->u.action.category;
                         if (category != WLAN_CATEGORY_MESH_ACTION &&
@@ -875,14 +880,16 @@ ieee80211_rx_h_check(struct ieee80211_rx_data *rx)
                  */
                 if (rx->sta && rx->sdata->vif.type == NL80211_IFTYPE_STATION &&
                     ieee80211_is_data_present(hdr->frame_control)) {
-                        u16 ethertype;
-                        u8 *payload;
-
-                        payload = rx->skb->data +
-                                ieee80211_hdrlen(hdr->frame_control);
-                        ethertype = (payload[6] << 8) | payload[7];
-                        if (cpu_to_be16(ethertype) ==
-                            rx->sdata->control_port_protocol)
+                        unsigned int hdrlen;
+                        __be16 ethertype;
+
+                        hdrlen = ieee80211_hdrlen(hdr->frame_control);
+
+                        if (rx->skb->len < hdrlen + 8)
+                                return RX_DROP_MONITOR;
+
+                        skb_copy_bits(rx->skb, hdrlen + 6, &ethertype, 2);
+                        if (ethertype == rx->sdata->control_port_protocol)
                                 return RX_CONTINUE;
                 }
 
@@ -1459,11 +1466,14 @@ ieee80211_rx_h_defragment(struct ieee80211_rx_data *rx)
 
         hdr = (struct ieee80211_hdr *)rx->skb->data;
         fc = hdr->frame_control;
+
+        if (ieee80211_is_ctl(fc))
+                return RX_CONTINUE;
+
         sc = le16_to_cpu(hdr->seq_ctrl);
         frag = sc & IEEE80211_SCTL_FRAG;
 
         if (likely((!ieee80211_has_morefrags(fc) && frag == 0) ||
-                   (rx->skb)->len < 24 ||
                    is_multicast_ether_addr(hdr->addr1))) {
                 /* not fragmented */
                 goto out;
@@ -1882,6 +1892,20 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
 
         hdr = (struct ieee80211_hdr *) skb->data;
         hdrlen = ieee80211_hdrlen(hdr->frame_control);
+
+        /* make sure fixed part of mesh header is there, also checks skb len */
+        if (!pskb_may_pull(rx->skb, hdrlen + 6))
+                return RX_DROP_MONITOR;
+
+        mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
+
+        /* make sure full mesh header is there, also checks skb len */
+        if (!pskb_may_pull(rx->skb,
+                           hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr)))
+                return RX_DROP_MONITOR;
+
+        /* reload pointers */
+        hdr = (struct ieee80211_hdr *) skb->data;
         mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
 
         /* frame is in RMC, don't forward */
@@ -1890,7 +1914,8 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
             mesh_rmc_check(hdr->addr3, mesh_hdr, rx->sdata))
                 return RX_DROP_MONITOR;
 
-        if (!ieee80211_is_data(hdr->frame_control))
+        if (!ieee80211_is_data(hdr->frame_control) ||
+            !(status->rx_flags & IEEE80211_RX_RA_MATCH))
                 return RX_CONTINUE;
 
         if (!mesh_hdr->ttl)
@@ -1904,9 +1929,12 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
                 if (is_multicast_ether_addr(hdr->addr1)) {
                         mpp_addr = hdr->addr3;
                         proxied_addr = mesh_hdr->eaddr1;
-                } else {
+                } else if (mesh_hdr->flags & MESH_FLAGS_AE_A5_A6) {
+                        /* has_a4 already checked in ieee80211_rx_mesh_check */
                         mpp_addr = hdr->addr4;
                         proxied_addr = mesh_hdr->eaddr2;
+                } else {
+                        return RX_DROP_MONITOR;
                 }
 
                 rcu_read_lock();
@@ -1934,12 +1962,9 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
         }
         skb_set_queue_mapping(skb, q);
 
-        if (!(status->rx_flags & IEEE80211_RX_RA_MATCH))
-                goto out;
-
         if (!--mesh_hdr->ttl) {
                 IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_ttl);
-                return RX_DROP_MONITOR;
+                goto out;
         }
 
         if (!ifmsh->mshcfg.dot11MeshForwarding)
@@ -2346,6 +2371,10 @@ ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
                 }
                 break;
         case WLAN_CATEGORY_SELF_PROTECTED:
+                if (len < (IEEE80211_MIN_ACTION_SIZE +
+                           sizeof(mgmt->u.action.u.self_prot.action_code)))
+                        break;
+
                 switch (mgmt->u.action.u.self_prot.action_code) {
                 case WLAN_SP_MESH_PEERING_OPEN:
                 case WLAN_SP_MESH_PEERING_CLOSE:
@@ -2364,6 +2393,10 @@ ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
                 }
                 break;
         case WLAN_CATEGORY_MESH_ACTION:
+                if (len < (IEEE80211_MIN_ACTION_SIZE +
+                           sizeof(mgmt->u.action.u.mesh_action.action_code)))
+                        break;
+
                 if (!ieee80211_vif_is_mesh(&sdata->vif))
                         break;
                 if (mesh_action_is_path_sel(mgmt) &&
@@ -2905,10 +2938,15 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw,
         if (ieee80211_is_data(fc) || ieee80211_is_mgmt(fc))
                 local->dot11ReceivedFragmentCount++;
 
-        if (ieee80211_is_mgmt(fc))
-                err = skb_linearize(skb);
-        else
+        if (ieee80211_is_mgmt(fc)) {
+                /* drop frame if too short for header */
+                if (skb->len < ieee80211_hdrlen(fc))
+                        err = -ENOBUFS;
+                else
+                        err = skb_linearize(skb);
+        } else {
                 err = !pskb_may_pull(skb, ieee80211_hdrlen(fc));
+        }
 
         if (err) {
                 dev_kfree_skb(skb);
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index daf55e1e0fd3..f7bb54f9ab72 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -664,7 +664,7 @@ static bool sta_info_cleanup_expire_buffered_ac(struct ieee80211_local *local,
                  */
                 if (!skb)
                         break;
-                dev_kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
         }
 
         /*
@@ -693,7 +693,7 @@ static bool sta_info_cleanup_expire_buffered_ac(struct ieee80211_local *local,
                 local->total_ps_buffered--;
                 ps_dbg(sta->sdata, "Buffered frame expired (STA %pM)\n",
                        sta->sta.addr);
-                dev_kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
         }
 
         /*
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index dd0e6f20fc51..6636d3962317 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -406,7 +406,7 @@ void ieee80211_add_pending_skb(struct ieee80211_local *local,
         int queue = info->hw_queue;
 
         if (WARN_ON(!info->control.vif)) {
-                kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
                 return;
         }
 
@@ -431,7 +431,7 @@ void ieee80211_add_pending_skbs_fn(struct ieee80211_local *local,
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
                 if (WARN_ON(!info->control.vif)) {
-                        kfree_skb(skb);
+                        ieee80211_free_txskb(&local->hw, skb);
                         continue;
                 }
 
@@ -643,13 +643,41 @@ u32 ieee802_11_parse_elems_crc(u8 *start, size_t len,
                         break;
                 }
 
-                if (id != WLAN_EID_VENDOR_SPECIFIC &&
-                    id != WLAN_EID_QUIET &&
-                    test_bit(id, seen_elems)) {
-                        elems->parse_error = true;
-                        left -= elen;
-                        pos += elen;
-                        continue;
+                switch (id) {
+                case WLAN_EID_SSID:
+                case WLAN_EID_SUPP_RATES:
+                case WLAN_EID_FH_PARAMS:
+                case WLAN_EID_DS_PARAMS:
+                case WLAN_EID_CF_PARAMS:
+                case WLAN_EID_TIM:
+                case WLAN_EID_IBSS_PARAMS:
+                case WLAN_EID_CHALLENGE:
+                case WLAN_EID_RSN:
+                case WLAN_EID_ERP_INFO:
+                case WLAN_EID_EXT_SUPP_RATES:
+                case WLAN_EID_HT_CAPABILITY:
+                case WLAN_EID_HT_OPERATION:
+                case WLAN_EID_VHT_CAPABILITY:
+                case WLAN_EID_VHT_OPERATION:
+                case WLAN_EID_MESH_ID:
+                case WLAN_EID_MESH_CONFIG:
+                case WLAN_EID_PEER_MGMT:
+                case WLAN_EID_PREQ:
+                case WLAN_EID_PREP:
+                case WLAN_EID_PERR:
+                case WLAN_EID_RANN:
+                case WLAN_EID_CHANNEL_SWITCH:
+                case WLAN_EID_EXT_CHANSWITCH_ANN:
+                case WLAN_EID_COUNTRY:
+                case WLAN_EID_PWR_CONSTRAINT:
+                case WLAN_EID_TIMEOUT_INTERVAL:
+                        if (test_bit(id, seen_elems)) {
+                                elems->parse_error = true;
+                                left -= elen;
+                                pos += elen;
+                                continue;
+                        }
+                        break;
                 }
 
                 if (calc_crc && id < 64 && (filter & (1ULL << id)))
diff --git a/net/mac80211/wpa.c b/net/mac80211/wpa.c
index bdb53aba888e..8bd2f5c6a56e 100644
--- a/net/mac80211/wpa.c
+++ b/net/mac80211/wpa.c
@@ -106,7 +106,8 @@ ieee80211_rx_h_michael_mic_verify(struct ieee80211_rx_data *rx)
                 if (status->flag & RX_FLAG_MMIC_ERROR)
                         goto mic_fail;
 
-                if (!(status->flag & RX_FLAG_IV_STRIPPED) && rx->key)
+                if (!(status->flag & RX_FLAG_IV_STRIPPED) && rx->key &&
+                    rx->key->conf.cipher == WLAN_CIPHER_SUITE_TKIP)
                         goto update_iv;
 
                 return RX_CONTINUE;
@@ -545,14 +546,19 @@ ieee80211_crypto_ccmp_decrypt(struct ieee80211_rx_data *rx)
 
 static void bip_aad(struct sk_buff *skb, u8 *aad)
 {
+        __le16 mask_fc;
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+
         /* BIP AAD: FC(masked) || A1 || A2 || A3 */
 
         /* FC type/subtype */
-        aad[0] = skb->data[0];
         /* Mask FC Retry, PwrMgt, MoreData flags to zero */
-        aad[1] = skb->data[1] & ~(BIT(4) | BIT(5) | BIT(6));
+        mask_fc = hdr->frame_control;
+        mask_fc &= ~cpu_to_le16(IEEE80211_FCTL_RETRY | IEEE80211_FCTL_PM |
+                                IEEE80211_FCTL_MOREDATA);
+        put_unaligned(mask_fc, (__le16 *) &aad[0]);
         /* A1 || A2 || A3 */
-        memcpy(aad + 2, skb->data + 4, 3 * ETH_ALEN);
+        memcpy(aad + 2, &hdr->addr1, 3 * ETH_ALEN);
 }
 
 
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
index 8d8d9bc4b6ff..60c3bbb63e8e 100644
--- a/net/nfc/Kconfig
+++ b/net/nfc/Kconfig
@@ -3,8 +3,8 @@
 #
 
 menuconfig NFC
-        depends on NET && EXPERIMENTAL
-        tristate "NFC subsystem support (EXPERIMENTAL)"
+        depends on NET
+        tristate "NFC subsystem support"
         default n
         help
           Say Y here if you want to build support for NFC (Near field
diff --git a/net/nfc/core.c b/net/nfc/core.c
index 479bee36dc3e..aa64ea441676 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -40,6 +40,9 @@
 int nfc_devlist_generation;
 DEFINE_MUTEX(nfc_devlist_mutex);
 
+/* NFC device ID bitmap */
+static DEFINE_IDA(nfc_index_ida);
+
 /**
  * nfc_dev_up - turn on the NFC device
  *
@@ -181,6 +184,7 @@ int nfc_stop_poll(struct nfc_dev *dev)
 
         dev->ops->stop_poll(dev);
         dev->polling = false;
+        dev->rf_mode = NFC_RF_NONE;
 
 error:
         device_unlock(&dev->dev);
@@ -194,7 +198,7 @@ static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
         if (dev->n_targets == 0)
                 return NULL;
 
-        for (i = 0; i < dev->n_targets ; i++) {
+        for (i = 0; i < dev->n_targets; i++) {
                 if (dev->targets[i].idx == target_idx)
                         return &dev->targets[i];
         }
@@ -274,12 +278,14 @@ int nfc_dep_link_down(struct nfc_dev *dev)
         if (!rc) {
                 dev->dep_link_up = false;
                 dev->active_target = NULL;
+                dev->rf_mode = NFC_RF_NONE;
                 nfc_llcp_mac_is_down(dev);
                 nfc_genl_dep_link_down_event(dev);
         }
 
 error:
         device_unlock(&dev->dev);
+
         return rc;
 }
 
@@ -503,6 +509,7 @@ EXPORT_SYMBOL(nfc_tm_activated);
 int nfc_tm_deactivated(struct nfc_dev *dev)
 {
         dev->dep_link_up = false;
+        dev->rf_mode = NFC_RF_NONE;
 
         return nfc_genl_tm_deactivated(dev);
 }
@@ -697,6 +704,8 @@ static void nfc_check_pres_work(struct work_struct *work)
 
         if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
                 rc = dev->ops->check_presence(dev, dev->active_target);
+                if (rc == -EOPNOTSUPP)
+                        goto exit;
                 if (!rc) {
                         mod_timer(&dev->check_pres_timer, jiffies +
                                   msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
@@ -708,6 +717,7 @@ static void nfc_check_pres_work(struct work_struct *work)
                 }
         }
 
+exit:
         device_unlock(&dev->dev);
 }
 
@@ -753,7 +763,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
                                     u32 supported_protocols,
                                     int tx_headroom, int tx_tailroom)
 {
-        static atomic_t dev_no = ATOMIC_INIT(0);
         struct nfc_dev *dev;
 
         if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
@@ -767,11 +776,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
         if (!dev)
                 return NULL;
 
-        dev->dev.class = &nfc_class;
-        dev->idx = atomic_inc_return(&dev_no) - 1;
-        dev_set_name(&dev->dev, "nfc%d", dev->idx);
-        device_initialize(&dev->dev);
-
         dev->ops = ops;
         dev->supported_protocols = supported_protocols;
         dev->tx_headroom = tx_headroom;
@@ -779,6 +783,7 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 
         nfc_genl_data_init(&dev->genl_data);
 
+        dev->rf_mode = NFC_RF_NONE;
 
         /* first generation must not be 0 */
         dev->targets_generation = 1;
@@ -806,6 +811,14 @@ int nfc_register_device(struct nfc_dev *dev)
 
         pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+        dev->idx = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL);
+        if (dev->idx < 0)
+                return dev->idx;
+
+        dev->dev.class = &nfc_class;
+        dev_set_name(&dev->dev, "nfc%d", dev->idx);
+        device_initialize(&dev->dev);
+
         mutex_lock(&nfc_devlist_mutex);
         nfc_devlist_generation++;
         rc = device_add(&dev->dev);
@@ -834,10 +847,12 @@ EXPORT_SYMBOL(nfc_register_device);
  */
 void nfc_unregister_device(struct nfc_dev *dev)
 {
-        int rc;
+        int rc, id;
 
         pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+        id = dev->idx;
+
         mutex_lock(&nfc_devlist_mutex);
         nfc_devlist_generation++;
 
@@ -856,6 +871,8 @@ void nfc_unregister_device(struct nfc_dev *dev)
                 pr_debug("The userspace won't be notified that the device %s was removed\n",
                          dev_name(&dev->dev));
 
+        ida_simple_remove(&nfc_index_ida, id);
+
 }
 EXPORT_SYMBOL(nfc_unregister_device);
 
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
index 71c6a7086b8f..07659cfd6d7b 100644
--- a/net/nfc/hci/command.c
+++ b/net/nfc/hci/command.c
@@ -257,16 +257,16 @@ static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host,
         *result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
                                       NFC_HCI_ADM_CREATE_PIPE,
                                       (u8 *) &params, sizeof(params), &skb);
-        if (*result == 0) {
-                resp = (struct hci_create_pipe_resp *)skb->data;
-                pipe = resp->pipe;
-                kfree_skb(skb);
+        if (*result < 0)
+                return NFC_HCI_INVALID_PIPE;
 
-                pr_debug("pipe created=%d\n", pipe);
+        resp = (struct hci_create_pipe_resp *)skb->data;
+        pipe = resp->pipe;
+        kfree_skb(skb);
 
-                return pipe;
-        } else
-                return NFC_HCI_INVALID_PIPE;
+        pr_debug("pipe created=%d\n", pipe);
+
+        return pipe;
 }
 
 static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
@@ -279,8 +279,6 @@ static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
 
 static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 {
-        int r;
-
         u8 param[2];
 
         /* TODO: Find out what the identity reference data is
@@ -288,10 +286,8 @@ static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 
         pr_debug("\n");
 
-        r = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
-                                NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
-
-        return 0;
+        return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+                                   NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
 }
 
 int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate)
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index 5fbb6e40793e..bc571b0efb92 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -65,8 +65,9 @@ static void nfc_hci_msg_tx_work(struct work_struct *work)
                                                           -ETIME);
                         kfree(hdev->cmd_pending_msg);
                         hdev->cmd_pending_msg = NULL;
-                } else
+                } else {
                         goto exit;
+                }
         }
 
 next_msg:
@@ -182,7 +183,7 @@ static u32 nfc_hci_sak_to_protocol(u8 sak)
         }
 }
 
-static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 {
         struct nfc_target *targets;
         struct sk_buff *atqa_skb = NULL;
@@ -263,7 +264,9 @@ static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
                 break;
         }
 
-        targets->hci_reader_gate = gate;
+        /* if driver set the new gate, we will skip the old one */
+        if (targets->hci_reader_gate == 0x00)
+                targets->hci_reader_gate = gate;
 
         r = nfc_targets_found(hdev->ndev, targets, 1);
 
@@ -275,6 +278,7 @@ exit:
 
         return r;
 }
+EXPORT_SYMBOL(nfc_hci_target_discovered);
 
 void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                             struct sk_buff *skb)
@@ -307,8 +311,13 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                                               nfc_hci_pipe2gate(hdev, pipe));
                 break;
         default:
-                /* TODO: Unknown events are hardware specific
-                 * pass them to the driver (needs a new hci_ops) */
+                if (hdev->ops->event_received) {
+                        hdev->ops->event_received(hdev,
+                                                nfc_hci_pipe2gate(hdev, pipe),
+                                                event, skb);
+                        return;
+                }
+
                 break;
         }
 
@@ -527,7 +536,8 @@ static int hci_start_poll(struct nfc_dev *nfc_dev,
                 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);
+                                          NFC_HCI_EVT_READER_REQUESTED,
+                                          NULL, 0);
 }
 
 static void hci_stop_poll(struct nfc_dev *nfc_dev)
@@ -538,6 +548,28 @@ static void hci_stop_poll(struct nfc_dev *nfc_dev)
                            NFC_HCI_EVT_END_OPERATION, NULL, 0);
 }
 
+static int hci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+                                __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->dep_link_up)
+                return hdev->ops->dep_link_up(hdev, target, comm_mode,
+                                                gb, gb_len);
+
+        return 0;
+}
+
+static int hci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->dep_link_down)
+                return hdev->ops->dep_link_down(hdev);
+
+        return 0;
+}
+
 static int hci_activate_target(struct nfc_dev *nfc_dev,
                                struct nfc_target *target, u32 protocol)
 {
@@ -586,8 +618,8 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
         switch (target->hci_reader_gate) {
         case NFC_HCI_RF_READER_A_GATE:
         case NFC_HCI_RF_READER_B_GATE:
-                if (hdev->ops->data_exchange) {
-                        r = hdev->ops->data_exchange(hdev, target, skb, cb,
+                if (hdev->ops->im_transceive) {
+                        r = hdev->ops->im_transceive(hdev, target, skb, cb,
                                                      cb_context);
                         if (r <= 0)     /* handled */
                                 break;
@@ -604,14 +636,14 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
                                            skb->len, hci_transceive_cb, hdev);
                 break;
         default:
-                if (hdev->ops->data_exchange) {
-                        r = hdev->ops->data_exchange(hdev, target, skb, cb,
+                if (hdev->ops->im_transceive) {
+                        r = hdev->ops->im_transceive(hdev, target, skb, cb,
                                                      cb_context);
                         if (r == 1)
                                 r = -ENOTSUPP;
-                }
-                else
+                } else {
                         r = -ENOTSUPP;
+                }
                 break;
         }
 
@@ -620,6 +652,16 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
         return r;
 }
 
+static int hci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->tm_send)
+                return hdev->ops->tm_send(hdev, skb);
+        else
+                return -ENOTSUPP;
+}
+
 static int hci_check_presence(struct nfc_dev *nfc_dev,
                               struct nfc_target *target)
 {
@@ -723,9 +765,12 @@ static struct nfc_ops hci_nfc_ops = {
         .dev_down = hci_dev_down,
         .start_poll = hci_start_poll,
         .stop_poll = hci_stop_poll,
+        .dep_link_up = hci_dep_link_up,
+        .dep_link_down = hci_dep_link_down,
         .activate_target = hci_activate_target,
         .deactivate_target = hci_deactivate_target,
         .im_transceive = hci_transceive,
+        .tm_send = hci_tm_send,
         .check_presence = hci_check_presence,
 };
 
@@ -848,7 +893,7 @@ void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
 }
 EXPORT_SYMBOL(nfc_hci_driver_failure);
 
-void inline nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 {
         nfc_llc_rcv_from_drv(hdev->llc, skb);
 }
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
index ae1205ded87f..fe5e966e5b88 100644
--- a/net/nfc/hci/llc.c
+++ b/net/nfc/hci/llc.c
@@ -72,7 +72,7 @@ int nfc_llc_register(const char *name, struct nfc_llc_ops *ops)
         llc_engine->ops = ops;
 
         INIT_LIST_HEAD(&llc_engine->entry);
-        list_add_tail (&llc_engine->entry, &llc_engines);
+        list_add_tail(&llc_engine->entry, &llc_engines);
 
         return 0;
 }
diff --git a/net/nfc/hci/llc_shdlc.c b/net/nfc/hci/llc_shdlc.c
index 01cbc72943cd..27b313befc35 100644
--- a/net/nfc/hci/llc_shdlc.c
+++ b/net/nfc/hci/llc_shdlc.c
@@ -634,9 +634,9 @@ static void llc_shdlc_sm_work(struct work_struct *work)
                         r = llc_shdlc_connect_initiate(shdlc);
                 else
                         r = -ETIME;
-                if (r < 0)
+                if (r < 0) {
                         llc_shdlc_connect_complete(shdlc, r);
-                else {
+                } else {
                         mod_timer(&shdlc->connect_timer, jiffies +
                                   msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
 
@@ -682,9 +682,8 @@ static void llc_shdlc_sm_work(struct work_struct *work)
                         llc_shdlc_handle_send_queue(shdlc);
                 }
 
-                if (shdlc->hard_fault) {
+                if (shdlc->hard_fault)
                         shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
-                }
                 break;
         default:
                 break;
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
index fbf5e8150908..a1a41cd68255 100644
--- a/net/nfc/llcp/Kconfig
+++ b/net/nfc/llcp/Kconfig
@@ -1,6 +1,6 @@
 config NFC_LLCP
-       depends on NFC && EXPERIMENTAL
-       bool "NFC LLCP support (EXPERIMENTAL)"
+       depends on NFC
+       bool "NFC LLCP support"
        default n
        help
          Say Y here if you want to build support for a kernel NFC LLCP
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index c45ccd6c094c..ed2d17312d61 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -261,7 +261,6 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
         struct sk_buff *skb;
         struct nfc_dev *dev;
         struct nfc_llcp_local *local;
-        u16 size = 0;
 
         pr_debug("Sending DISC\n");
 
@@ -273,17 +272,10 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
         if (dev == NULL)
                 return -ENODEV;
 
-        size += LLCP_HEADER_SIZE;
-        size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-
-        skb = alloc_skb(size, GFP_ATOMIC);
+        skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
         if (skb == NULL)
                 return -ENOMEM;
 
-        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-        skb = llcp_add_header(skb, sock->dsap, sock->ssap, LLCP_PDU_DISC);
-
         skb_queue_tail(&local->tx_queue, skb);
 
         return 0;
@@ -324,8 +316,7 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
         struct sk_buff *skb;
         u8 *service_name_tlv = NULL, service_name_tlv_length;
         u8 *miux_tlv = NULL, miux_tlv_length;
-        u8 *rw_tlv = NULL, rw_tlv_length, rw;
-        __be16 miux;
+        u8 *rw_tlv = NULL, rw_tlv_length;
         int err;
         u16 size = 0;
 
@@ -343,13 +334,11 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
                 size += service_name_tlv_length;
         }
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_tlv_length);
         size += miux_tlv_length;
 
-        rw = LLCP_MAX_RW;
-        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
         size += rw_tlv_length;
 
         pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
@@ -386,8 +375,7 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
         struct nfc_llcp_local *local;
         struct sk_buff *skb;
         u8 *miux_tlv = NULL, miux_tlv_length;
-        u8 *rw_tlv = NULL, rw_tlv_length, rw;
-        __be16 miux;
+        u8 *rw_tlv = NULL, rw_tlv_length;
         int err;
         u16 size = 0;
 
@@ -397,13 +385,11 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
         if (local == NULL)
                 return -ENODEV;
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_tlv_length);
         size += miux_tlv_length;
 
-        rw = LLCP_MAX_RW;
-        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
         size += rw_tlv_length;
 
         skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
@@ -428,6 +414,52 @@ error_tlv:
         return err;
 }
 
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap)
+{
+        struct sk_buff *skb;
+        struct nfc_dev *dev;
+        u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2];
+        u16 size = 0;
+
+        pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap);
+
+        if (local == NULL)
+                return -ENODEV;
+
+        dev = local->dev;
+        if (dev == NULL)
+                return -ENODEV;
+
+        sdres[0] = tid;
+        sdres[1] = sap;
+        sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0,
+                                       &sdres_tlv_length);
+        if (sdres_tlv == NULL)
+                return -ENOMEM;
+
+        size += LLCP_HEADER_SIZE;
+        size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+        size += sdres_tlv_length;
+
+        skb = alloc_skb(size, GFP_KERNEL);
+        if (skb == NULL) {
+                kfree(sdres_tlv);
+                return -ENOMEM;
+        }
+
+        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+        skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+        memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length);
+
+        skb_queue_tail(&local->tx_queue, skb);
+
+        kfree(sdres_tlv);
+
+        return 0;
+}
+
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
 {
         struct sk_buff *skb;
@@ -541,6 +573,52 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
         return len;
 }
 
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len)
+{
+        struct sk_buff *pdu;
+        struct nfc_llcp_local *local;
+        size_t frag_len = 0, remaining_len;
+        u8 *msg_ptr;
+        int err;
+
+        pr_debug("Send UI frame len %zd\n", len);
+
+        local = sock->local;
+        if (local == NULL)
+                return -ENODEV;
+
+        remaining_len = len;
+        msg_ptr = (u8 *) msg->msg_iov;
+
+        while (remaining_len > 0) {
+
+                frag_len = min_t(size_t, sock->miu, remaining_len);
+
+                pr_debug("Fragment %zd bytes remaining %zd",
+                         frag_len, remaining_len);
+
+                pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                         frag_len + LLCP_HEADER_SIZE, &err);
+                if (pdu == NULL) {
+                        pr_err("Could not allocate PDU\n");
+                        continue;
+                }
+
+                pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+                memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+                /* No need to check for the peer RW for UI frames */
+                skb_queue_tail(&local->tx_queue, pdu);
+
+                remaining_len -= frag_len;
+                msg_ptr += frag_len;
+        }
+
+        return len;
+}
+
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
 {
         struct sk_buff *skb;
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index cc10d073c338..f6804532047a 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -45,12 +45,38 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
         write_unlock(&l->lock);
 }
 
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+        struct nfc_llcp_local *local = sock->local;
+        struct sk_buff *s, *tmp;
+
+        pr_debug("%p\n", &sock->sk);
+
+        skb_queue_purge(&sock->tx_queue);
+        skb_queue_purge(&sock->tx_pending_queue);
+        skb_queue_purge(&sock->tx_backlog_queue);
+
+        if (local == NULL)
+                return;
+
+        /* Search for local pending SKBs that are related to this socket */
+        skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+                if (s->sk != &sock->sk)
+                        continue;
+
+                skb_unlink(s, &local->tx_queue);
+                kfree_skb(s);
+        }
+}
+
 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 {
         struct sock *sk;
         struct hlist_node *node, *tmp;
         struct nfc_llcp_sock *llcp_sock;
 
+        skb_queue_purge(&local->tx_queue);
+
         write_lock(&local->sockets.lock);
 
         sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
@@ -58,6 +84,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 
                 bh_lock_sock(sk);
 
+                nfc_llcp_socket_purge(llcp_sock);
+
                 if (sk->sk_state == LLCP_CONNECTED)
                         nfc_put_device(llcp_sock->dev);
 
@@ -65,7 +93,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
                         struct nfc_llcp_sock *lsk, *n;
                         struct sock *accept_sk;
 
-                        list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+                        list_for_each_entry_safe(lsk, n,
+                                                 &llcp_sock->accept_queue,
                                                  accept_queue) {
                                 accept_sk = &lsk->sk;
                                 bh_lock_sock(accept_sk);
@@ -85,6 +114,16 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
                         }
                 }
 
+                /*
+                 * If we have a connection less socket bound, we keep it alive
+                 * if the device is still present.
+                 */
+                if (sk->sk_state == LLCP_BOUND && sk->sk_type == SOCK_DGRAM &&
+                    listen == true) {
+                        bh_unlock_sock(sk);
+                        continue;
+                }
+
                 sk->sk_state = LLCP_CLOSED;
 
                 bh_unlock_sock(sk);
@@ -134,7 +173,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 {
         struct sock *sk;
         struct hlist_node *node;
-        struct nfc_llcp_sock *llcp_sock;
+        struct nfc_llcp_sock *llcp_sock, *tmp_sock;
 
         pr_debug("ssap dsap %d %d\n", ssap, dsap);
 
@@ -146,10 +185,12 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
         llcp_sock = NULL;
 
         sk_for_each(sk, node, &local->sockets.head) {
-                llcp_sock = nfc_llcp_sock(sk);
+                tmp_sock = nfc_llcp_sock(sk);
 
-                if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
+                if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+                        llcp_sock = tmp_sock;
                         break;
+                }
         }
 
         read_unlock(&local->sockets.lock);
@@ -249,7 +290,12 @@ struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
 
                 pr_debug("llcp sock %p\n", tmp_sock);
 
-                if (tmp_sock->sk.sk_state != LLCP_LISTEN)
+                if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+                    tmp_sock->sk.sk_state != LLCP_LISTEN)
+                        continue;
+
+                if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+                    tmp_sock->sk.sk_state != LLCP_BOUND)
                         continue;
 
                 if (tmp_sock->service_name == NULL ||
@@ -421,10 +467,9 @@ static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 {
         u8 *gb_cur, *version_tlv, version, version_length;
-        u8 *lto_tlv, lto, lto_length;
+        u8 *lto_tlv, lto_length;
         u8 *wks_tlv, wks_length;
         u8 *miux_tlv, miux_length;
-        __be16 miux;
         u8 gb_len = 0;
         int ret = 0;
 
@@ -433,9 +478,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
                                          1, &version_length);
         gb_len += version_length;
 
-        /* 1500 ms */
-        lto = 150;
-        lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
+        lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
         gb_len += lto_length;
 
         pr_debug("Local wks 0x%lx\n", local->local_wks);
@@ -443,8 +486,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
                                      &wks_length);
         gb_len += wks_length;
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_length);
         gb_len += miux_length;
 
@@ -610,7 +652,10 @@ static void nfc_llcp_tx_work(struct work_struct *work)
         if (skb != NULL) {
                 sk = skb->sk;
                 llcp_sock = nfc_llcp_sock(sk);
-                if (llcp_sock != NULL) {
+
+                if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+                        nfc_llcp_send_symm(local->dev);
+                } else {
                         int ret;
 
                         pr_debug("Sending pending skb\n");
@@ -629,8 +674,6 @@ static void nfc_llcp_tx_work(struct work_struct *work)
                                 skb_queue_tail(&llcp_sock->tx_pending_queue,
                                                skb);
                         }
-                } else {
-                        nfc_llcp_send_symm(local->dev);
                 }
         } else {
                 nfc_llcp_send_symm(local->dev);
@@ -704,6 +747,39 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
         return NULL;
 }
 
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+                             struct sk_buff *skb)
+{
+        struct nfc_llcp_sock *llcp_sock;
+        struct nfc_llcp_ui_cb *ui_cb;
+        u8 dsap, ssap;
+
+        dsap = nfc_llcp_dsap(skb);
+        ssap = nfc_llcp_ssap(skb);
+
+        ui_cb = nfc_llcp_ui_skb_cb(skb);
+        ui_cb->dsap = dsap;
+        ui_cb->ssap = ssap;
+
+        printk("%s %d %d\n", __func__, dsap, ssap);
+
+        pr_debug("%d %d\n", dsap, ssap);
+
+        /* We're looking for a bound socket, not a client one */
+        llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+        if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+                return;
+
+        /* There is no sequence with UI frames */
+        skb_pull(skb, LLCP_HEADER_SIZE);
+        if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+                pr_err("receive queue is full\n");
+                skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
+        }
+
+        nfc_llcp_sock_put(llcp_sock);
+}
+
 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
                                   struct sk_buff *skb)
 {
@@ -823,9 +899,6 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 fail:
         /* Send DM */
         nfc_llcp_send_dm(local, dsap, ssap, reason);
-
-        return;
-
 }
 
 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
@@ -953,6 +1026,9 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 
         sk = &llcp_sock->sk;
         lock_sock(sk);
+
+        nfc_llcp_socket_purge(llcp_sock);
+
         if (sk->sk_state == LLCP_CLOSED) {
                 release_sock(sk);
                 nfc_llcp_sock_put(llcp_sock);
@@ -1027,7 +1103,7 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
         }
 
         if (llcp_sock == NULL) {
-                pr_err("Invalid DM\n");
+                pr_debug("Already closed\n");
                 return;
         }
 
@@ -1038,8 +1114,100 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
         sk->sk_state_change(sk);
 
         nfc_llcp_sock_put(llcp_sock);
+}
 
-        return;
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+                              struct sk_buff *skb)
+{
+        struct nfc_llcp_sock *llcp_sock;
+        u8 dsap, ssap, *tlv, type, length, tid, sap;
+        u16 tlv_len, offset;
+        char *service_name;
+        size_t service_name_len;
+
+        dsap = nfc_llcp_dsap(skb);
+        ssap = nfc_llcp_ssap(skb);
+
+        pr_debug("%d %d\n", dsap, ssap);
+
+        if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+                pr_err("Wrong SNL SAP\n");
+                return;
+        }
+
+        tlv = &skb->data[LLCP_HEADER_SIZE];
+        tlv_len = skb->len - LLCP_HEADER_SIZE;
+        offset = 0;
+
+        while (offset < tlv_len) {
+                type = tlv[0];
+                length = tlv[1];
+
+                switch (type) {
+                case LLCP_TLV_SDREQ:
+                        tid = tlv[2];
+                        service_name = (char *) &tlv[3];
+                        service_name_len = length - 1;
+
+                        pr_debug("Looking for %.16s\n", service_name);
+
+                        if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+                            !strncmp(service_name, "urn:nfc:sn:sdp",
+                                     service_name_len)) {
+                                sap = 1;
+                                goto send_snl;
+                        }
+
+                        llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+                                                          service_name_len);
+                        if (!llcp_sock) {
+                                sap = 0;
+                                goto send_snl;
+                        }
+
+                        /*
+                         * We found a socket but its ssap has not been reserved
+                         * yet. We need to assign it for good and send a reply.
+                         * The ssap will be freed when the socket is closed.
+                         */
+                        if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+                                atomic_t *client_count;
+
+                                sap = nfc_llcp_reserve_sdp_ssap(local);
+
+                                pr_debug("Reserving %d\n", sap);
+
+                                if (sap == LLCP_SAP_MAX) {
+                                        sap = 0;
+                                        goto send_snl;
+                                }
+
+                                client_count =
+                                        &local->local_sdp_cnt[sap -
+                                                              LLCP_WKS_NUM_SAP];
+
+                                atomic_inc(client_count);
+
+                                llcp_sock->ssap = sap;
+                                llcp_sock->reserved_ssap = sap;
+                        } else {
+                                sap = llcp_sock->ssap;
+                        }
+
+                        pr_debug("%p %d\n", llcp_sock, sap);
+
+send_snl:
+                        nfc_llcp_send_snl(local, tid, sap);
+                        break;
+
+                default:
+                        pr_err("Invalid SNL tlv value 0x%x\n", type);
+                        break;
+                }
+
+                offset += length + 2;
+                tlv += length + 2;
+        }
 }
 
 static void nfc_llcp_rx_work(struct work_struct *work)
@@ -1072,6 +1240,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                 pr_debug("SYMM\n");
                 break;
 
+        case LLCP_PDU_UI:
+                pr_debug("UI\n");
+                nfc_llcp_recv_ui(local, skb);
+                break;
+
         case LLCP_PDU_CONNECT:
                 pr_debug("CONNECT\n");
                 nfc_llcp_recv_connect(local, skb);
@@ -1092,6 +1265,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                 nfc_llcp_recv_dm(local, skb);
                 break;
 
+        case LLCP_PDU_SNL:
+                pr_debug("SNL\n");
+                nfc_llcp_recv_snl(local, skb);
+                break;
+
         case LLCP_PDU_I:
         case LLCP_PDU_RR:
         case LLCP_PDU_RNR:
@@ -1104,8 +1282,6 @@ static void nfc_llcp_rx_work(struct work_struct *work)
         schedule_work(&local->tx_work);
         kfree_skb(local->rx_pending);
         local->rx_pending = NULL;
-
-        return;
 }
 
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
@@ -1121,8 +1297,6 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
         local->rx_pending = skb_get(skb);
         del_timer(&local->link_timer);
         schedule_work(&local->rx_work);
-
-        return;
 }
 
 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
@@ -1205,6 +1379,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
         rwlock_init(&local->connecting_sockets.lock);
         rwlock_init(&local->raw_sockets.lock);
 
+        local->lto = 150; /* 1500 ms */
+        local->rw = LLCP_MAX_RW;
+        local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
         nfc_llcp_build_gb(local);
 
         local->remote_miu = LLCP_DEFAULT_MIU;
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index fdb2d24e60bd..0d62366f8cc3 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -64,6 +64,9 @@ struct nfc_llcp_local {
         u32 target_idx;
         u8 rf_mode;
         u8 comm_mode;
+        u8 lto;
+        u8 rw;
+        __be16 miux;
         unsigned long local_wks;      /* Well known services */
         unsigned long local_sdp;      /* Local services  */
         unsigned long local_sap; /* Local SAPs, not available for discovery */
@@ -124,6 +127,13 @@ struct nfc_llcp_sock {
         struct sock *parent;
 };
 
+struct nfc_llcp_ui_cb {
+        __u8 dsap;
+        __u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
 #define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
 #define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
 
@@ -209,10 +219,13 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_symm(struct nfc_dev *dev);
 int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap);
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
                           struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len);
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
 
 /* Socket API */
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 63e4cdc92376..0fa1e92ceac8 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -205,8 +205,8 @@ static int llcp_sock_listen(struct socket *sock, int backlog)
 
         lock_sock(sk);
 
-        if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
-            || sk->sk_state != LLCP_BOUND) {
+        if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+            sk->sk_state != LLCP_BOUND) {
                 ret = -EBADFD;
                 goto error;
         }
@@ -608,6 +608,25 @@ static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 
         lock_sock(sk);
 
+        if (sk->sk_type == SOCK_DGRAM) {
+                struct sockaddr_nfc_llcp *addr =
+                        (struct sockaddr_nfc_llcp *)msg->msg_name;
+
+                if (msg->msg_namelen < sizeof(*addr)) {
+                        release_sock(sk);
+
+                        pr_err("Invalid socket address length %d\n",
+                               msg->msg_namelen);
+
+                        return -EINVAL;
+                }
+
+                release_sock(sk);
+
+                return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+                                              msg, len);
+        }
+
         if (sk->sk_state != LLCP_CONNECTED) {
                 release_sock(sk);
                 return -ENOTCONN;
@@ -663,11 +682,28 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
                 return -EFAULT;
         }
 
+        if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+                struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+                struct sockaddr_nfc_llcp sockaddr;
+
+                pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+                sockaddr.sa_family = AF_NFC;
+                sockaddr.nfc_protocol = NFC_PROTO_NFC_DEP;
+                sockaddr.dsap = ui_cb->dsap;
+                sockaddr.ssap = ui_cb->ssap;
+
+                memcpy(msg->msg_name, &sockaddr, sizeof(sockaddr));
+                msg->msg_namelen = sizeof(sockaddr);
+        }
+
         /* Mark read part of skb as used */
         if (!(flags & MSG_PEEK)) {
 
                 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
-                if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {
+                if (sk->sk_type == SOCK_STREAM ||
+                    sk->sk_type == SOCK_DGRAM ||
+                    sk->sk_type == SOCK_RAW) {
                         skb_pull(skb, copied);
                         if (skb->len) {
                                 skb_queue_head(&sk->sk_receive_queue, skb);
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
index decdc49b26d8..6d69b5f0f19b 100644
--- a/net/nfc/nci/Kconfig
+++ b/net/nfc/nci/Kconfig
@@ -1,6 +1,6 @@
 config NFC_NCI
-        depends on NFC && EXPERIMENTAL
-        tristate "NCI protocol support (EXPERIMENTAL)"
+        depends on NFC
+        tristate "NCI protocol support"
         default n
         help
           NCI (NFC Controller Interface) is a communication protocol between
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index acf9abb7d99b..5f98dc1bf039 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -205,10 +205,10 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
         cmd.num_disc_configs = 0;
 
         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-            (protocols & NFC_PROTO_JEWEL_MASK
-             || protocols & NFC_PROTO_MIFARE_MASK
-             || protocols & NFC_PROTO_ISO14443_MASK
-             || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+            (protocols & NFC_PROTO_JEWEL_MASK ||
+             protocols & NFC_PROTO_MIFARE_MASK ||
+             protocols & NFC_PROTO_ISO14443_MASK ||
+             protocols & NFC_PROTO_NFC_DEP_MASK)) {
                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
                         NCI_NFC_A_PASSIVE_POLL_MODE;
                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -224,8 +224,8 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
         }
 
         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-            (protocols & NFC_PROTO_FELICA_MASK
-             || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+            (protocols & NFC_PROTO_FELICA_MASK ||
+             protocols & NFC_PROTO_NFC_DEP_MASK)) {
                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
                         NCI_NFC_F_PASSIVE_POLL_MODE;
                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -414,13 +414,13 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
         struct nci_set_config_param param;
         __u8 local_gb[NFC_MAX_GT_LEN];
-        int i, rc = 0;
+        int i;
 
         param.val = nfc_get_local_general_bytes(nfc_dev, &param.len);
         if ((param.val == NULL) || (param.len == 0))
-                return rc;
+                return 0;
 
-        if (param.len > NCI_MAX_PARAM_LEN)
+        if (param.len > NFC_MAX_GT_LEN)
                 return -EINVAL;
 
         for (i = 0; i < param.len; i++)
@@ -429,10 +429,8 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
         param.id = NCI_PN_ATR_REQ_GEN_BYTES;
         param.val = local_gb;
 
-        rc = nci_request(ndev, nci_set_config_req, (unsigned long)&param,
-                         msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
-
-        return rc;
+        return nci_request(ndev, nci_set_config_req, (unsigned long)&param,
+                           msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 }
 
 static int nci_start_poll(struct nfc_dev *nfc_dev,
@@ -579,7 +577,6 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev,
         }
 }
 
-
 static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
                            __u8 comm_mode, __u8 *gb, size_t gb_len)
 {
@@ -806,8 +803,8 @@ int nci_recv_frame(struct sk_buff *skb)
 
         pr_debug("len %d\n", skb->len);
 
-        if (!ndev || (!test_bit(NCI_UP, &ndev->flags)
-                      && !test_bit(NCI_INIT, &ndev->flags))) {
+        if (!ndev || (!test_bit(NCI_UP, &ndev->flags) &&
+            !test_bit(NCI_INIT, &ndev->flags))) {
                 kfree_skb(skb);
                 return -ENXIO;
         }
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index c1b5285cbde7..3568ae16786d 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -29,6 +29,8 @@
 
 #include "nfc.h"
 
+#include "llcp/llcp.h"
+
 static struct genl_multicast_group nfc_genl_event_mcgrp = {
         .name = NFC_GENL_MCAST_EVENT_NAME,
 };
@@ -364,7 +366,8 @@ static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
         if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
             nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
             nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) ||
-            nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up))
+            nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
+            nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
                 goto nla_put_failure;
 
         return genlmsg_end(msg, hdr);
@@ -590,7 +593,7 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
             ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
               !info->attrs[NFC_ATTR_PROTOCOLS]) &&
-             !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
+              !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
                 return -EINVAL;
 
         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
@@ -715,6 +718,146 @@ static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
         return rc;
 }
 
+static int nfc_genl_send_params(struct sk_buff *msg,
+                                struct nfc_llcp_local *local,
+                                u32 portid, u32 seq)
+{
+        void *hdr;
+
+        hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
+                          NFC_CMD_LLC_GET_PARAMS);
+        if (!hdr)
+                return -EMSGSIZE;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
+            nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
+            nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
+            nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
+                goto nla_put_failure;
+
+        return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+
+        genlmsg_cancel(msg, hdr);
+        return -EMSGSIZE;
+}
+
+static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_llcp_local *local;
+        int rc = 0;
+        struct sk_buff *msg = NULL;
+        u32 idx;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+                return -EINVAL;
+
+        idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+        dev = nfc_get_device(idx);
+        if (!dev)
+                return -ENODEV;
+
+        device_lock(&dev->dev);
+
+        local = nfc_llcp_find_local(dev);
+        if (!local) {
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg) {
+                rc = -ENOMEM;
+                goto exit;
+        }
+
+        rc = nfc_genl_send_params(msg, local, info->snd_portid, info->snd_seq);
+
+exit:
+        device_unlock(&dev->dev);
+
+        nfc_put_device(dev);
+
+        if (rc < 0) {
+                if (msg)
+                        nlmsg_free(msg);
+
+                return rc;
+        }
+
+        return genlmsg_reply(msg, info);
+}
+
+static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_llcp_local *local;
+        u8 rw = 0;
+        u16 miux = 0;
+        u32 idx;
+        int rc = 0;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+            (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
+             !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
+             !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
+                return -EINVAL;
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
+                rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
+
+                if (rw > LLCP_MAX_RW)
+                        return -EINVAL;
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
+                miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
+
+                if (miux > LLCP_MAX_MIUX)
+                        return -EINVAL;
+        }
+
+        idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+        dev = nfc_get_device(idx);
+        if (!dev)
+                return -ENODEV;
+
+        device_lock(&dev->dev);
+
+        local = nfc_llcp_find_local(dev);
+        if (!local) {
+                nfc_put_device(dev);
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
+                if (dev->dep_link_up) {
+                        rc = -EINPROGRESS;
+                        goto exit;
+                }
+
+                local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
+                local->rw = rw;
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
+                local->miux = cpu_to_be16(miux);
+
+exit:
+        device_unlock(&dev->dev);
+
+        nfc_put_device(dev);
+
+        return rc;
+}
+
 static struct genl_ops nfc_genl_ops[] = {
         {
                 .cmd = NFC_CMD_GET_DEVICE,
@@ -759,6 +902,16 @@ static struct genl_ops nfc_genl_ops[] = {
                 .done = nfc_genl_dump_targets_done,
                 .policy = nfc_genl_policy,
         },
+        {
+                .cmd = NFC_CMD_LLC_GET_PARAMS,
+                .doit = nfc_genl_llc_get_params,
+                .policy = nfc_genl_policy,
+        },
+        {
+                .cmd = NFC_CMD_LLC_SET_PARAMS,
+                .doit = nfc_genl_llc_set_params,
+                .policy = nfc_genl_policy,
+        },
 };
 
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index c5e42b79a418..87d914d2876a 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -56,6 +56,7 @@ 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);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
 int __init nfc_llcp_init(void);
 void nfc_llcp_exit(void);
 
@@ -97,6 +98,11 @@ static inline int nfc_llcp_data_received(struct nfc_dev *dev,
         return 0;
 }
 
+static inline struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+        return NULL;
+}
+
 static inline int nfc_llcp_init(void)
 {
         return 0;
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index 8b8a6a2b2bad..313bf1bc848a 100644
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@@ -256,7 +256,6 @@ static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
         return rc ? : copied;
 }
 
-
 static const struct proto_ops rawsock_ops = {
         .family         = PF_NFC,
         .owner          = THIS_MODULE,
diff --git a/net/wireless/core.c b/net/wireless/core.c
index ce1ad776dfb5..26711f46a3be 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -529,8 +529,7 @@ int wiphy_register(struct wiphy *wiphy)
                 for (i = 0; i < sband->n_channels; i++) {
                         sband->channels[i].orig_flags =
                                 sband->channels[i].flags;
-                        sband->channels[i].orig_mag =
-                                sband->channels[i].max_antenna_gain;
+                        sband->channels[i].orig_mag = INT_MAX;
                         sband->channels[i].orig_mpwr =
                                 sband->channels[i].max_power;
                         sband->channels[i].band = band;
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index 46aeafce08d0..4bfd14f7c592 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -473,20 +473,14 @@ int __cfg80211_mlme_deauth(struct cfg80211_registered_device *rdev,
                 .reason_code = reason,
                 .ie = ie,
                 .ie_len = ie_len,
+                .local_state_change = local_state_change,
         };
 
         ASSERT_WDEV_LOCK(wdev);
 
-        if (local_state_change) {
-                if (wdev->current_bss &&
-                    ether_addr_equal(wdev->current_bss->pub.bssid, bssid)) {
-                        cfg80211_unhold_bss(wdev->current_bss);
-                        cfg80211_put_bss(&wdev->current_bss->pub);
-                        wdev->current_bss = NULL;
-                }
-
+        if (local_state_change && (!wdev->current_bss ||
+            !ether_addr_equal(wdev->current_bss->pub.bssid, bssid)))
                 return 0;
-        }
 
         return rdev_deauth(rdev, dev, &req);
 }
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index 3b8cbbc214db..bcc7d7ee5a51 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -908,7 +908,7 @@ static void handle_channel(struct wiphy *wiphy,
                         map_regdom_flags(reg_rule->flags) | bw_flags;
                 chan->max_antenna_gain = chan->orig_mag =
                         (int) MBI_TO_DBI(power_rule->max_antenna_gain);
-                chan->max_power = chan->orig_mpwr =
+                chan->max_reg_power = chan->max_power = chan->orig_mpwr =
                         (int) MBM_TO_DBM(power_rule->max_eirp);
                 return;
         }
@@ -1331,7 +1331,8 @@ static void handle_channel_custom(struct wiphy *wiphy,
 
         chan->flags |= map_regdom_flags(reg_rule->flags) | bw_flags;
         chan->max_antenna_gain = (int) MBI_TO_DBI(power_rule->max_antenna_gain);
-        chan->max_power = (int) MBM_TO_DBM(power_rule->max_eirp);
+        chan->max_reg_power = chan->max_power =
+                (int) MBM_TO_DBM(power_rule->max_eirp);
 }
 
 static void handle_band_custom(struct wiphy *wiphy, enum ieee80211_band band,
diff --git a/net/wireless/util.c b/net/wireless/util.c
index 343f13c1d31d..5b6c1df72f31 100644
--- a/net/wireless/util.c
+++ b/net/wireless/util.c
@@ -311,23 +311,21 @@ unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
 }
 EXPORT_SYMBOL(ieee80211_get_hdrlen_from_skb);
 
-static int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
+unsigned int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
 {
         int ae = meshhdr->flags & MESH_FLAGS_AE;
-        /* 7.1.3.5a.2 */
+        /* 802.11-2012, 8.2.4.7.3 */
         switch (ae) {
+        default:
         case 0:
                 return 6;
         case MESH_FLAGS_AE_A4:
                 return 12;
         case MESH_FLAGS_AE_A5_A6:
                 return 18;
-        case (MESH_FLAGS_AE_A4 | MESH_FLAGS_AE_A5_A6):
-                return 24;
-        default:
-                return 6;
         }
 }
+EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
 
 int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                            enum nl80211_iftype iftype)
@@ -375,6 +373,8 @@ int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                         /* make sure meshdr->flags is on the linear part */
                         if (!pskb_may_pull(skb, hdrlen + 1))
                                 return -1;
+                        if (meshdr->flags & MESH_FLAGS_AE_A4)
+                                return -1;
                         if (meshdr->flags & MESH_FLAGS_AE_A5_A6) {
                                 skb_copy_bits(skb, hdrlen +
                                         offsetof(struct ieee80211s_hdr, eaddr1),
@@ -399,6 +399,8 @@ int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                         /* make sure meshdr->flags is on the linear part */
                         if (!pskb_may_pull(skb, hdrlen + 1))
                                 return -1;
+                        if (meshdr->flags & MESH_FLAGS_AE_A5_A6)
+                                return -1;
                         if (meshdr->flags & MESH_FLAGS_AE_A4)
                                 skb_copy_bits(skb, hdrlen +
                                         offsetof(struct ieee80211s_hdr, eaddr1),